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