/* Start user code for adding. Do not edit comment generated here */ void packet_process(void) { int pwm; /* duty cycle for duty assignment mode */ int mode; /* assigned mode from master packet */ /* if ignore bit set, do nothing */ if (masterPacket[0] & 2) return; mode = masterPacket[0] & 12; /* servo off mode */ if (mode == 0) { set_motor_output(0); TRG = 0; count = 0; } /* duty assignment mode */ if (mode == 4) { pwm = (double)((int8_t)masterPacket[3]) / 100 * 3100; set_motor_output(pwm); } /* position control mode */ if (mode == 8) { pid_control(); } /* else reservation mode, don't need to do anything */ /* if this is the time we are processing this packet, and if required send reply */ if (firstCall == 1 && masterPacket[1] & 1 == 1) { firstCall = 0; bufferOut[0] = masterPacket[0]; bufferOut[2] = count >> 8; bufferOut[3] = count; bufferOut[4] = crc((((long)bufferIn[0]) << 24) + (((long)bufferIn[1]) << 16) + (((long)bufferIn[2]) << 8) + ((long) bufferIn[3]));*/ P1.5 = 1; R_UART2_Send(bufferOut , 5); /* when send finishes, it will toggle DE for receive */ }
void main (void) { long prevCounter = 0; unsigned char tempTrigger = 0; char tempStepPulses = 0; long tempCurrentCounter = 0; unsigned char currentADvalue = tempADSpeed; long desiredCounter = 0; long error = 0; long prevError = 0; long velocity = 0; float integral = 0.0; float derivative = 0.0; long output = 0; char testState = 0; long testCounter = 0; char homeFlag = 0; enum { LOW = 0, HIGH = 1} homeDir = LOW; /*************** SETUP INTERRUPTS ******************/ INTCONbits.GIEH = 0; // Temporarily disable global interrupts INTCONbits.GIEL = 0; // Temporarily disable peripheral interrupts INTCONbits.TMR0IE = 1; // Enable TMR0 Overflow interrupt INTCONbits.INT0IE = 1; // Enable external interrupt on INT0/RB0 INTCONbits.RBIE = 1; // Enable Port B change interrupt //INTCON2bits.RBPU = 0; // Enable Port B pull-ups INTCON2bits.RBPU = 1; // Disable Port B pull-ups INTCON2bits.INTEDG0 = 1; // Interrupt on rising edge for INT0 INTCON2bits.TMR0IP = 0; // Set TMR0 interrupt priority to Low INTCON2bits.RBIP = 1; // Set Port B change interrupt to High RCONbits.IPEN = 1; // Enable priority levels on interrupts /****************************************************/ /***************** SETUP TMR0 ***********************/ //T0CONbits.TMR0ON = 0; // Temporarily disable TMR0 until the end of the setup; T0CONbits.T08BIT = 1; // Use an 8-bit timer (1). Use a 16-bit timer (0). T0CONbits.T0CS = 0; // Use internal clock cycles for the timer. T0CONbits.PSA = 0; // Use prescalar to adjust overflow rate (0). Do not use prescalar (1). /* Assign Prescalar */ T0CONbits.T0PS2 = 0; T0CONbits.T0PS1 = 1; T0CONbits.T0PS0 = 1; /* This makes go slower */ //T0CONbits.T0PS2 = 1; //T0CONbits.T0PS1 = 1; //T0CONbits.T0PS0 = 1; T0CONbits.TMR0ON = 1; /***************************************************/ /************** SETUP I/O PINS *********************/ TRISBbits.TRISB4 = 1; // Encoder ChA input TRISBbits.TRISB5 = 1; // Encoder ChB input TRISBbits.TRISB6 = 0; // Set as output to avoid Port B interrupt on change TRISBbits.TRISB7 = 0; // Set as output to avoid Port B interrupt on change TRISBbits.TRISB0 = 1; // Step pulse input TRISBbits.TRISB1 = 1; // Direction input //TRISAbits.TRISA4 = 0; // Possible motor output pin RA4 TRISAbits.TRISA0 = 1; // Set RA0 as input for SPEED pin (analog input for A/D) TRISAbits.TRISA1 = 0; // Set RA1 as output for diagnostic LED //ADCON0bits.VCFG0 = 0; // Set A/D to External Vref(+/-) using 1 and 1 //ADCON0bits.VCFG1 = 0; // Set A/D to Vss and Vdd references using 0 and 0 /***************************************************/ /**************** SETUP A/D ************************/ /* Configure analog and digital pins */ ADCON1bits.PCFG0 = 1; // Set AN0 port as digital input (Home switch) ADCON1bits.PCFG1 = 1; // RA1/AN1 diagnostic LED ADCON1bits.PCFG2 = 1; // Reserved - test pad on AN2 ADCON1bits.PCFG3 = 1; // Reserved - test pad on AN3 ADCON1bits.PCFG4 = 1; // Set AN4/RB0 as a digital pin (Step pulse input) ADCON1bits.PCFG5 = 1; // Set AN5/RB1 as a digital pin (Direction input) ADCON1bits.PCFG6 = 1; // Set AN6/RB4 as a digital pin (ChA input) // // /* Select A/D input channel as AN0 */ // ADCON0bits.CHS0 = 0; // ADCON0bits.CHS1 = 0; // ADCON0bits.CHS2 = 0; // // /* Set A/D clock as Fosc/2 */ // ADCON2bits.ADCS0 = 0; // ADCON2bits.ADCS1 = 0; // ADCON2bits.ADCS2 = 0; // // ADCON2bits.ADFM = 0; // Left justify A/D conversion so only the 8 MSB are read later // // /* Turn on A/D module */ // ADCON0bits.ADON = 1; /* Turn off A/D module */ ADCON0bits.ADON = 0; /***************************************************/ /**************** SETUP PWM ************************/ /* Configure PWM pins P1A and P1B as inputs. */ TRISBbits.TRISB3 = 1; TRISBbits.TRISB2 = 1; /* Use PR2 to set the PWM period */ PR2 = 0xFF; // Equation 15-1 PWM Period = [(PR2 + 1)] * 4 * TOSC * TMR2 Prescaler // or about 1 / 490Hz /* Set PWM Mode with P1A as a single modulated output */ CCP1CONbits.CCP1M0 = 0; CCP1CONbits.CCP1M1 = 0; CCP1CONbits.CCP1M2 = 1; CCP1CONbits.CCP1M3 = 1; CCP1CONbits.P1M0 = 0; CCP1CONbits.P1M1 = 0; /* LSB of the PWM duty cycle */ CCP1CONbits.DC1B1 = 0; CCP1CONbits.DC1B0 = 0; /* MSB of the PWM duty cycle */ CCPR1L = 0x00; /* Configure TMR2 */ PIR1bits.TMR2IF = 0; // Clear interupt flag for TMR2 equal to PR2 T2CONbits.T2CKPS1 = 1; // Set prescaler to 16 to decrease PWM frequency T2CONbits.T2CKPS0 = 1; T2CONbits.TMR2ON = 1; // Turn on Timer2 /* Wait for the TMR2IF flag go high before procedding */ while (!(PIR1bits.TMR2IF)) { int temp = 0; } /* Set the PWM pins P1A and P1B as outputs. */ TRISBbits.TRISB3 = 0; TRISBbits.TRISB2 = 0; ECCPASbits.ECCPASE = 0; /*****************************************************/ /***************** SET OSC SPEED *********************/ //OSCCONbits.IRCF0 = 1; //OSCCONbits.IRCF1 = 1; //OSCCONbits.IRCF2 = 1; OSCCONbits.IRCF0 = 1; OSCCONbits.IRCF1 = 1; OSCCONbits.IRCF2 = 1; /*****************************************************/ // This line is part of the initial check sequence and is not // needed for normal operation: desiredCounter = 0; trigger = 0; stepPulses = 0; currentCounter = 0; set_motor_output( 0, 0 ); /****************** ENABLE INTERRUPTS *****************/ INTCONbits.GIEH = 1; // Enable global interrupts INTCONbits.GIEL = 1; // Enable peripheral innterupts /******************************************************/ while (1) { INTCONbits.GIEH = 0; tempTrigger = trigger; INTCONbits.GIEH = 1; if ( tempTrigger ) { INTCONbits.GIEH = 0; trigger = 0; tempCurrentCounter = currentCounter; tempStepPulses = stepPulses; stepPulses = 0; // Check if step pin has been held HIGH. If it has been held HIGH // long enough to indicate a HOME command, set home flag, otherwise // just increment stepHighCounter. stepHighCounter gets reset in // in interrupt routine that reads step and direction lines. if (stepHighCounter) { if (stepHighCounter > stepHomeThreshold) { homeFlag = 1; stepHighCounter = 0; homeDir = PORTBbits.RB1; //set home direction to direction input pin } else { if (PORTBbits.RB0) { stepHighCounter++; } } } //stepTimerFoo++; //tempStepTimerFoo = stepTimerFoo; INTCONbits.GIEH = 1; // if tempStepTimerFoo > something // then set homeFlag, set homeDir // if homeflag // if homeswitch // turn off homing // else // set motor go to home in homeDir direction // end //else // do normal motor control //end /* INTCONbits.GIEH = 0; trigger = 0; INTCONbits.GIEH = 1; //if (PORTAbits.RA1) // PORTAbits.RA1 = 0; //else // PORTAbits.RA1 = 1; INTCONbits.GIEH = 0; tempCurrentCounter = currentCounter; tempStepPulses = stepPulses; stepPulses = 0; INTCONbits.GIEH = 1; */ // velocity = (tempCurrentCounter - prevCounter)*counterToSpeedScalar; // prevCounter = tempCurrentCounter; // Uncomment this for normal operation desiredCounter = desiredCounter + (tempStepPulses*numCountsPerStep); if (homeFlag) // If we are in homing mode { if (PORTAbits.RA0) // if homeswitch is ON { // We're home! set_motor_output(0,0); homeFlag = 0; desiredCounter = 0; // turn off interrupts to clear sensitive variables INTCONbits.GIEH = 0; currentCounter = 0; INTCONbits.GIEH = 1; } else // if homeswitch is OFF - we are still going home { if (homeDir) // going up { set_motor_output(abs(upTorqueLimit),1); } else // going down { set_motor_output(abs(downTorqueLimit), 0); } } } else { error = desiredCounter - tempCurrentCounter; output = Kp * error; ////////////// Set motor output ////////////////////////////// // // The variable "output" controls voltage to motor. Examples: // // this makes the motor go up: // set_motor_output(30,1); // // this makes the motor go down: // set_motor_output(30,0); // if (output <= 0) { if (output < downTorqueLimit) { output = downTorqueLimit; } set_motor_output(abs(output),0); } else { if (output > upTorqueLimit) { output = upTorqueLimit; } set_motor_output(abs(output),1); } } } } }