avr: fixes, pid tuning
[ros_wild_thumper.git] / avr / motor_ctrl / main.c
1 #include <stdio.h>
2 #include <stdlib.h>
3 #include <limits.h>
4 #include <avr/io.h>
5 #include <avr/interrupt.h>
6 #include <avr/sleep.h>
7 #include "uart.h"
8
9 /*
10  * I2C Register Map (8 Bit)
11  * 0x00 Register select
12  * 0x01 Motor 1 PWM MSB
13  * 0x02 Motor 1 PWM LSB
14  * 0x03 Motor 2 PWM MSB
15  * 0x04 Motor 2 PWM LSB
16  * 0x05 Motor 3 PWM MSB
17  * 0x06 Motor 3 PWM LSB
18  * 0x07 Motor 4 PWM MSB
19  * 0x08 Motor 4 PWM LSB
20  * free
21  * 0x10 Hall 1 MSB
22  * 0x11 Hall 1 LSB
23  * 0x12 Hall 2 MSB
24  * 0x13 Hall 2 LSB
25  * 0x14 Hall 3 MSB
26  * 0x15 Hall 3 LSB
27  * 0x16 Hall 4 MSB
28  * 0x17 Hall 4 LSB
29  * free
30  * 0x20 Motor 1 speed wish MSB
31  * 0x21 Motor 1 speed wish LSB
32  * 0x22 Motor 2 speed wish MSB
33  * 0x23 Motor 2 speed wish LSB
34  * 0x24 Motor 3 speed wish MSB
35  * 0x25 Motor 3 speed wish LSB
36  * 0x26 Motor 4 speed wish MSB
37  * 0x27 Motor 4 speed wish LSB
38  * free
39  * 0x30 Motor 1 speed MSB
40  * 0x31 Motor 1 speed LSB
41  * 0x32 Motor 2 speed MSB
42  * 0x33 Motor 2 speed LSB
43  * 0x34 Motor 3 speed MSB
44  * 0x35 Motor 3 speed LSB
45  * 0x36 Motor 4 speed MSB
46  * 0x37 Motor 4 speed LSB
47  * free
48  * 0x90 Motor 1 switch
49  * 0x91 Motor 2 switch
50  * 0x92 Motor 3 switch
51  * 0x93 Motor 4 switch
52  * free
53  * 0xff Bootloader
54  */
55
56
57 #define TWI_ACK         TWCR = (1<<TWEA) | (1<<TWINT) | (1<<TWEN) | (1<<TWIE)
58 #define TWI_RESET       TWCR &= ~((1 << TWSTO) | (1 << TWEN)); TWI_ACK
59 #define TWI_NAK         TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE)
60
61 #define KP 0.009
62 #define KI 0.051429
63 #define KD 0.000378
64 #define TIMER1_T 0.01
65
66 enum mode {
67         MOTOR_MANUAL,
68         MOTOR_PID
69 };
70
71 static volatile uint8_t ireg=0;
72 static volatile uint8_t bootloader=0;
73 static volatile int16_t motor1=0;
74 static volatile int16_t motor2=0;
75 static volatile int16_t motor3=0;
76 static volatile int16_t motor4=0;
77 static volatile int16_t pos1=0;
78 static volatile int16_t pos2=0;
79 static volatile int16_t pos3=0;
80 static volatile int16_t pos4=0;
81 static volatile enum mode motor1_mode=MOTOR_MANUAL;
82 static volatile enum mode motor2_mode=MOTOR_MANUAL;
83 static volatile enum mode motor3_mode=MOTOR_MANUAL;
84 static volatile enum mode motor4_mode=MOTOR_MANUAL;
85 static volatile uint8_t motor1_switch=0;
86 static volatile uint8_t motor2_switch=0;
87 static volatile uint8_t motor3_switch=0;
88 static volatile uint8_t motor4_switch=0;
89 static volatile int16_t speed1_wish=0;
90 static volatile int16_t speed2_wish=0;
91 static volatile int16_t speed3_wish=0;
92 static volatile int16_t speed4_wish=0;
93 static volatile uint8_t run_pid=0;
94 static int16_t speed1=0;
95 static int16_t speed2=0;
96 static int16_t speed3=0;
97 static int16_t speed4=0;
98
99 ISR(TWI_vect)
100 {
101         static uint8_t tmp=0;
102         static int16_t tmp16=0;
103
104         switch (TWSR & 0xF8)
105         {  
106                 case 0x60: // start write
107                         TWI_ACK;
108                         ireg = 0;
109                         break;
110                 case 0x80: // write
111                         switch(ireg) {
112                                 case 0x00: // register select
113                                         ireg = TWDR;
114                                         ireg--; // because we do ireg++ below
115                                         TWI_ACK;
116                                         break;
117                                 case 0x01: // Motor 1 MSB
118                                         tmp = TWDR;
119                                         TWI_ACK;
120                                         break;
121                                 case 0x02: // Motor 1 LSB
122                                         motor1 = tmp<<8 | TWDR;
123                                         motor1_mode = MOTOR_MANUAL;
124                                         TWI_ACK;
125                                         break;
126                                 case 0x03: // Motor 2 MSB
127                                         tmp = TWDR;
128                                         TWI_ACK;
129                                         break;
130                                 case 0x04: // Motor 2 LSB
131                                         motor2 = tmp<<8 | TWDR;
132                                         motor2_mode = MOTOR_MANUAL;
133                                         TWI_ACK;
134                                         break;
135                                 case 0x05: // Motor 3 MSB
136                                         tmp = TWDR;
137                                         TWI_ACK;
138                                         break;
139                                 case 0x06: // Motor 3 LSB
140                                         motor3 = tmp<<8 | TWDR;
141                                         motor3_mode = MOTOR_MANUAL;
142                                         TWI_ACK;
143                                         break;
144                                 case 0x07: // Motor 4 MSB
145                                         tmp = TWDR;
146                                         TWI_ACK;
147                                         break;
148                                 case 0x08: // Motor 4 LSB
149                                         motor4 = tmp<<8 | TWDR;
150                                         motor4_mode = MOTOR_MANUAL;
151                                         TWI_ACK;
152                                         break;
153                                 case 0x20: // Motor 1 speed wish MSB
154                                         tmp = TWDR;
155                                         TWI_ACK;
156                                         break;
157                                 case 0x21: // Motor 1 speed wish LSB
158                                         speed1_wish = tmp<<8 | TWDR;
159                                         motor1_mode = MOTOR_PID;
160                                         TWI_ACK;
161                                         break;
162                                 case 0x22: // Motor 2 speed wish MSB
163                                         tmp = TWDR;
164                                         TWI_ACK;
165                                         break;
166                                 case 0x23: // Motor 2 speed wish LSB
167                                         speed2_wish = tmp<<8 | TWDR;
168                                         motor2_mode = MOTOR_PID;
169                                         TWI_ACK;
170                                         break;
171                                 case 0x24: // Motor 3 speed wish MSB
172                                         tmp = TWDR;
173                                         TWI_ACK;
174                                         break;
175                                 case 0x25: // Motor 3 speed wish LSB
176                                         speed3_wish = tmp<<8 | TWDR;
177                                         motor3_mode = MOTOR_PID;
178                                         TWI_ACK;
179                                         break;
180                                 case 0x26: // Motor 4 speed wish MSB
181                                         tmp = TWDR;
182                                         TWI_ACK;
183                                         break;
184                                 case 0x27: // Motor 4 speed wish LSB
185                                         speed4_wish = tmp<<8 | TWDR;
186                                         motor4_mode = MOTOR_PID;
187                                         TWI_ACK;
188                                         break;
189                                 case 0x90: // Motor 1 switch
190                                         motor1_switch = TWDR;
191                                         TWI_ACK;
192                                         break;
193                                 case 0x91: // Motor 2 switch
194                                         motor2_switch = TWDR;
195                                         TWI_ACK;
196                                         break;
197                                 case 0x92: // Motor 3 switch
198                                         motor3_switch = TWDR;
199                                         TWI_ACK;
200                                         break;
201                                 case 0x93: // Motor 4 switch
202                                         motor4_switch = TWDR;
203                                         TWI_ACK;
204                                         break;
205                                 case 0xff: // bootloader
206                                         bootloader = TWDR;
207                                 default:
208                                         TWI_NAK;
209                         }
210                         ireg++;
211                         break;
212                 case 0xA8: // start read
213                 case 0xB8: // read
214                         switch(ireg) {
215                                 case 0x02: // Motor 1 PWM
216                                         TWDR = OCR1A;
217                                         TWI_ACK;
218                                         break;
219                                 case 0x04: // Motor 2 PWM
220                                         TWDR = OCR1B;
221                                         TWI_ACK;
222                                         break;
223                                 case 0x06: // Motor 3 PWM
224                                         TWDR = OCR2;
225                                         TWI_ACK;
226                                         break;
227                                 case 0x08: // Motor 4 PWM
228                                         TWDR = OCR0;
229                                         TWI_ACK;
230                                         break;
231                                 case 0x10: // Hall 1 MSB
232                                         tmp16 = pos1;
233                                         TWDR = tmp16>>8;
234                                         TWI_ACK;
235                                         break;
236                                 case 0x11: // Hall 1 LSB
237                                         TWDR = tmp16;
238                                         TWI_ACK;
239                                         break;
240                                 case 0x12: // Hall 2 MSB
241                                         tmp16 = pos2;
242                                         TWDR = tmp16>>8;
243                                         TWI_ACK;
244                                         break;
245                                 case 0x13: // Hall 2 LSB
246                                         TWDR = tmp16;
247                                         TWI_ACK;
248                                         break;
249                                 case 0x14: // Hall 3 MSB
250                                         tmp16 = pos3;
251                                         TWDR = tmp16>>8;
252                                         TWI_ACK;
253                                         break;
254                                 case 0x15: // Hall 3 LSB
255                                         TWDR = tmp16;
256                                         TWI_ACK;
257                                         break;
258                                 case 0x16: // Hall 4 MSB
259                                         tmp16 = pos4;
260                                         TWDR = tmp16>>8;
261                                         TWI_ACK;
262                                         break;
263                                 case 0x17: // Hall 4 LSB
264                                         TWDR = tmp16;
265                                         TWI_ACK;
266                                         break;
267                                 case 0x20: // Motor 1 speed wish MSB
268                                         TWDR = speed1_wish>>8;
269                                         TWI_ACK;
270                                         break;
271                                 case 0x21: // Motor 1 speed wish LSB
272                                         TWDR = speed1_wish;
273                                         TWI_ACK;
274                                         break;
275                                 case 0x22: // Motor 2 speed wish MSB
276                                         TWDR = speed2_wish>>8;
277                                         TWI_ACK;
278                                         break;
279                                 case 0x23: // Motor 2 speed wish LSB
280                                         TWDR = speed2_wish;
281                                         TWI_ACK;
282                                         break;
283                                 case 0x24: // Motor 3 speed wish MSB
284                                         TWDR = speed3_wish>>8;
285                                         TWI_ACK;
286                                         break;
287                                 case 0x25: // Motor 3 speed wish LSB
288                                         TWDR = speed3_wish;
289                                         TWI_ACK;
290                                         break;
291                                 case 0x26: // Motor 4 speed wish MSB
292                                         TWDR = speed4_wish>>8;
293                                         TWI_ACK;
294                                         break;
295                                 case 0x27: // Motor 4 speed wish LSB
296                                         TWDR = speed4_wish;
297                                         TWI_ACK;
298                                         break;
299                                 case 0x30: // Motor 1 speed MSB
300                                         TWDR = speed1>>8;
301                                         TWI_ACK;
302                                         break;
303                                 case 0x31: // Motor 1 speed LSB
304                                         TWDR = speed1;
305                                         TWI_ACK;
306                                         break;
307                                 case 0x32: // Motor 2 speed MSB
308                                         TWDR = speed2>>8;
309                                         TWI_ACK;
310                                         break;
311                                 case 0x33: // Motor 2 speed LSB
312                                         TWDR = speed2;
313                                         TWI_ACK;
314                                         break;
315                                 case 0x34: // Motor 3 speed MSB
316                                         TWDR = speed3>>8;
317                                         TWI_ACK;
318                                         break;
319                                 case 0x35: // Motor 3 speed LSB
320                                         TWDR = speed3;
321                                         TWI_ACK;
322                                         break;
323                                 case 0x36: // Motor 4 speed MSB
324                                         TWDR = speed4>>8;
325                                         TWI_ACK;
326                                         break;
327                                 case 0x37: // Motor 4 speed LSB
328                                         TWDR = speed4;
329                                         TWI_ACK;
330                                         break;
331                                 default:
332                                         TWDR = 0;
333                                         TWI_NAK;
334                         }
335                         ireg++;
336                         break;
337                 default:
338                         TWI_RESET;
339         }
340 }
341
342
343 static void update_hall1(void) {
344         unsigned char status = (PINA >> 0) & 0x3;
345         static unsigned char oldstatus=0;
346         unsigned char diff, new;
347
348         new = 0;
349         if (status & 0x1)
350                 new = 0x3;
351         if (status & 0x2)
352                 new ^= 0x1;                                     // convert gray to binary
353         diff = oldstatus - new;                         // difference last - new
354         if (diff & 0x1) {                               // bit 0 = value (1)
355                 oldstatus = new;                                        // store new as next last
356                 if (motor1_switch) pos1 += (diff & 2) - 1;              // bit 1 = direction (+/-)
357                 else pos1 -= (diff & 2) - 1;
358         }
359 }
360
361
362 static void update_hall2(void) {
363         unsigned char status = (PINA >> 4) & 0x3;
364         static unsigned char oldstatus=0;
365         unsigned char diff, new;
366
367         new = 0;
368         if (status & 0x1)
369                 new = 0x3;
370         if (status & 0x2)
371                 new ^= 0x1;                                     // convert gray to binary
372         diff = oldstatus - new;                         // difference last - new
373         if (diff & 0x1) {                               // bit 0 = value (1)
374                 oldstatus = new;                                        // store new as next last
375                 if (motor2_switch) pos2 -= (diff & 2) - 1;              // bit 1 = direction (+/-)
376                 else pos2 += (diff & 2) - 1;
377         }
378 }
379
380
381 static void update_hall3(void) {
382         unsigned char status = (PINA >> 2) & 0x3;
383         static unsigned char oldstatus=0;
384         unsigned char diff, new;
385
386         new = 0;
387         if (status & 0x1)
388                 new = 0x3;
389         if (status & 0x2)
390                 new ^= 0x1;                                     // convert gray to binary
391         diff = oldstatus - new;                         // difference last - new
392         if (diff & 0x1) {                               // bit 0 = value (1)
393                 oldstatus = new;                                        // store new as next last
394                 if (motor3_switch) pos3 -= (diff & 2) - 1;              // bit 1 = direction (+/-)
395                 else pos3 += (diff & 2) - 1;
396         }
397 }
398
399
400 static void update_hall4(void) {
401         unsigned char status = (PINA >> 6) & 0x3;
402         static unsigned char oldstatus=0;
403         unsigned char diff, new;
404
405         new = 0;
406         if (status & 0x1)
407                 new = 0x3;
408         if (status & 0x2)
409                 new ^= 0x1;                                     // convert gray to binary
410         diff = oldstatus - new;                         // difference last - new
411         if (diff & 0x1) {                               // bit 0 = value (1)
412                 oldstatus = new;                                        // store new as next last
413                 if (motor4_switch) pos4 += (diff & 2) - 1;              // bit 1 = direction (+/-)
414                 else pos4 -= (diff & 2) - 1;
415         }
416 }
417
418
419 static void update_motor(void) {
420         static int16_t m1_old=SHRT_MIN;
421         static int16_t m2_old=SHRT_MIN;
422         static int16_t m3_old=SHRT_MIN;
423         static int16_t m4_old=SHRT_MIN;
424
425         if (m1_old != motor1) { // update only when changed
426                 if (motor1 == 0) {
427                         // stop
428                         PORTC |= (1 << 3) | (1 << 2);
429                 } else if ((!motor1_switch && motor1 > 0) || (motor1_switch && motor1 < 0)) {
430                         // forward
431                         PORTC &= ~(1 << 3) & ~(1 << 2);
432                 } else { // motor1 < 0
433                         // backward
434                         PORTC &= ~(1 << 2);
435                         PORTC |=  (1 << 3);
436                 }
437
438                 m1_old = motor1;
439                 OCR1A = abs(motor1);
440         }
441
442         if (m2_old != motor2) { // update only when changed
443                 if (motor2 == 0) {
444                         // stop
445                         PORTC |= (1 << 5) | (1 << 4);
446                 } else if ((!motor2_switch && motor2 > 0) || (motor2_switch && motor2 < 0)) {
447                         // forward
448                         PORTC &= ~(1 << 5) & ~(1 << 4);
449                 } else { // motor2 < 0
450                         // backward
451                         PORTC &= ~(1 << 4);
452                         PORTC |=  (1 << 5);
453                 }
454
455                 m2_old = motor2;
456                 OCR1B = abs(motor2);
457         }
458
459         if (m3_old != motor3) { // update only when changed
460                 if (motor3 == 0) {
461                         // stop
462                         PORTC |= (1 << 7) | (1 << 6);
463                 } else if ((!motor3_switch && motor3 > 0) || (motor3_switch && motor3 < 0)) {
464                         // forward
465                         PORTC &= ~(1 << 7) & ~(1 << 6);
466                 } else { // motor3 < 0
467                         // backward
468                         PORTC &= ~(1 << 6);
469                         PORTC |=  (1 << 7);
470                 }
471
472                 m3_old = motor3;
473                 OCR2 = abs(motor3);
474         }
475
476         if (m4_old != motor4) { // update only when changed
477                 if (motor4 == 0) {
478                         // stop
479                         PORTD |= (1 << 3) | (1 << 2);
480                 } else if ((!motor4_switch && motor4 > 0) || (motor4_switch && motor4 < 0)) {
481                         // forward
482                         PORTD &= ~(1 << 3) & ~(1 << 2);
483                 } else { // motor4 < 0
484                         // backward
485                         PORTD &= ~(1 << 2);
486                         PORTD |=  (1 << 3);
487                 }
488
489                 m4_old = motor4;
490                 OCR0 = abs(motor4);
491         }
492 }
493
494
495 void update_pid(void) {
496         static int16_t pos1_last=0;
497         static int16_t pos2_last=0;
498         static int16_t pos3_last=0;
499         static int16_t pos4_last=0;
500         static int16_t eold1=0;
501         static int16_t eold2=0;
502         static int16_t eold3=0;
503         static int16_t eold4=0;
504         static int32_t esum1=0;
505         static int32_t esum2=0;
506         static int32_t esum3=0;
507         static int32_t esum4=0;
508
509         speed1 = (pos1 - pos1_last)/TIMER1_T;
510         pos1_last = pos1;
511         speed2 = (pos2 - pos2_last)/TIMER1_T;
512         pos2_last = pos2;
513         speed3 = (pos3 - pos3_last)/TIMER1_T;
514         pos3_last = pos3;
515         speed4 = (pos4 - pos4_last)/TIMER1_T;
516         pos4_last = pos4;
517
518         if (motor1_mode == MOTOR_PID) {
519                 if (speed1_wish == 0) {
520                         motor1 = 0;
521                 } else {
522                         int16_t e = speed1_wish - speed1;
523                         esum1+=e;
524                         motor1 += KP*e + KI*TIMER1_T*esum1 + KD/TIMER1_T*(e - eold1);
525                         eold1 = e;
526
527                         if (motor1 > 255) motor1 = 255;
528                         else if (motor1 < -255) motor1 = -255;
529                 }
530         }
531         if (motor2_mode == MOTOR_PID) {
532                 if (speed2_wish == 0) {
533                         motor2 = 0;
534                 } else {
535                         int16_t e = speed2_wish - speed2;
536                         esum2+=e;
537                         motor2 += KP*e + KI*TIMER1_T*esum2 + KD/TIMER1_T*(e - eold2);
538                         eold2 = e;
539
540                         if (motor2 > 255) motor2 = 255;
541                         else if (motor2 < -255) motor2 = -255;
542                 }
543         }
544         if (motor3_mode == MOTOR_PID) {
545                 if (speed3_wish == 0) {
546                         motor3 = 0;
547                 } else {
548                         int16_t e = speed3_wish - speed3;
549                         esum3+=e;
550                         motor3 += KP*e + KI*TIMER1_T*esum3 + KD/TIMER1_T*(e - eold3);
551                         eold3 = e;
552
553                         if (motor3 > 255) motor3 = 255;
554                         else if (motor3 < -255) motor3 = -255;
555                 }
556         }
557         if (motor4_mode == MOTOR_PID) {
558                 if (speed4_wish == 0) {
559                         motor4 = 0;
560                 } else {
561                         int16_t e = speed4_wish - speed4;
562                         esum4+=e;
563                         motor4 += KP*e + KI*TIMER1_T*esum4 + KD/TIMER1_T*(e - eold4);
564                         eold4 = e;
565
566                         if (motor4 > 255) motor4 = 255;
567                         else if (motor4 < -255) motor4 = -255;
568                 }
569         }
570 }
571
572
573 ISR(TIMER1_OVF_vect) {
574         update_hall1();
575         update_hall2();
576         update_hall3();
577         update_hall4();
578         
579         run_pid++;
580 }
581
582
583 int main(void) {
584         // Outputs
585         DDRB = (1 << 3);
586         DDRC = (1 << 7) | (1 << 6) | (1 << 5) | (1 << 4) | (1 << 3) | (1 << 2);
587         DDRD = (1 << 7) | (1 << 5) | (1 << 4) | (1 << 3) | (1 << 2);
588
589         bootloader = 0x00;
590         setup_uart(9600);
591         uart_setup_stdout();
592
593         // I2C
594         TWAR = 0x50;
595         TWI_RESET;
596
597         // Motor 1 & 2
598         // Timer 1: Fast PWM inverting mode, Top=256 => 15.625kHz
599         // Prescaler=1
600         TCCR1A = (1 << COM1A1) | (1 << COM1B1) | (1 << COM1A0) | (1 << COM1B0) | (1 << WGM10);
601         TCCR1B = (1 << WGM12) | (1 << CS10);
602         OCR1A = 0;
603         OCR1B = 0;
604
605         // Motor 3
606         // Timer 2: Fast PWM inverting mode, Top=256 => 15.625kHz
607         // Prescaler=1
608         TCCR2 = (1 << WGM21) | (1 << WGM20) | (1 << COM21) | (1 << COM20) | (1 << CS20);
609         OCR2 = 0;
610
611         // Motor 4
612         // Timer 0: Fast PWM inverting mode, Top=256 => 15.625kHz
613         // Prescaler=1
614         TCCR0 = (1 << WGM01) | (1 << WGM00) | (1 << COM01) | (1 << COM00) | (1 << CS00);
615         OCR0 = 0;
616
617         printf("\r\nStart\r\n");
618
619         set_sleep_mode(SLEEP_MODE_IDLE);
620         // Enable Timer 1 Overflow Interrupt
621         TIMSK = (1 << TOIE1);
622         sei();
623
624         while(1) {
625                 switch(ireg) {
626                         case 0xff: // Magic reg that starts the bootloader
627                                 if (bootloader == 0xa5) {
628                                         cli();
629                                         {
630                                                 void (*start)(void) = (void*)0x1800;
631                                                 start();
632                                         }
633                                 }
634                                 break;
635                 }
636                 
637
638                 if (run_pid >= 156) { // ~100Hz
639                         run_pid=0;
640                         update_pid();
641                 }
642
643                 update_motor();
644
645                 sleep_mode();
646         }
647
648         return 0;
649 }