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
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)
55 #define TIMER0_T 0.020
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;
103 static uint8_t tmp=0;
104 static int16_t tmp16=0;
108 case 0x60: // start write
114 case 0x00: // register select
116 ireg--; // because we do ireg++ below
119 case 0x01: // Motor 1 MSB
123 case 0x02: // Motor 1 LSB
124 motor1 = tmp<<8 | TWDR;
125 motor1_mode = MOTOR_MANUAL;
128 case 0x03: // Motor 2 MSB
132 case 0x04: // Motor 2 LSB
133 motor2 = tmp<<8 | TWDR;
134 motor2_mode = MOTOR_MANUAL;
137 case 0x05: // Motor 3 MSB
141 case 0x06: // Motor 3 LSB
142 motor3 = tmp<<8 | TWDR;
143 motor3_mode = MOTOR_MANUAL;
146 case 0x07: // Motor 4 MSB
150 case 0x08: // Motor 4 LSB
151 motor4 = tmp<<8 | TWDR;
152 motor4_mode = MOTOR_MANUAL;
155 case 0x20: // Motor 1 speed wish MSB
159 case 0x21: // Motor 1 speed wish LSB
160 speed1_wish = tmp<<8 | TWDR;
161 motor1_mode = MOTOR_PID;
164 case 0x22: // Motor 2 speed wish MSB
168 case 0x23: // Motor 2 speed wish LSB
169 speed2_wish = tmp<<8 | TWDR;
170 motor2_mode = MOTOR_PID;
173 case 0x24: // Motor 3 speed wish MSB
177 case 0x25: // Motor 3 speed wish LSB
178 speed3_wish = tmp<<8 | TWDR;
179 motor3_mode = MOTOR_PID;
182 case 0x26: // Motor 4 speed wish MSB
186 case 0x27: // Motor 4 speed wish LSB
187 speed4_wish = tmp<<8 | TWDR;
188 motor4_mode = MOTOR_PID;
191 case 0x90: // Motor 1 switch
192 motor1_switch = TWDR;
195 case 0x91: // Motor 2 switch
196 motor2_switch = TWDR;
199 case 0x92: // Motor 3 switch
200 motor3_switch = TWDR;
203 case 0x93: // Motor 4 switch
204 motor4_switch = TWDR;
207 case 0xff: // bootloader
214 case 0xA8: // start read
217 case 0x02: // Motor 1 PWM
221 case 0x04: // Motor 2 PWM
225 case 0x06: // Motor 3 PWM
229 case 0x08: // Motor 4 PWM
233 case 0x10: // Hall 1 MSB
238 case 0x11: // Hall 1 LSB
242 case 0x12: // Hall 2 MSB
247 case 0x13: // Hall 2 LSB
251 case 0x14: // Hall 3 MSB
256 case 0x15: // Hall 3 LSB
260 case 0x16: // Hall 4 MSB
265 case 0x17: // Hall 4 LSB
269 case 0x20: // Motor 1 speed wish MSB
270 TWDR = speed1_wish>>8;
273 case 0x21: // Motor 1 speed wish LSB
277 case 0x22: // Motor 2 speed wish MSB
278 TWDR = speed2_wish>>8;
281 case 0x23: // Motor 2 speed wish LSB
285 case 0x24: // Motor 3 speed wish MSB
286 TWDR = speed3_wish>>8;
289 case 0x25: // Motor 3 speed wish LSB
293 case 0x26: // Motor 4 speed wish MSB
294 TWDR = speed4_wish>>8;
297 case 0x27: // Motor 4 speed wish LSB
313 static void update_hall1(void) {
314 unsigned char status = (PINA >> 0) & 0x3;
315 static unsigned char oldstatus=0;
316 unsigned char diff, new;
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;
332 static void update_hall2(void) {
333 unsigned char status = (PINA >> 2) & 0x3;
334 static unsigned char oldstatus=0;
335 unsigned char diff, new;
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;
351 static void update_hall3(void) {
352 unsigned char status = (PINA >> 4) & 0x3;
353 static unsigned char oldstatus=0;
354 unsigned char diff, new;
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;
370 static void update_hall4(void) {
371 unsigned char status = (PINA >> 6) & 0x3;
372 static unsigned char oldstatus=0;
373 unsigned char diff, new;
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;
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;
395 if (m1_old != motor1) { // update only when changed
398 PORTC |= (1 << 3) | (1 << 2);
399 } else if ((!motor1_switch && motor1 > 0) || (motor1_switch && motor1 < 0)) {
401 PORTC &= ~(1 << 3) & ~(1 << 2);
402 } else { // motor1 < 0
412 if (m2_old != motor2) { // update only when changed
415 PORTC |= (1 << 5) | (1 << 4);
416 } else if ((!motor2_switch && motor2 > 0) || (motor2_switch && motor2 < 0)) {
418 PORTC &= ~(1 << 5) & ~(1 << 4);
419 } else { // motor2 < 0
429 if (m3_old != motor3) { // update only when changed
432 PORTC |= (1 << 7) | (1 << 6);
433 } else if ((!motor3_switch && motor3 > 0) || (motor3_switch && motor3 < 0)) {
435 PORTC &= ~(1 << 7) & ~(1 << 6);
436 } else { // motor3 < 0
446 if (m4_old != motor4) { // update only when changed
449 PORTD |= (1 << 3) | (1 << 2);
450 } else if ((!motor4_switch && motor4 > 0) || (motor4_switch && motor4 < 0)) {
452 PORTD &= ~(1 << 3) & ~(1 << 2);
453 } else { // motor4 < 0
465 ISR(TIMER0_OVF_vect) {
472 if (motor1_mode == MOTOR_PID) {
473 speed1 = (pos1 - pos1_last)/TIMER0_T;
475 if (speed1_wish == 0) {
478 int16_t e = speed1_wish - speed1;
480 motor1 += KP*e + KI*TIMER0_T*esum1 + KD/TIMER0_T*(e - eold1);
483 if (motor1 > 255) motor1 = 255;
484 else if (motor1 < -255) motor1 = -255;
489 if (motor2_mode == MOTOR_PID) {
490 speed2 = (pos2 - pos2_last)/TIMER0_T;
492 if (speed2_wish == 0) {
495 int16_t e = speed2_wish - speed2;
497 motor2 += KP*e + KI*TIMER0_T*esum2 + KD/TIMER0_T*(e - eold2);
500 if (motor2 > 255) motor2 = 255;
501 else if (motor2 < -255) motor2 = -255;
506 if (motor3_mode == MOTOR_PID) {
507 speed3 = (pos3 - pos3_last)/TIMER0_T;
509 if (speed3_wish == 0) {
512 int16_t e = speed3_wish - speed3;
514 motor3 += KP*e + KI*TIMER0_T*esum3 + KD/TIMER0_T*(e - eold3);
517 if (motor3 > 255) motor3 = 255;
518 else if (motor3 < -255) motor3 = -255;
523 if (motor4_mode == MOTOR_PID) {
524 speed4 = (pos4 - pos4_last)/TIMER0_T;
526 if (speed4_wish == 0) {
529 int16_t e = speed4_wish - speed4;
531 motor4 += KP*e + KI*TIMER0_T*esum4 + KD/TIMER0_T*(e - eold4);
534 if (motor4 > 255) motor4 = 255;
535 else if (motor4 < -255) motor4 = -255;
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);
558 // Timer 1: Fast PWM inverting mode, Top=256 => 15.625Hz
560 TCCR1A = (1 << COM1A1) | (1 << COM1B1) | (1 << WGM10);
561 TCCR1B = (1 << WGM12) | (1 << CS10);
566 // Timer 2: Fast PWM, Top=256 => 15.625Hz
568 TCCR2 = (1 << WGM21) | (1 << WGM20) | (1 << COM21) | (1 << CS20);
572 // Timer 0: Fast PWM, Top=256 => 15.625Hz
574 TCCR0 = (1 << WGM01) | (1 << WGM00) | (1 << COM01) | (1 << CS00);
577 printf("\r\nStart\r\n");
579 set_sleep_mode(SLEEP_MODE_IDLE);
580 // Enable Timer 1 Overflow Interrupt
581 TIMSK = (1 << TOIE1);
586 case 0xff: // Magic reg that starts the bootloader
587 if (bootloader == 0xa5) {
590 void (*start)(void) = (void*)0x1800;