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 }