X-Git-Url: https://defiant.homedns.org/gitweb/?p=ros_wild_thumper.git;a=blobdiff_plain;f=avr%2Fmotor_ctrl%2Fmain.c;h=9d33c986b4acf0cb221f6fc8d7d1fc8a36cf930a;hp=0c07d13456b31283ae67ca2e010a76432a9a5fcf;hb=d59156fdf0d81fc362bb56e5404069234ee436a5;hpb=73f9b5e5a082dd91b6d96fdddbe953afe4362b9d diff --git a/avr/motor_ctrl/main.c b/avr/motor_ctrl/main.c index 0c07d13..9d33c98 100644 --- a/avr/motor_ctrl/main.c +++ b/avr/motor_ctrl/main.c @@ -36,6 +36,15 @@ * 0x26 Motor 4 speed wish MSB * 0x27 Motor 4 speed wish LSB * free + * 0x30 Motor 1 speed MSB + * 0x31 Motor 1 speed LSB + * 0x32 Motor 2 speed MSB + * 0x33 Motor 2 speed LSB + * 0x34 Motor 3 speed MSB + * 0x35 Motor 3 speed LSB + * 0x36 Motor 4 speed MSB + * 0x37 Motor 4 speed LSB + * free * 0x90 Motor 1 switch * 0x91 Motor 2 switch * 0x92 Motor 3 switch @@ -49,10 +58,10 @@ #define TWI_RESET TWCR &= ~((1 << TWSTO) | (1 << TWEN)); TWI_ACK #define TWI_NAK TWCR = (1<>8; + TWI_ACK; + break; + case 0x31: // Motor 1 speed LSB + TWDR = speed1; + TWI_ACK; + break; + case 0x32: // Motor 2 speed MSB + TWDR = speed2>>8; + TWI_ACK; + break; + case 0x33: // Motor 2 speed LSB + TWDR = speed2; + TWI_ACK; + break; + case 0x34: // Motor 3 speed MSB + TWDR = speed3>>8; + TWI_ACK; + break; + case 0x35: // Motor 3 speed LSB + TWDR = speed3; + TWI_ACK; + break; + case 0x36: // Motor 4 speed MSB + TWDR = speed4>>8; + TWI_ACK; + break; + case 0x37: // Motor 4 speed LSB + TWDR = speed4; + TWI_ACK; + break; default: TWDR = 0; TWI_NAK; @@ -323,14 +353,14 @@ static void update_hall1(void) { diff = oldstatus - new; // difference last - new if (diff & 0x1) { // bit 0 = value (1) oldstatus = new; // store new as next last - if (motor1_switch) pos1 -= (diff & 2) - 1; // bit 1 = direction (+/-) - else pos1 += (diff & 2) - 1; + if (motor1_switch) pos1 += (diff & 2) - 1; // bit 1 = direction (+/-) + else pos1 -= (diff & 2) - 1; } } static void update_hall2(void) { - unsigned char status = (PINA >> 2) & 0x3; + unsigned char status = (PINA >> 4) & 0x3; static unsigned char oldstatus=0; unsigned char diff, new; @@ -342,14 +372,14 @@ static void update_hall2(void) { diff = oldstatus - new; // difference last - new if (diff & 0x1) { // bit 0 = value (1) oldstatus = new; // store new as next last - if (motor2_switch) pos2 += (diff & 2) - 1; // bit 1 = direction (+/-) - else pos2 -= (diff & 2) - 1; + if (motor2_switch) pos2 -= (diff & 2) - 1; // bit 1 = direction (+/-) + else pos2 += (diff & 2) - 1; } } static void update_hall3(void) { - unsigned char status = (PINA >> 4) & 0x3; + unsigned char status = (PINA >> 2) & 0x3; static unsigned char oldstatus=0; unsigned char diff, new; @@ -361,8 +391,8 @@ static void update_hall3(void) { diff = oldstatus - new; // difference last - new if (diff & 0x1) { // bit 0 = value (1) oldstatus = new; // store new as next last - if (motor3_switch) pos3 += (diff & 2) - 1; // bit 1 = direction (+/-) - else pos3 -= (diff & 2) - 1; + if (motor3_switch) pos3 -= (diff & 2) - 1; // bit 1 = direction (+/-) + else pos3 += (diff & 2) - 1; } } @@ -462,84 +492,94 @@ static void update_motor(void) { } -ISR(TIMER0_OVF_vect) { - update_hall1(); - update_hall2(); - update_hall3(); - update_hall4(); +void update_pid(void) { + static int16_t pos1_last=0; + static int16_t pos2_last=0; + static int16_t pos3_last=0; + static int16_t pos4_last=0; + static int16_t eold1=0; + static int16_t eold2=0; + static int16_t eold3=0; + static int16_t eold4=0; + static int32_t esum1=0; + static int32_t esum2=0; + static int32_t esum3=0; + static int32_t esum4=0; + + speed1 = (pos1 - pos1_last)/TIMER1_T; + pos1_last = pos1; + speed2 = (pos2 - pos2_last)/TIMER1_T; + pos2_last = pos2; + speed3 = (pos3 - pos3_last)/TIMER1_T; + pos3_last = pos3; + speed4 = (pos4 - pos4_last)/TIMER1_T; + pos4_last = pos4; - // PID control if (motor1_mode == MOTOR_PID) { - speed1 = (pos1 - pos1_last)/TIMER0_T; - if (speed1_wish == 0) { motor1 = 0; } else { int16_t e = speed1_wish - speed1; esum1+=e; - motor1 += KP*e + KI*TIMER0_T*esum1 + KD/TIMER0_T*(e - eold1); + motor1 += KP*e + KI*TIMER1_T*esum1 + KD/TIMER1_T*(e - eold1); eold1 = e; if (motor1 > 255) motor1 = 255; else if (motor1 < -255) motor1 = -255; } - - pos1_last = pos1; } if (motor2_mode == MOTOR_PID) { - speed2 = (pos2 - pos2_last)/TIMER0_T; - if (speed2_wish == 0) { motor2 = 0; } else { int16_t e = speed2_wish - speed2; esum2+=e; - motor2 += KP*e + KI*TIMER0_T*esum2 + KD/TIMER0_T*(e - eold2); + motor2 += KP*e + KI*TIMER1_T*esum2 + KD/TIMER1_T*(e - eold2); eold2 = e; if (motor2 > 255) motor2 = 255; else if (motor2 < -255) motor2 = -255; } - - pos2_last = pos2; } if (motor3_mode == MOTOR_PID) { - speed3 = (pos3 - pos3_last)/TIMER0_T; - if (speed3_wish == 0) { motor3 = 0; } else { int16_t e = speed3_wish - speed3; esum3+=e; - motor3 += KP*e + KI*TIMER0_T*esum3 + KD/TIMER0_T*(e - eold3); + motor3 += KP*e + KI*TIMER1_T*esum3 + KD/TIMER1_T*(e - eold3); eold3 = e; if (motor3 > 255) motor3 = 255; else if (motor3 < -255) motor3 = -255; } - - pos3_last = pos3; } if (motor4_mode == MOTOR_PID) { - speed4 = (pos4 - pos4_last)/TIMER0_T; - if (speed4_wish == 0) { motor4 = 0; } else { int16_t e = speed4_wish - speed4; esum4+=e; - motor4 += KP*e + KI*TIMER0_T*esum4 + KD/TIMER0_T*(e - eold4); + motor4 += KP*e + KI*TIMER1_T*esum4 + KD/TIMER1_T*(e - eold4); eold4 = e; if (motor4 > 255) motor4 = 255; else if (motor4 < -255) motor4 = -255; } - - pos4_last = pos4; } } +ISR(TIMER1_OVF_vect) { + update_hall1(); + update_hall2(); + update_hall3(); + update_hall4(); + + run_pid++; +} + + int main(void) { // Outputs DDRB = (1 << 3); @@ -555,23 +595,23 @@ int main(void) { TWI_RESET; // Motor 1 & 2 - // Timer 1: Fast PWM inverting mode, Top=256 => 15.625Hz + // Timer 1: Fast PWM inverting mode, Top=256 => 15.625kHz // Prescaler=1 - TCCR1A = (1 << COM1A1) | (1 << COM1B1) | (1 << WGM10); + TCCR1A = (1 << COM1A1) | (1 << COM1B1) | (1 << COM1A0) | (1 << COM1B0) | (1 << WGM10); TCCR1B = (1 << WGM12) | (1 << CS10); OCR1A = 0; OCR1B = 0; // Motor 3 - // Timer 2: Fast PWM, Top=256 => 15.625Hz + // Timer 2: Fast PWM inverting mode, Top=256 => 15.625kHz // Prescaler=1 - TCCR2 = (1 << WGM21) | (1 << WGM20) | (1 << COM21) | (1 << CS20); + TCCR2 = (1 << WGM21) | (1 << WGM20) | (1 << COM21) | (1 << COM20) | (1 << CS20); OCR2 = 0; // Motor 4 - // Timer 0: Fast PWM, Top=256 => 15.625Hz + // Timer 0: Fast PWM inverting mode, Top=256 => 15.625kHz // Prescaler=1 - TCCR0 = (1 << WGM01) | (1 << WGM00) | (1 << COM01) | (1 << CS00); + TCCR0 = (1 << WGM01) | (1 << WGM00) | (1 << COM01) | (1 << COM00) | (1 << CS00); OCR0 = 0; printf("\r\nStart\r\n"); @@ -593,6 +633,13 @@ int main(void) { } break; } + + + if (run_pid >= 156) { // ~100Hz + run_pid=0; + update_pid(); + } + update_motor(); sleep_mode();