5 #include <avr/interrupt.h>
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
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
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
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)
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;
101 static uint8_t tmp=0;
102 static int16_t tmp16=0;
106 case 0x60: // start write
112 case 0x00: // register select
114 ireg--; // because we do ireg++ below
117 case 0x01: // Motor 1 MSB
121 case 0x02: // Motor 1 LSB
122 motor1 = tmp<<8 | TWDR;
123 motor1_mode = MOTOR_MANUAL;
126 case 0x03: // Motor 2 MSB
130 case 0x04: // Motor 2 LSB
131 motor2 = tmp<<8 | TWDR;
132 motor2_mode = MOTOR_MANUAL;
135 case 0x05: // Motor 3 MSB
139 case 0x06: // Motor 3 LSB
140 motor3 = tmp<<8 | TWDR;
141 motor3_mode = MOTOR_MANUAL;
144 case 0x07: // Motor 4 MSB
148 case 0x08: // Motor 4 LSB
149 motor4 = tmp<<8 | TWDR;
150 motor4_mode = MOTOR_MANUAL;
153 case 0x20: // Motor 1 speed wish MSB
157 case 0x21: // Motor 1 speed wish LSB
158 speed1_wish = tmp<<8 | TWDR;
159 motor1_mode = MOTOR_PID;
162 case 0x22: // Motor 2 speed wish MSB
166 case 0x23: // Motor 2 speed wish LSB
167 speed2_wish = tmp<<8 | TWDR;
168 motor2_mode = MOTOR_PID;
171 case 0x24: // Motor 3 speed wish MSB
175 case 0x25: // Motor 3 speed wish LSB
176 speed3_wish = tmp<<8 | TWDR;
177 motor3_mode = MOTOR_PID;
180 case 0x26: // Motor 4 speed wish MSB
184 case 0x27: // Motor 4 speed wish LSB
185 speed4_wish = tmp<<8 | TWDR;
186 motor4_mode = MOTOR_PID;
189 case 0x90: // Motor 1 switch
190 motor1_switch = TWDR;
193 case 0x91: // Motor 2 switch
194 motor2_switch = TWDR;
197 case 0x92: // Motor 3 switch
198 motor3_switch = TWDR;
201 case 0x93: // Motor 4 switch
202 motor4_switch = TWDR;
205 case 0xff: // bootloader
212 case 0xA8: // start read
215 case 0x02: // Motor 1 PWM
219 case 0x04: // Motor 2 PWM
223 case 0x06: // Motor 3 PWM
227 case 0x08: // Motor 4 PWM
231 case 0x10: // Hall 1 MSB
236 case 0x11: // Hall 1 LSB
240 case 0x12: // Hall 2 MSB
245 case 0x13: // Hall 2 LSB
249 case 0x14: // Hall 3 MSB
254 case 0x15: // Hall 3 LSB
258 case 0x16: // Hall 4 MSB
263 case 0x17: // Hall 4 LSB
267 case 0x20: // Motor 1 speed wish MSB
268 TWDR = speed1_wish>>8;
271 case 0x21: // Motor 1 speed wish LSB
275 case 0x22: // Motor 2 speed wish MSB
276 TWDR = speed2_wish>>8;
279 case 0x23: // Motor 2 speed wish LSB
283 case 0x24: // Motor 3 speed wish MSB
284 TWDR = speed3_wish>>8;
287 case 0x25: // Motor 3 speed wish LSB
291 case 0x26: // Motor 4 speed wish MSB
292 TWDR = speed4_wish>>8;
295 case 0x27: // Motor 4 speed wish LSB
299 case 0x30: // Motor 1 speed MSB
303 case 0x31: // Motor 1 speed LSB
307 case 0x32: // Motor 2 speed MSB
311 case 0x33: // Motor 2 speed LSB
315 case 0x34: // Motor 3 speed MSB
319 case 0x35: // Motor 3 speed LSB
323 case 0x36: // Motor 4 speed MSB
327 case 0x37: // Motor 4 speed LSB
343 static void update_hall1(void) {
344 unsigned char status = (PINA >> 0) & 0x3;
345 static unsigned char oldstatus=0;
346 unsigned char diff, new;
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;
362 static void update_hall2(void) {
363 unsigned char status = (PINA >> 4) & 0x3;
364 static unsigned char oldstatus=0;
365 unsigned char diff, new;
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;
381 static void update_hall3(void) {
382 unsigned char status = (PINA >> 2) & 0x3;
383 static unsigned char oldstatus=0;
384 unsigned char diff, new;
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;
400 static void update_hall4(void) {
401 unsigned char status = (PINA >> 6) & 0x3;
402 static unsigned char oldstatus=0;
403 unsigned char diff, new;
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;
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;
425 if (m1_old != motor1) { // update only when changed
428 PORTC |= (1 << 3) | (1 << 2);
429 } else if ((!motor1_switch && motor1 > 0) || (motor1_switch && motor1 < 0)) {
431 PORTC &= ~(1 << 3) & ~(1 << 2);
432 } else { // motor1 < 0
442 if (m2_old != motor2) { // update only when changed
445 PORTC |= (1 << 5) | (1 << 4);
446 } else if ((!motor2_switch && motor2 > 0) || (motor2_switch && motor2 < 0)) {
448 PORTC &= ~(1 << 5) & ~(1 << 4);
449 } else { // motor2 < 0
459 if (m3_old != motor3) { // update only when changed
462 PORTC |= (1 << 7) | (1 << 6);
463 } else if ((!motor3_switch && motor3 > 0) || (motor3_switch && motor3 < 0)) {
465 PORTC &= ~(1 << 7) & ~(1 << 6);
466 } else { // motor3 < 0
476 if (m4_old != motor4) { // update only when changed
479 PORTD |= (1 << 3) | (1 << 2);
480 } else if ((!motor4_switch && motor4 > 0) || (motor4_switch && motor4 < 0)) {
482 PORTD &= ~(1 << 3) & ~(1 << 2);
483 } else { // motor4 < 0
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;
509 speed1 = (pos1 - pos1_last)/TIMER1_T;
511 speed2 = (pos2 - pos2_last)/TIMER1_T;
513 speed3 = (pos3 - pos3_last)/TIMER1_T;
515 speed4 = (pos4 - pos4_last)/TIMER1_T;
518 if (motor1_mode == MOTOR_PID) {
519 if (speed1_wish == 0) {
522 int16_t e = speed1_wish - speed1;
524 motor1 += KP*e + KI*TIMER1_T*esum1 + KD/TIMER1_T*(e - eold1);
527 if (motor1 > 255) motor1 = 255;
528 else if (motor1 < -255) motor1 = -255;
531 if (motor2_mode == MOTOR_PID) {
532 if (speed2_wish == 0) {
535 int16_t e = speed2_wish - speed2;
537 motor2 += KP*e + KI*TIMER1_T*esum2 + KD/TIMER1_T*(e - eold2);
540 if (motor2 > 255) motor2 = 255;
541 else if (motor2 < -255) motor2 = -255;
544 if (motor3_mode == MOTOR_PID) {
545 if (speed3_wish == 0) {
548 int16_t e = speed3_wish - speed3;
550 motor3 += KP*e + KI*TIMER1_T*esum3 + KD/TIMER1_T*(e - eold3);
553 if (motor3 > 255) motor3 = 255;
554 else if (motor3 < -255) motor3 = -255;
557 if (motor4_mode == MOTOR_PID) {
558 if (speed4_wish == 0) {
561 int16_t e = speed4_wish - speed4;
563 motor4 += KP*e + KI*TIMER1_T*esum4 + KD/TIMER1_T*(e - eold4);
566 if (motor4 > 255) motor4 = 255;
567 else if (motor4 < -255) motor4 = -255;
573 ISR(TIMER1_OVF_vect) {
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);
598 // Timer 1: Fast PWM inverting mode, Top=256 => 15.625kHz
600 TCCR1A = (1 << COM1A1) | (1 << COM1B1) | (1 << COM1A0) | (1 << COM1B0) | (1 << WGM10);
601 TCCR1B = (1 << WGM12) | (1 << CS10);
606 // Timer 2: Fast PWM inverting mode, Top=256 => 15.625kHz
608 TCCR2 = (1 << WGM21) | (1 << WGM20) | (1 << COM21) | (1 << COM20) | (1 << CS20);
612 // Timer 0: Fast PWM inverting mode, Top=256 => 15.625kHz
614 TCCR0 = (1 << WGM01) | (1 << WGM00) | (1 << COM01) | (1 << COM00) | (1 << CS00);
617 printf("\r\nStart\r\n");
619 set_sleep_mode(SLEEP_MODE_IDLE);
620 // Enable Timer 1 Overflow Interrupt
621 TIMSK = (1 << TOIE1);
626 case 0xff: // Magic reg that starts the bootloader
627 if (bootloader == 0xa5) {
630 void (*start)(void) = (void*)0x1800;
638 if (run_pid >= 156) { // ~100Hz