]> defiant.homedns.org Git - ros_wild_thumper.git/blobdiff - avr/motor_ctrl/main.c
projects / ros_wild_thumper.git / blobdiff
? search:
summary | shortlog | log | commit | commitdiff | tree
raw | inline | side by side
avr: fixes, pid tuning
[ros_wild_thumper.git] / avr / motor_ctrl / main.c
diff --git a/avr/motor_ctrl/main.c b/avr/motor_ctrl/main.c
index 0c07d13456b31283ae67ca2e010a76432a9a5fcf..9d33c986b4acf0cb221f6fc8d7d1fc8a36cf930a 100644 (file)
--- 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<<TWINT) | (1<<TWEN) | (1<<TWIE)
 
-#define KP 10.0
-#define KI 0.0 
-#define KD 0.0 
-#define TIMER0_T 0.020
+#define KP 0.009
+#define KI 0.051429
+#define KD 0.000378
+#define TIMER1_T 0.01
 
 enum mode {
        MOTOR_MANUAL,
@@ -81,22 +90,11 @@ static volatile int16_t speed1_wish=0;
 static volatile int16_t speed2_wish=0;
 static volatile int16_t speed3_wish=0;
 static volatile int16_t speed4_wish=0;
-static volatile int16_t speed1=0;
-static volatile int16_t speed2=0;
-static volatile int16_t speed3=0;
-static volatile int16_t speed4=0;
-static volatile int16_t eold1=0;
-static volatile int16_t eold2=0;
-static volatile int16_t eold3=0;
-static volatile int16_t eold4=0;
-static volatile int32_t esum1=0;
-static volatile int32_t esum2=0;
-static volatile int32_t esum3=0;
-static volatile int32_t esum4=0;
-static volatile int16_t pos1_last=0;
-static volatile int16_t pos2_last=0;
-static volatile int16_t pos3_last=0;
-static volatile int16_t pos4_last=0;
+static volatile uint8_t run_pid=0;
+static int16_t speed1=0;
+static int16_t speed2=0;
+static int16_t speed3=0;
+static int16_t speed4=0;
 
 ISR(TWI_vect)
 {
@@ -298,6 +296,38 @@ ISR(TWI_vect)
                                        TWDR = speed4_wish;
                                        TWI_ACK;
                                        break;
+                               case 0x30: // Motor 1 speed MSB
+                                       TWDR = speed1>>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();
ROS Base Controller for my Wild Thumper Robot - https://hackaday.io/project/25406-wild-thumper-based-ros-robot