modules/devices/brushless_motors/brushless_3phase_digital_hall_double/brushless.c

   1 /*
   2  *  Copyright Droids Corporation, Microb Technology, Eirbot (2005)
   3  *
   4  *  This program is free software; you can redistribute it and/or modify
   5  *  it under the terms of the GNU General Public License as published by
   6  *  the Free Software Foundation; either version 2 of the License, or
   7  *  (at your option) any later version.
   8  *
   9  *  This program is distributed in the hope that it will be useful,
  10  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  *  GNU General Public License for more details.
  13  *
  14  *  You should have received a copy of the GNU General Public License
  15  *  along with this program; if not, write to the Free Software
  16  *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
  17  *
  18  *  Revision : $Id: brushless.c,v 1.2.2.3 2007-05-23 17:18:12 zer0 Exp $
  19  *
  20  */
  21
  22
  23 /** This module handles a brushless motor with 3 phases, wired in triangle or star.
  24     3 hall sensors are used wih digital output.
  25     3 PWM are outputted, these MUST be synchronized !!
  26
  27     The control value is a voltage. This can be assimiled to a torque at low speeds.
  28
  29     There is a possibility of also of limiting the speed. This is accomplished by slowing down the sampling speed of the
  30     sensors. Doing this,the motor effective torque is reduced when the speed is such that the sensor actuation
  31     approaches the sampling frequency.
  32     use this technique carefully, because the motor has already his full voltage applied, an can dissipate a lot of energy, especially at low speeds.
  33
  34
  35     there is no external manage function, as the manage is done at the PWM speed (overflow of one PWM timer is used.)
  36     This function is speed optimized.
  37 */
  38
  39
  40 /** ceci est une commande de moteur brushless triphase en etoile,
  41     utilisant 3 capteurs a effet hall digitaux.
  42     En sortie on a 3 PWM (il est necessaire de les synchroniser !)
  43
  44     pour le commande en couple, on joue sur les pwm (pseudo-couple)
  45     et pour la commande en vitesse, on joue sur la frequence de rafraichissement des phases
  46     par rapport aux capteurs.
  47     cette astuce simple permet de faire facilement une consigne de vitesse
  48     (pas besoin d'asservissement)
  49     mais des vitesses faibles sont saccadees.*/
  50
  51
  52 #include <avr/io.h>
  53 #include <avr/signal.h>
  54
  55
  56 #include <pwm.h>
  57
  58 #include <aversive.h>
  59
  60
  61 #include <avr/pgmspace.h>
  62
  63 #include <brushless.h>
  64
  65 #include "brushless_3phase_digital_hall_double_config.h"
  66
  67 #if (BRUSHLESS_TYPE != BRUSHLESS_DIGITAL_DOUBLE)
  68 #error mauvais fichier de config "brushless_double_config.h"
  69 #endif
  70
  71
  72
  73
  74 /** calculating the event to use, based on PWM definition of the phase 1
  75     The two motors function on the same timer! */
  76
  77 #if (BRUSHLESS_TIMER == 0)
  78 #define INIT_INT()      sbi(TIMSK, TOIE0)
  79 #define INT SIG_OVERFLOW0
  80
  81 #elif (BRUSHLESS_TIMER == 1)
  82 #define INIT_INT()      sbi(TIMSK, TOIE1)
  83 #define INT SIG_OVERFLOW1
  84
  85 #elif (BRUSHLESS_TIMER == 2)
  86 #define INIT_INT()      sbi(TIMSK, TOIE2)
  87 #define INT SIG_OVERFLOW2
  88
  89 #elif (BRUSHLESS_TIMER == 3)
  90 #define INIT_INT()      sbi(ETIMSK, TOIE3) /* extended timsk for this one ! */
  91 #define INT SIG_OVERFLOW3
  92
  93 #endif
  94
  95
  96 /** 2 LUT tables:
  97     sensors > relative electric angle
  98     sensors > phase information
  99
 100
 101     decimal value :          0      1      2      3      4      5      6      7
 102     sensors state :        000    001    010    011    100    101    110    111
 103     is this a valid state? :         NO    yes    yes    yes    yes    yes    yes     NO     */
 104
 105 // conversion to electrical angle
 106 // modulo 1 electrical turn.
 107 // in RAM for faster acess
 108 const int8_t g_brushless_angle[]=          {0,     1,     5,     0,     3,     2,     4,     0};
 109
 110 // in progmem for ram optimization
 111 const int8_t PROGMEM g_brushless_phase1[]= {0,     1,    -1,     0,     0,     1,    -1,     0};
 112 const int8_t PROGMEM g_brushless_phase2[]= {0,     0,     1,     1,    -1,    -1,     0,     0};
 113 const int8_t PROGMEM g_brushless_phase3[]= {0,    -1,     0,    -1,     1,     0,     1,     0};
 114
 115 // the zeroes in the phase give a 0 output if there are no sensors connected, or another problem like this.
 116 // it could be a good idea to enable the pull up resistors !
 117
 118 void brushless_speed_update_manage(void * dummy);
 119
 120
 121 // mem of previous position for speed calc.
 122 brushless_position g_brushless_0_position_previous;
 123 brushless_position g_brushless_1_position_previous;
 124
 125 // memory for current measurements
 126 brushless   g_brushless_0;
 127 brushless   g_brushless_1;
 128
 129 // pseudo torque to use
 130 brushless_torque  g_brushless_0_torque;
 131 brushless_torque  g_brushless_1_torque;
 132
 133 // given with speed limit
 134 uint16_t g_brushless_0_pwm_divider = 1;
 135 uint16_t g_brushless_1_pwm_divider = 1;
 136
 137
 138 // function pointer definition for event
 139 void (*periodic_event_0)(brushless) = 0; //NULL;
 140 void (*periodic_event_1)(brushless) = 0; //NULL;
 141
 142
 143 /** This function is made of 5 parts :
 144     - pwm divsion         : gives a master frequency for the next parts, by dividing the PWM frequency (can also be set to 1)
 145     - sensors acquisition : done every time
 146     - angle update        : done when there is a change in sensor state since last angle update
 147     - PWM update          : done only every x (every g_brushless_recurrence -1) and skipped if there is no change since last pwm update
 148     - speed update        : done only every y () this updates the speed
 149
 150
 151     Typically, PWM update is slower than angle update, in order to not skip angle counting.
 152
 153
 154     Typically, speed update is a lot slower than angle update in order to have a speed that is sampled at a correct rate.
 155     This event can trigger an action, this is especially used if you have a servo control.
 156
 157
 158     examples for a pretty fast motor :
 159     - PWM at 30 kHz
 160     - angle update at 10 kHz
 161     - PWM update at 5 kHz or lower, depending on speed setting
 162     - speed update at 100 Hz
 163
 164 */
 165
 166
 167 volatile uint8_t pwm_division_timer asm("pwm_div")  = 1;
 168 volatile uint8_t interrupt_pwm;
 169 volatile uint8_t pwm_previous_sensors_0 = 0;     // previous state of sensors for PWM update
 170 volatile uint8_t pwm_previous_sensors_1 = 0;     // previous state of sensors for PWM update
 171
 172
 173
 174 #ifdef ASMHEADER
 175 /** Here the division is done in ASM before saving a lot of registers on the stack.
 176     This gains a lot of time when using a 8 bit timer without prescale
 177     (one interrupt every 256 clocks !!)
 178     */
 179
 180 void INT (void) __attribute__ ((naked)); // no register save, and no exit !!
 181 SIGNAL(INT)
 182 {
 183 asm volatile(
 184
 185 /* division, done very early, for optimization */
 186 "PUSH    R1              \n\t"
 187 "IN      R1,0x3F         \n\t"
 188 "PUSH    R1              \n\t"
 189 "LDS     R1,pwm_div      \n\t"
 190 "DEC     R1              \n\t"
 191 "BREQ    continue        \n\t"
 192 "STS     pwm_div,R1      \n\t" /*we store only if negative. if positive, it will be done in the C code*/
 193
 194
 195 /* early go out of int */
 196 "go_out:                 \n\t"
 197 "POP     R1              \n\t"
 198 "OUT     0x3F,R1         \n\t"
 199 "POP     R1              \n\t"
 200 "RETI                    \n\t"
 201
 202 /* restoring context, double work with the following interrupt function, but no other way */
 203 "continue:               \n\t"
 204 "POP     R1              \n\t"
 205 "OUT     0x3F,R1         \n\t"
 206 "POP     R1              \n\t"
 207
 208 ::);
 209
 210 }             // no jump, we pass implicitely.
 211 SIGNAL(DUMMY) // we hope that the two are after each other !! (it is always the case with GCC)
 212 {
 213
 214 #else // the same code in C
 215 SIGNAL(INT)
 216 {
 217 /** PWM division part */
 218 if (--pwm_division_timer != 0)
 219   return;
 220
 221
 222 #endif //common code
 223
 224
 225   pwm_division_timer = BRUSHLESS_PWM_TO_SAMPLE_DIVISOR;
 226   // end of frequency division state machine
 227
 228
 229
 230   /** various definitions */
 231
 232   // angle update variables
 233   static int8_t angle_electrical_previous_0 = 0; // previous electrical angle
 234   static int8_t angle_electrical_previous_1 = 0; // previous electrical angle
 235   static uint8_t angle_previous_sensors_0 = 0;     // previous state of sensors for angle calc
 236   static uint8_t angle_previous_sensors_1 = 0;     // previous state of sensors for angle calc
 237   int8_t angle_electrical, diff;
 238   uint8_t sensors_0 = 0;
 239   uint8_t sensors_1 = 0;
 240
 241   // pwm update variables
 242   static uint16_t pwm_update_timer_0 = 1;
 243   static uint16_t pwm_update_timer_1 = 1;
 244
 245
 246     {
 247     /*********** Motor 0 angle update  **********/
 248
 249
 250     /** sensors acquisition part
 251     extraction of the sensor signals, and built up of a 3 bit ordened byte
 252     this is done every time */
 253
 254     if(bit_is_set(PIN(BRUSHLESS_0_SENSOR_1_PORT),BRUSHLESS_0_SENSOR_1_BIT))
 255       sensors_0 +=1;
 256     if(bit_is_set(PIN(BRUSHLESS_0_SENSOR_2_PORT),BRUSHLESS_0_SENSOR_2_BIT))
 257       sensors_0 +=2;
 258     if(bit_is_set(PIN(BRUSHLESS_0_SENSOR_3_PORT),BRUSHLESS_0_SENSOR_3_BIT))
 259       sensors_0 +=4;
 260
 261     #ifdef BRUSHLESS_0_SENSORS_INVERT
 262       sensors_0  = (~ sensors_0) & 0x7;  // inversion of the sensors
 263     #endif
 264
 265
 266
 267     /** the angle update part */
 268
 269     #ifndef LOADTEST
 270     // skipping if no change
 271     if(sensors_0 != angle_previous_sensors_0)
 272     #endif
 273       {
 274       angle_previous_sensors_0 = sensors_0;
 275
 276
 277       angle_electrical = g_brushless_angle[sensors_0];      // calculate electrical angle
 278
 279       diff = angle_electrical - angle_electrical_previous_0;  // convert to angle delta
 280       angle_electrical_previous_0 = angle_electrical;         // store for next time
 281
 282       // clipping
 283       if (diff > 3)
 284         diff -=6;
 285       else if (diff < -3)
 286         diff +=6;
 287
 288       #ifndef BRUSHLESS_0_INVERT
 289         diff *= -1;    // inversion of the angle reading
 290       #endif
 291
 292       // update of the absolute angle with the delta
 293       //IRQ_LOCK(); // not necessary coz we enable ints only after
 294       g_brushless_0.position -= (brushless_position)diff * BRUSHLESS_POSITION_PRECISION;
 295       //IRQ_UNLOCK();
 296
 297       }
 298
 299     /*********** END Motor 0 angle update  **********/
 300
 301     /*********** Motor 1 angle update  **********/
 302
 303
 304     /** sensors acquisition part
 305     extraction of the sensor signals, and built up of a 3 bit ordened byte
 306     this is done every time */
 307
 308     if(bit_is_set(PIN(BRUSHLESS_1_SENSOR_1_PORT),BRUSHLESS_1_SENSOR_1_BIT))
 309       sensors_1 +=1;
 310     if(bit_is_set(PIN(BRUSHLESS_1_SENSOR_2_PORT),BRUSHLESS_1_SENSOR_2_BIT))
 311       sensors_1 +=2;
 312     if(bit_is_set(PIN(BRUSHLESS_1_SENSOR_3_PORT),BRUSHLESS_1_SENSOR_3_BIT))
 313       sensors_1 +=4;
 314
 315     #ifdef BRUSHLESS_1_SENSORS_INVERT
 316       sensors_1  = (~ sensors_1) & 0x7;  // inversion of the sensors
 317     #endif
 318
 319
 320
 321     /** the angle update part */
 322
 323     #ifndef LOADTEST
 324     // skipping if no change
 325     if(sensors_1 != angle_previous_sensors_1)
 326     #endif
 327       {
 328       angle_previous_sensors_1 = sensors_1;
 329
 330
 331       angle_electrical = g_brushless_angle[sensors_1];      // calculate electrical angle
 332
 333       diff = angle_electrical - angle_electrical_previous_1;  // convert to angle delta
 334       angle_electrical_previous_1 = angle_electrical;         // store for next time
 335
 336       // clipping
 337       if (diff > 3)
 338         diff -=6;
 339       else if (diff < -3)
 340         diff +=6;
 341
 342       #ifndef BRUSHLESS_1_INVERT
 343         diff *= -1;    // inversion of the angle reading
 344       #endif
 345
 346       // update of the absolute angle with the delta
 347       //IRQ_LOCK(); // not necessary coz we enable ints only after
 348       g_brushless_1.position -= (brushless_position)diff * BRUSHLESS_POSITION_PRECISION;
 349       //IRQ_UNLOCK();
 350
 351       }
 352
 353 /*********** END Motor 1 angle update  **********/
 354     }
 355
 356
 357   if(interrupt_pwm ==0)
 358     {
 359     interrupt_pwm =1;
 360
 361     sei();
 362
 363     /*********** Motor 0 PWM update  **********/
 364
 365     // frequency division state machine
 366     if (--pwm_update_timer_0 == 0)
 367       {
 368       uint8_t flags;
 369       IRQ_LOCK(flags);
 370       pwm_update_timer_0 = g_brushless_0_pwm_divider; // protected against glitches
 371       IRQ_UNLOCK(flags);
 372       // end of frequency division state machine
 373
 374       #ifndef LOADTEST
 375       // skipping if same as last time
 376       if(sensors_0 != pwm_previous_sensors_0)
 377       #endif
 378         {
 379         brushless_torque torque;
 380
 381         pwm_previous_sensors_0 = sensors_0;
 382
 383
 384         IRQ_LOCK(flags);
 385         torque = g_brushless_0_torque;
 386         IRQ_UNLOCK(flags);
 387
 388
 389
 390
 391         BRUSHLESS_0_PWM_SET_1(
 392                               PWM_MAX/2 +                         // offset 50%
 393                               ((int8_t)pgm_read_byte(g_brushless_phase1 + sensors_0)) * // conversion from sensors
 394                               torque /*>> 1*/ );      // torque must be divided by 2. this is now done in the acess function
 395         BRUSHLESS_0_PWM_SET_2(
 396                               PWM_MAX/2 +
 397                               ((int8_t)pgm_read_byte(g_brushless_phase2 + sensors_0)) *
 398                               torque /*>> 1*/ );
 399         BRUSHLESS_0_PWM_SET_3(
 400                               PWM_MAX/2 +
 401                               ((int8_t)pgm_read_byte(g_brushless_phase3 + sensors_0)) *
 402                               torque /*>> 1*/ );
 403         }
 404       }
 405
 406
 407     /*********** END Motor 0 PWM update  **********/
 408
 409
 410     /*********** Motor 1 PWM update  **********/
 411
 412     // frequency division state machine
 413     if (--pwm_update_timer_1 == 0)
 414       {
 415       uint8_t flags;
 416       IRQ_LOCK(flags);
 417       pwm_update_timer_1 = g_brushless_1_pwm_divider; // protected against glitches
 418       IRQ_UNLOCK(flags);
 419       // end of frequency division state machine
 420
 421       #ifndef LOADTEST
 422       // skipping if same as last time
 423       if(sensors_1 != pwm_previous_sensors_1)
 424       #endif
 425         {
 426         brushless_torque torque;
 427
 428         pwm_previous_sensors_1 = sensors_1;
 429
 430
 431         IRQ_LOCK(flags);
 432         torque = g_brushless_1_torque;
 433         IRQ_UNLOCK(flags);
 434
 435
 436
 437
 438         BRUSHLESS_1_PWM_SET_1(
 439                               PWM_MAX/2 +                         // offset 50%
 440                               ((int8_t)pgm_read_byte(g_brushless_phase1 + sensors_1)) * // conversion from sensors
 441                               torque /*>> 1*/ );      // torque must be divided by 2. this is now done in the acess function
 442         BRUSHLESS_1_PWM_SET_2(
 443                               PWM_MAX/2 +
 444                               ((int8_t)pgm_read_byte(g_brushless_phase2 + sensors_1)) *
 445                               torque /*>> 1*/ );
 446         BRUSHLESS_1_PWM_SET_3(
 447                               PWM_MAX/2 +
 448                               ((int8_t)pgm_read_byte(g_brushless_phase3 + sensors_1)) *
 449                               torque /*>> 1*/ );
 450         }
 451       }
 452     /*********** END Motor 1 PWM update  **********/
 453     interrupt_pwm =0;
 454     }
 455
 456   /** speed update part */
 457
 458 #ifndef BRUSHLESS_MANAGE_EXTERNAL
 459   // speed update variables
 460   static uint16_t speed_division_timer = 1; // only one needed
 461   // frequency division state machine
 462   if (--speed_division_timer == 0)
 463     {
 464
 465     speed_division_timer = BRUSHLESS_SAMPLE_TO_EVENT_DIVISOR;
 466     // end of frequency division state machine
 467
 468     brushless_speed_update_manage((void *)0);
 469
 470     }
 471 #endif
 472
 473 }
 474
 475 void brushless_speed_update_manage(void * dummy)
 476 {
 477
 478         uint8_t flags;
 479
 480         // speed calculation, protected against glitches
 481         IRQ_LOCK(flags);
 482         g_brushless_0.speed = g_brushless_0.position - g_brushless_0_position_previous;
 483         g_brushless_0_position_previous = g_brushless_0.position;
 484         IRQ_UNLOCK(flags);
 485
 486         IRQ_LOCK(flags);
 487         g_brushless_1.speed = g_brushless_1.position - g_brushless_1_position_previous;
 488         g_brushless_1_position_previous = g_brushless_1.position;
 489         IRQ_UNLOCK(flags);
 490
 491         // event call, with no imbrication autorized !
 492         {
 493                 void (*f)(brushless);
 494                 static volatile uint8_t in_progress = 0;
 495
 496                 if(in_progress ==0) {
 497                         in_progress = 1;
 498
 499                         IRQ_LOCK(flags);
 500                         f = periodic_event_0;
 501                         IRQ_UNLOCK(flags);
 502
 503                         if(f)
 504                                 f(g_brushless_0);
 505
 506
 507                         IRQ_LOCK(flags);
 508                         f = periodic_event_1;
 509                         IRQ_UNLOCK(flags);
 510
 511                         if(f)
 512                                 f(g_brushless_1);
 513
 514                         in_progress = 0;
 515                 }
 516         }
 517 }
 518
 519
 520
 521 brushless_speed speed_mem_0   = BRUSHLESS_MAX_SPEED;
 522 brushless_torque torque_mem_0 = 0;
 523 brushless_speed speed_mem_1   = BRUSHLESS_MAX_SPEED;
 524 brushless_torque torque_mem_1 = 0;
 525
 526
 527 /** initialisation, also executes pwm_init */
 528 void brushless_init(void)
 529 {
 530
 531         pwm_init();
 532
 533         // pull up resistors enable, if feature enabled
 534 #ifdef BRUSHLESS_0_SENSORS_PULL_UP_RESISTORS
 535         sbi(BRUSHLESS_0_SENSOR_1_PORT,BRUSHLESS_0_SENSOR_1_BIT);
 536         sbi(BRUSHLESS_0_SENSOR_2_PORT,BRUSHLESS_0_SENSOR_2_BIT);
 537         sbi(BRUSHLESS_0_SENSOR_3_PORT,BRUSHLESS_0_SENSOR_3_BIT);
 538 #endif
 539
 540 #ifdef BRUSHLESS_1_SENSORS_PULL_UP_RESISTORS
 541         sbi(BRUSHLESS_1_SENSOR_1_PORT,BRUSHLESS_1_SENSOR_1_BIT);
 542         sbi(BRUSHLESS_1_SENSOR_2_PORT,BRUSHLESS_1_SENSOR_2_BIT);
 543         sbi(BRUSHLESS_1_SENSOR_3_PORT,BRUSHLESS_1_SENSOR_3_BIT);
 544 #endif
 545
 546
 547         INIT_INT();
 548
 549 }
 550
 551
 552
 553
 554 /******** set parameters, two times *******************/
 555 void brushless_0_set_parameters(brushless_speed speed, brushless_torque torque)
 556 {
 557         uint16_t pwm_divider = 0;
 558         uint8_t flags;
 559
 560         // memory of settings
 561         speed_mem_0 = speed;
 562         torque_mem_0 = torque;
 563
 564
 565         if(speed ==0)
 566                 torque = 0;
 567         else
 568                 pwm_divider =  BRUSHLESS_MAX_SPEED / speed ;
 569
 570         if (pwm_divider ==0)
 571                 pwm_divider =1;
 572
 573         torque /= 2; // division is made here instead of in int function
 574
 575         // inversion
 576 #ifdef BRUSHLESS_0_INVERT
 577         torque *= -1;
 578 #endif
 579
 580         IRQ_LOCK(flags);
 581         g_brushless_0_pwm_divider  = pwm_divider;
 582         g_brushless_0_torque = torque;
 583         IRQ_UNLOCK(flags);
 584
 585         pwm_previous_sensors_0 = 0; // force application of the torque
 586 }
 587
 588 void brushless_1_set_parameters(brushless_speed speed, brushless_torque torque)
 589 {
 590         uint16_t pwm_divider = 0;
 591         uint8_t flags;
 592
 593         // memory of settings
 594         speed_mem_1 = speed;
 595         torque_mem_1 = torque;
 596
 597
 598         if(speed ==0)
 599                 torque = 0;
 600         else
 601                 pwm_divider =  BRUSHLESS_MAX_SPEED / speed ;
 602
 603         if (pwm_divider ==0)
 604                 pwm_divider =1;
 605
 606         torque /= 2; // division is made here instead of in int function
 607
 608         // inversion
 609 #ifdef BRUSHLESS_1_INVERT
 610         torque *= -1;
 611 #endif
 612
 613         IRQ_LOCK(flags);
 614         g_brushless_1_pwm_divider  = pwm_divider;
 615         g_brushless_1_torque = torque;
 616         IRQ_UNLOCK(flags);
 617
 618         pwm_previous_sensors_1 = 0; // force application of the torque
 619 }
 620
 621
 622 /******** get current speed and position, two times *******************/
 623 brushless brushless_0_get_mesures(void)
 624 {
 625         brushless ret;
 626         uint8_t flags;
 627
 628         IRQ_LOCK(flags);
 629         ret = g_brushless_0;
 630         IRQ_UNLOCK(flags);
 631
 632         return ret;
 633 }
 634 brushless brushless_1_get_mesures(void)
 635 {
 636         brushless ret;
 637         uint8_t flags;
 638
 639         IRQ_LOCK(flags);
 640         ret = g_brushless_1;
 641         IRQ_UNLOCK(flags);
 642
 643         return ret;
 644 }
 645
 646
 647 /******** set the position counter, two times *******************/
 648 void brushless_0_set_position(brushless_position p)
 649 {
 650         uint8_t flags;
 651         IRQ_LOCK(flags);
 652         g_brushless_0_position_previous += p - g_brushless_0.position; // avoids speed glitches  on pos change
 653         g_brushless_0.position = p;
 654         IRQ_UNLOCK(flags);
 655 }
 656 void brushless_1_set_position(brushless_position p)
 657 {
 658         uint8_t flags;
 659         IRQ_LOCK(flags);
 660         g_brushless_1_position_previous += p - g_brushless_1.position; // avoids speed glitches  on pos change
 661         g_brushless_1.position = p;
 662         IRQ_UNLOCK(flags);
 663 }
 664
 665
 666 void brushless_0_register_periodic_event(void (*f)(brushless))
 667 {
 668         uint8_t flags;
 669         IRQ_LOCK(flags);
 670         periodic_event_0 = f;
 671         IRQ_UNLOCK(flags);
 672 }
 673 void brushless_1_register_periodic_event(void (*f)(brushless))
 674 {
 675         uint8_t flags;
 676         IRQ_LOCK(flags);
 677         periodic_event_1 = f;
 678         IRQ_UNLOCK(flags);
 679 }
 680
 681
 682
 683 /** acess functions for the control system interface */
 684
 685
 686 /** get speed function, compatible with control_system. Argument not used. */
 687 int32_t brushless_get_speed(void * motor_num)
 688 {
 689         brushless retour;
 690
 691         if(motor_num)
 692                 retour = brushless_1_get_mesures();
 693         else
 694                 retour = brushless_0_get_mesures();
 695
 696         return (int32_t)(retour.speed);
 697 }
 698
 699 /** get position function, compatible with control_system. Argument not used. */
 700 int32_t brushless_get_pos(void * motor_num)
 701 {
 702         brushless retour;
 703
 704         if(motor_num)
 705                 retour = brushless_1_get_mesures();
 706         else
 707                 retour = brushless_0_get_mesures();
 708
 709         return (int32_t)(retour.position);
 710 }
 711
 712 /** set torque function, compatible with control_system. first argument not used. */
 713 void brushless_set_torque(void * motor_num, int32_t torque)
 714 {
 715         if(motor_num)
 716                 brushless_1_set_parameters(speed_mem_1, (brushless_torque) torque);
 717         else
 718                 brushless_0_set_parameters(speed_mem_0, (brushless_torque) torque);
 719 }
 720
 721 /** set speed function, compatible with control_system. first argument not used. */
 722 void brushless_set_speed(void * motor_num, int32_t speed)
 723 {
 724         if(motor_num)
 725                 brushless_1_set_parameters((brushless_speed) speed, torque_mem_1);
 726         else
 727                 brushless_0_set_parameters((brushless_speed) speed, torque_mem_0);
 728 }