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 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;
93 static int16_t tmp16=0;
97 case 0x60: // start write
103 case 0x00: // register select
105 ireg--; // because we do ireg++ below
108 case 0x01: // Motor 1 MSB
112 case 0x02: // Motor 1 LSB
113 motor1 = tmp<<8 | TWDR;
114 motor1_mode = MOTOR_MANUAL;
117 case 0x03: // Motor 2 MSB
121 case 0x04: // Motor 2 LSB
122 motor2 = tmp<<8 | TWDR;
123 motor2_mode = MOTOR_MANUAL;
126 case 0x05: // Motor 3 MSB
130 case 0x06: // Motor 3 LSB
131 motor3 = tmp<<8 | TWDR;
132 motor3_mode = MOTOR_MANUAL;
135 case 0x07: // Motor 4 MSB
139 case 0x08: // Motor 4 LSB
140 motor4 = tmp<<8 | TWDR;
141 motor4_mode = MOTOR_MANUAL;
144 case 0x20: // Motor 1 speed wish MSB
148 case 0x21: // Motor 1 speed wish LSB
149 speed1_wish = tmp<<8 | TWDR;
150 motor1_mode = MOTOR_PID;
153 case 0x22: // Motor 2 speed wish MSB
157 case 0x23: // Motor 2 speed wish LSB
158 speed2_wish = tmp<<8 | TWDR;
159 motor2_mode = MOTOR_PID;
162 case 0x24: // Motor 3 speed wish MSB
166 case 0x25: // Motor 3 speed wish LSB
167 speed3_wish = tmp<<8 | TWDR;
168 motor3_mode = MOTOR_PID;
171 case 0x26: // Motor 4 speed wish MSB
175 case 0x27: // Motor 4 speed wish LSB
176 speed4_wish = tmp<<8 | TWDR;
177 motor4_mode = MOTOR_PID;
180 case 0x90: // Motor 1 switch
181 motor1_switch = TWDR;
184 case 0x91: // Motor 2 switch
185 motor2_switch = TWDR;
188 case 0x92: // Motor 3 switch
189 motor3_switch = TWDR;
192 case 0x93: // Motor 4 switch
193 motor4_switch = TWDR;
196 case 0xff: // bootloader
203 case 0xA8: // start read
206 case 0x02: // Motor 1 PWM
210 case 0x04: // Motor 2 PWM
214 case 0x06: // Motor 3 PWM
218 case 0x08: // Motor 4 PWM
222 case 0x10: // Hall 1 MSB
227 case 0x11: // Hall 1 LSB
231 case 0x12: // Hall 2 MSB
236 case 0x13: // Hall 2 LSB
240 case 0x14: // Hall 3 MSB
245 case 0x15: // Hall 3 LSB
249 case 0x16: // Hall 4 MSB
254 case 0x17: // Hall 4 LSB
258 case 0x20: // Motor 1 speed wish MSB
259 TWDR = speed1_wish>>8;
262 case 0x21: // Motor 1 speed wish LSB
266 case 0x22: // Motor 2 speed wish MSB
267 TWDR = speed2_wish>>8;
270 case 0x23: // Motor 2 speed wish LSB
274 case 0x24: // Motor 3 speed wish MSB
275 TWDR = speed3_wish>>8;
278 case 0x25: // Motor 3 speed wish LSB
282 case 0x26: // Motor 4 speed wish MSB
283 TWDR = speed4_wish>>8;
286 case 0x27: // Motor 4 speed wish LSB
302 static void update_hall1(void) {
303 unsigned char status = (PINA >> 0) & 0x3;
304 static unsigned char oldstatus=0;
305 unsigned char diff, new;
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;
321 static void update_hall2(void) {
322 unsigned char status = (PINA >> 2) & 0x3;
323 static unsigned char oldstatus=0;
324 unsigned char diff, new;
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;
340 static void update_hall3(void) {
341 unsigned char status = (PINA >> 4) & 0x3;
342 static unsigned char oldstatus=0;
343 unsigned char diff, new;
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;
359 static void update_hall4(void) {
360 unsigned char status = (PINA >> 6) & 0x3;
361 static unsigned char oldstatus=0;
362 unsigned char diff, new;
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;
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;
384 if (m1_old != motor1) { // update only when changed
387 PORTC |= (1 << 3) | (1 << 2);
388 } else if ((!motor1_switch && motor1 > 0) || (motor1_switch && motor1 < 0)) {
390 PORTC &= ~(1 << 3) & ~(1 << 2);
391 } else { // motor1 < 0
401 if (m2_old != motor2) { // update only when changed
404 PORTC |= (1 << 5) | (1 << 4);
405 } else if ((!motor2_switch && motor2 > 0) || (motor2_switch && motor2 < 0)) {
407 PORTC &= ~(1 << 5) & ~(1 << 4);
408 } else { // motor2 < 0
418 if (m3_old != motor3) { // update only when changed
421 PORTC |= (1 << 7) | (1 << 6);
422 } else if ((!motor3_switch && motor3 > 0) || (motor3_switch && motor3 < 0)) {
424 PORTC &= ~(1 << 7) & ~(1 << 6);
425 } else { // motor3 < 0
435 if (m4_old != motor4) { // update only when changed
438 PORTD |= (1 << 3) | (1 << 2);
439 } else if ((!motor4_switch && motor4 > 0) || (motor4_switch && motor4 < 0)) {
441 PORTD &= ~(1 << 3) & ~(1 << 2);
442 } else { // motor4 < 0
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;
468 if (motor1_mode == MOTOR_PID) {
469 speed1 = (pos1 - pos1_last)/TIMER0_T;
471 if (speed1_wish == 0) {
474 int16_t e = speed1_wish - speed1;
476 motor1 += KP*e + KI*TIMER0_T*esum1 + KD/TIMER0_T*(e - eold1);
479 if (motor1 > 255) motor1 = 255;
480 else if (motor1 < -255) motor1 = -255;
485 if (motor2_mode == MOTOR_PID) {
486 speed2 = (pos2 - pos2_last)/TIMER0_T;
488 if (speed2_wish == 0) {
491 int16_t e = speed2_wish - speed2;
493 motor2 += KP*e + KI*TIMER0_T*esum2 + KD/TIMER0_T*(e - eold2);
496 if (motor2 > 255) motor2 = 255;
497 else if (motor2 < -255) motor2 = -255;
502 if (motor3_mode == MOTOR_PID) {
503 speed3 = (pos3 - pos3_last)/TIMER0_T;
505 if (speed3_wish == 0) {
508 int16_t e = speed3_wish - speed3;
510 motor3 += KP*e + KI*TIMER0_T*esum3 + KD/TIMER0_T*(e - eold3);
513 if (motor3 > 255) motor3 = 255;
514 else if (motor3 < -255) motor3 = -255;
519 if (motor4_mode == MOTOR_PID) {
520 speed4 = (pos4 - pos4_last)/TIMER0_T;
522 if (speed4_wish == 0) {
525 int16_t e = speed4_wish - speed4;
527 motor4 += KP*e + KI*TIMER0_T*esum4 + KD/TIMER0_T*(e - eold4);
530 if (motor4 > 255) motor4 = 255;
531 else if (motor4 < -255) motor4 = -255;
539 ISR(TIMER1_OVF_vect) {
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);
564 // Timer 1: Fast PWM inverting mode, Top=256 => 15.625kHz
566 TCCR1A = (1 << COM1A1) | (1 << COM1B1) | (1 << COM1A0) | (1 << COM1B0) | (1 << WGM10);
567 TCCR1B = (1 << WGM12) | (1 << CS10);
572 // Timer 2: Fast PWM inverting mode, Top=256 => 15.625kHz
574 TCCR2 = (1 << WGM21) | (1 << WGM20) | (1 << COM21) | (1 << COM20) | (1 << CS20);
578 // Timer 0: Fast PWM inverting mode, Top=256 => 15.625kHz
580 TCCR0 = (1 << WGM01) | (1 << WGM00) | (1 << COM01) | (1 << COM00) | (1 << CS00);
583 printf("\r\nStart\r\n");
585 set_sleep_mode(SLEEP_MODE_IDLE);
586 // Enable Timer 1 Overflow Interrupt
587 TIMSK = (1 << TOIE1);
592 case 0xff: // Magic reg that starts the bootloader
593 if (bootloader == 0xa5) {
596 void (*start)(void) = (void*)0x1800;
604 if (run_pid >= 781) { // ~20Hz