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