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