//latest code, now with speed controller working //valid input for the motor: -3200 to 3200rpm //updated 2/16/04 now with output enable ~ajk //updated 2/17/04 now has configuration bits ~ajk //updated 3/23/04 now has automatic shut off if no command is received // from the DimPC for about 1 second. The fins are reset also. ~ajk //updated 3/24/04 changed proportional gain assignments to vary over range of speed ~ajk //updated 7/31/04 added more complex speed compensation routine. speed comp is not based // the error between actual and commanded speed ~ajk #include #include #include #include #include #include #include #include #include //set configuration bits (all disabled and Oscillator = HS//////////////// #pragma romdata config=0x300000 unsigned char rom _CONFIG1L = 0xff; // CONFIG1L (300000) unsigned char rom _CONFIG1H = 0x22; // CONFIG1H (300001) unsigned char rom _CONFIG2L = 0x0d; // CONFIG2L (300002) unsigned char rom _CONFIG2H = 0x0e; // CONFIG2H (300003) unsigned char rom _CONFIG3L = 0xff; // CONFIG3L (300004) unsigned char rom _CONFIG3H = 0x01; // CONFIG3H (300005) unsigned char rom _CONFIG4L = 0x80; // CONFIG4L (300006) unsigned char rom _CONFIG4H = 0xff; // CONFIG4H (300007) unsigned char rom _CONFIG5L = 0x0f; // CONFIG5L (300008) unsigned char rom _CONFIG5H = 0xc0; // CONFIG5H (300009) unsigned char rom _CONFIG6L = 0x0f; // CONFIG6L (30000A) unsigned char rom _CONFIG6H = 0xe0; // CONFIG6H (30000B) unsigned char rom _CONFIG7L = 0x0f; // CONFIG7L (30000C) unsigned char rom _CONFIG7H = 0x40; // CONFIG7H (30000D) #pragma romdata //initialized variables/////////////////////////////////////////////////// #pragma idata prop_const = 0x100 const static unsigned int SERVO_A = 1; //servo control variables const static unsigned int SERVO_B = 2; // const static unsigned int SERVO_C = 3; // const static unsigned int SERVO_ON = 1; // const static unsigned int SERVO_OFF = 0; // const static unsigned int SCP_PERIOD = 16667; //servo control timer const static unsigned int TIMER_OVERFLOW = 0xFFF0; //variables const static unsigned int SERVO_RANGE_MAX = 5000; //servo limits const static unsigned int SERVO_RANGE_MIN = 3000; // const static signed int MOTOR_SPEED_MAX = 1000; //duty cycle limit const static signed int RPM_MAX = 3200; //actual commanded speed limits const static signed int RPM_MIN = -3200; // static signed int goal_speed=0; //variables for speed sensing and static signed int current_quad = 0; //control static signed long duty_cycle_max = 1024; // maximum duty cycle allowed static unsigned char rlow = 0; // static unsigned char rhigh = 0; // static signed long quad_count = 0; // static signed long quad_total = 0; // static signed int speed_error_prev = 0; // static signed double osc_ctrl = 0; //used to control damping of speed static unsigned char new_speed = 0; //used for reporting speed static unsigned char out_enable =1; //used to turn speed reporting on/off static int overflow_count = 0; //counter variable for DimPC fault shut-off static unsigned char fail_data[] = {'a','4','0','0','0','b','4','0','0','0','c','4','0','0','0','m','0','\n'}; //unintialized variables///////////////////////////////////////////////////////////////////////////// #pragma udata prop_var = 0x200 static unsigned int period_a, period_b, period_c; //servo control variables static unsigned int servo, scp_state; // static unsigned int servo_range_mid; // static unsigned int time; // static unsigned char rx_data[64]; //in/out serial string variables static unsigned char tx_data[32]; // static unsigned int rx_data_i; // static unsigned int tx_data_i; // static char* cmd; // static int cmd_end; // static unsigned int temp; //temporary variables static signed int temp2; // static int echo; //for debugging static signed long duty_cycle; //motor controller duty cycle static unsigned char direction = 1; //motor direction, startup clockwise static signed int speed; //speed in rpm, used to be a double static signed int speed_error; //for control loop, used to be a double static signed int speed_comp; //for control loop, used to be a double static double P; //proportional gain static double D; //derivative gain //main code///////////////////////////////////////////////////////////////////////////// #pragma code void main(void) { ADCON1 = 0x06; //set up port A all outputs TRISA = 0x00; // //setup the USART for asynchronous TXSTAbits.BRGH = 1; // high baud rate SPBRG = 10; // 57.6kbaud TXSTAbits.SYNC = 0; // asynchronous TXSTAbits.TX9 = 0; // 8-bits RCSTAbits.RX9 = 0; // 8-bits TXSTAbits.TXEN = 1; // transmit enable RCSTAbits.CREN = 1; // RX in continuous recieve mode PIE1bits.TXIE = 0; // USART TX interrupt disabled PIE1bits.RCIE = 1; // enable rx interupt IPR1bits.RCIP = 1; // rx interrupt is high priority RCSTAbits.SPEN = 1; // enable serial port // setup interrupts, enable all RCONbits.IPEN = 1; //enable interrupt priority feature INTCONbits.GIE = 1; //all high priority interrupts enabled INTCONbits.PEIE = 1; //all low priority interrupts enabled // set up timer 0 to generate interrupt // timer0 is for RC servo outputs // setup timer 0 (interrup on overflow,16bit) T0CONbits.TMR0ON = 1; // enable timer 0 T0CONbits.T08BIT = 0; // 16 bit T0CONbits.T0CS = 0; // internal clock T0CONbits.PSA = 1; // prescalar disabled T0CONbits.T0PS2 = 0; // set prescalar value to 4 T0CONbits.T0PS1 = 0; T0CONbits.T0PS0 = 1; INTCONbits.TMR0IE = 1; //enable the interrupt INTCON2bits.TMR0IP = 1; // timer0 set to high priority interrupt WriteTimer0( 0x8000 ); // initialize servo variables servo_range_mid = (SERVO_RANGE_MAX + SERVO_RANGE_MIN)/2; period_a = servo_range_mid; period_b = servo_range_mid; period_c = servo_range_mid; servo = SERVO_A; scp_state = SERVO_ON; // initialize motor commands and PWM module direction = 1; PORTCbits.RC4 = 1; duty_cycle = 0; T2CON = 0b00001011; //last two bits are prescalar = 16 PR2 = 0xFF; TRISCbits.TRISC2 = 0; CCP1CON= 0b00001100; T2CONbits.TMR2ON = 1; SetDCPWM1(duty_cycle); // setup PORTB for input (0:7, quadrature decoder inputs) TRISB = 0xFF; // setup part of PORTC (quadtrature decoder control signals) TRISCbits.TRISC0 = 0; TRISCbits.TRISC1 = 0; TRISCbits.TRISC3 = 0; TRISCbits.TRISC4 = 0; // setup timer 1, for motor encoder reading T1CONbits.RD16 = 1; //16 bit operation T1CONbits.T1CKPS1 = 0; // no prescalar T1CONbits.T1CKPS0 = 0; T1CONbits.T1OSCEN = 0; // oscillator disabled T1CONbits.TMR1CS = 0; // internal clock T1CONbits.TMR1ON = 1; // timer on // setup timer 1 interrupt PIE1bits.TMR1IE = 1; // enable timer 1 interrupt IPR1bits.TMR1IP = 0; // timer1 low priority interrupt WriteTimer1(1); // setup timer 3 for DimPC failure--shut off motor after 1 second // of not receiving a command from the DimPC T3CONbits.RD16 = 1; //16 bit operation T3CONbits.T3CKPS1 = 1; //set prescalar value to 8 T3CONbits.T3CKPS0 = 1; T3CONbits.T3CCP2 = 0; //TMR1 is CCP clock, not TMR3 T3CONbits.T3CCP1 = 0; T3CONbits.TMR3CS = 0; //use internal clock T3CONbits.TMR3ON = 0; //timer on //set up timer 3 interrupt PIE2bits.TMR3IE = 0; //enable timer 3 interrupt IPR2bits.TMR3IP = 0; //low priority interrupt WriteTimer3(1); //initialize timer //the main while loop//////////////////////////////////////////////////// while(1) { cmd_end = 0; //resetting variables so next command can be read rx_data_i = 0; while(cmd_end != 1) { //echo back speed, if a new speed has been measured //if output enable is set then send data, otherwise skip echo if(out_enable == 1) { if(new_speed == 1) { while (TXSTAbits.TRMT==0); TXREG = 'P'; ltoa(duty_cycle,tx_data); tx_data_i = 0; echo=0; while(echo<=0) { while (TXSTAbits.TRMT==0); //wait till ready to send if(tx_data[tx_data_i] == NULL) { echo=1; //end of string TXREG = '\t'; } else TXREG=tx_data[tx_data_i]; tx_data_i++; } while (TXSTAbits.TRMT==0); TXREG = 's'; itoa(speed,tx_data); //takes a lot of time to compute! tx_data_i = 0; echo=0; while(echo<=0) { while (TXSTAbits.TRMT==0); //wait till ready to send if(tx_data[tx_data_i] == NULL) { echo=1; //end of string TXREG = '\n'; while (TXSTAbits.TRMT==0); TXREG = '\r'; } else TXREG=tx_data[tx_data_i]; tx_data_i++; } new_speed = 0; //reset the polled variable } //end of new speed if loop } //end of output enable if loop } //end of waiting for new command //processes the input command //Reset TMR3 to start counting 1 second overflow_count = 0; //reset time allowed before next cmd WriteTimer3(1); //reset timer 3 // servo A command cmd = strchr(rx_data, 'a'); if(cmd != NULL) { temp = atoi(&cmd[1]); if(temp >= SERVO_RANGE_MIN && temp <= SERVO_RANGE_MAX) period_a = temp; } // end servo a command // servo B command cmd = strchr(rx_data, 'b'); if(cmd != NULL) { temp = atoi(&cmd[1]); if(temp >= SERVO_RANGE_MIN && temp <= SERVO_RANGE_MAX) period_b = temp; } // end servo b command // servo C command cmd = strchr(rx_data, 'c'); if(cmd != NULL) { temp = atoi(&cmd[1]); if(temp >= SERVO_RANGE_MIN && temp <= SERVO_RANGE_MAX) period_c = temp; } // end servo c command // propulsion motor command cmd = strchr(rx_data, 'm'); if(cmd != NULL) { temp2 = atoi(&cmd[1]); if ((temp2 >= RPM_MIN) && (temp2 <= RPM_MAX)) {goal_speed=temp2;} //using the 'abs' command gave a compiling error??? } // end motor command // disable outputing speed cmd = strchr(rx_data, 'x'); if(cmd != NULL) { out_enable = 0; //clear output enable flag } // end output disable command // enable outputing speed cmd = strchr(rx_data, 'e'); if(cmd != NULL) { out_enable = 1; //set output enable flag } // end output enable command } // end main while } // end main /////////////////////////////////////////////////////////////////////////////// // Interrupt Service Routines Code /////////////////////////////////////////////////////////////////////////////// void ISRhigh(void); void ISRlow(void); #pragma code isr_vect_high = 0x0008 void isrhandlerhigh(void) // This function directs execution to the { // actual interrupt code _asm goto ISRhigh _endasm } #pragma code isr_vect_low = 0x0018 void isrhandlerlow(void) // This function directs execution to the { // actual interrupt code _asm goto ISRlow _endasm } //high priority interrupt //handles servo motor PWM generation //and reading in new commands #pragma code #pragma interrupt ISRhigh save = PROD void ISRhigh(void) { if(INTCONbits.TMR0IF == 1) { switch(servo) { case 1: //SERVO_A: if(scp_state == SERVO_ON) { time = TIMER_OVERFLOW - period_a; PORTAbits.RA0 = 1; scp_state = SERVO_OFF; } else { time = TIMER_OVERFLOW - (SCP_PERIOD - period_a); //time = period_a; PORTAbits.RA0 = 0; scp_state = SERVO_ON; servo = SERVO_B; } break; case 2: //SERVO_B: if(scp_state == SERVO_ON) { time = TIMER_OVERFLOW - period_b; PORTAbits.RA1 = 1; scp_state = SERVO_OFF; } else { time = TIMER_OVERFLOW - (SCP_PERIOD - period_b); PORTAbits.RA1 = 0; scp_state = SERVO_ON; servo = SERVO_C; } break; case 3: //SERVO_C: if(scp_state == SERVO_ON) { time = TIMER_OVERFLOW - period_c; PORTAbits.RA2 = 1; scp_state = SERVO_OFF; } else { time = TIMER_OVERFLOW - (SCP_PERIOD - period_c); PORTAbits.RA2 = 0; scp_state = SERVO_ON; servo = SERVO_A; } break; default: break; }//end switch INTCONbits.TMR0IF = 0; // clear interrup flag if(time > 0) WriteTimer0(time); } else if(PIR1bits.RCIF == 1) //then we have recieved a new character { if(RCSTAbits.OERR == 1) //check for overrunn error; reset if needed { RCSTAbits.CREN = 0; while (TXSTAbits.TRMT==0); TXREG = '*'; RCSTAbits.CREN = 1; } if(rx_data_i < 0) rx_data_i = 0; rx_data[rx_data_i] = RCREG; // read i'th command byte if (rx_data[rx_data_i] == '\n') cmd_end = 1; //end of command detected else { rx_data_i++; if(rx_data_i >= 64) rx_data_i = 0; //rollover if needed } PIR1bits.RCIF = 0; //reset the flag } } // end isr_functions //low priority interrupt //handles reading drive motor speed from motor encoder and //implements a PD compensation on commanded speed //stops motor is no command has been received in past 1 second #pragma code #pragma interruptlow ISRlow save = PROD void ISRlow(void) { if(PIR1bits.TMR1IF == 1) { // read quadrature decoder and update duty cycle command if needed PORTCbits.RC0 = 0; // SEL high byte PORTCbits.RC1 = 0; // OE on rhigh = PORTB; //upper 4 bits read PORTCbits.RC0 = 1; // SEL low byte rlow = PORTB; //lower 8 bits read PORTCbits.RC1 = 1; // OE off PORTCbits.RC3 = 0; // reset active PORTCbits.RC0 = 0; // SEL high byte (return to this for next INTR) PORTCbits.RC3 = 1; // reset off current_quad = rlow + 256*rhigh; //total number of pulses this time through if(direction == 0) { if(current_quad != 0) { current_quad = 4096 - current_quad; //result here should always be positive current_quad = -current_quad; } } quad_total = quad_total + current_quad; //sum pulses quad_count = quad_count++; //reading the encoder 10 times before processing if( quad_count >= 10 ) { new_speed = 1; //for reporting speed in main code //compute speed in RPM's //speed = quad_total * SAMPLE_FREQ * 60 / 500 / 4 (rpm) //SAMPLE_FREQ (10.1Hz) is frequency of getting to this inner loop; 500 is pulses per //revolution of encoder; divide by 4 'cause it's a quadrature encoder signal speed = quad_total * .303; //same as above, less comp. time(.303) //PD speed controller if (direction == 0) duty_cycle = -duty_cycle; //restoring sign of duty_cycle speed_error = goal_speed - speed; //speed error //set up control gains based on desired speed //P = 0.5 - (0.0001)*(goal_speed); //variable gain based on speed P = 0.5 - (0.0001)*(speed_error); D = 1.0; if (goal_speed == 0) { duty_cycle = 0; //no control used if trying to stop the motor speed = 0; } else { speed_comp = P*speed_error + D*(speed_error-speed_error_prev)*.099; //pd compensation osc_ctrl = 0.025*(speed_error); //allow for variable damping in speed control if (osc_ctrl < 0) osc_ctrl *= -1; if (speed_comp > osc_ctrl) speed_comp = osc_ctrl; //Max duty cycle change = +/- 25 else if(speed_comp < -osc_ctrl) speed_comp = -osc_ctrl; duty_cycle = duty_cycle + speed_comp; //updating the duty cycle if (duty_cycle < 0) //duty_cycle cannot be negative duty_cycle = 0; } if (duty_cycle > MOTOR_SPEED_MAX) duty_cycle = MOTOR_SPEED_MAX; //applying a saturation limit PORTCbits.RC4 = direction; //1 = clockwise, 0 = counterclockwise SetDCPWM1(duty_cycle); //set PWM signal to motor control chip speed_error_prev = speed_error; //save current error as old error for next time quad_count = 0; //reset variables quad_total = 0; } PIR1bits.TMR1IF = 0; // clear interrup flag WriteTimer1(41000); // set timer for next interrupt } else if(PIR2bits.TMR3IF == 1) { overflow_count = overflow_count + 1; //increment overflow counter if(overflow_count == 0x9) // 5 = 1.05second { int i; for(i=0; i<18; i++) //load stop data into command buffer { rx_data[i] = fail_data[i]; //fail_data stops motor and resets fins } cmd_end = 1; //set received command flag to process cmd overflow_count = 0; //reset counter } PIR2bits.TMR3IF = 0; //clear interrupt flag WriteTimer3(1); //reset timer for next interrupt } }