modules/devices/brushless_motors/brushless_3phase_digital_hall/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_config.h"
  66
  67 #if (BRUSHLESS_TYPE != BRUSHLESS_DIGITAL)
  68 #error mauvais fichier de config "brushless_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
 119 void brushless_speed_update_manage(void * dummy);
 120
 121 // mem of previous position for speed calc.
 122 brushless_position g_brushless_0_position_previous;
 123
 124
 125 // memory for current measurements
 126 brushless   g_brushless_0;
 127
 128
 129 // pseudo torque to use
 130 brushless_torque  g_brushless_0_torque;
 131
 132
 133 // given with speed limit
 134 uint16_t g_brushless_0_pwm_divider = 1;
 135
 136
 137
 138 // function pointer definition for event
 139 void (*periodic_event_0)(brushless) = 0; //NULL;
 140
 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
 171
 172
 173 #ifdef ASMHEADER
 174 /** Here the division is done in ASM before saving a lot of registers on the stack.
 175     This gains a lot of time when using a 8 bit timer without prescale
 176     (one interrupt every 256 clocks !!)
 177     */
 178
 179 void INT (void) __attribute__ ((naked)); // no register save, and no exit !!
 180 SIGNAL(INT)
 181 {
 182 asm volatile(
 183
 184 /* division, done very early, for optimization */
 185 "PUSH    R1              \n\t"
 186 "IN      R1,0x3F         \n\t"
 187 "PUSH    R1              \n\t"
 188 "LDS     R1,pwm_div      \n\t"
 189 "DEC     R1              \n\t"
 190 "BREQ    continue        \n\t"
 191 "STS     pwm_div,R1      \n\t" /*we store only if negative. if positive, it will be done in the C code*/
 192
 193
 194 /* early go out of int */
 195 "go_out:                 \n\t"
 196 "POP     R1              \n\t"
 197 "OUT     0x3F,R1         \n\t"
 198 "POP     R1              \n\t"
 199 "RETI                    \n\t"
 200
 201 /* restoring context, double work with the following interrupt function, but no other way */
 202 "continue:               \n\t"
 203 "POP     R1              \n\t"
 204 "OUT     0x3F,R1         \n\t"
 205 "POP     R1              \n\t"
 206
 207 ::);
 208
 209 }             // no jump, we pass implicitely.
 210 SIGNAL(DUMMY) // we hope that the two are after each other !! (it is always the case with GCC)
 211 {
 212
 213 #else // the same code in C
 214 SIGNAL(INT)
 215 {
 216 /** PWM division part */
 217 if (--pwm_division_timer != 0)
 218   return;
 219
 220
 221 #endif //common code
 222
 223
 224   pwm_division_timer = BRUSHLESS_PWM_TO_SAMPLE_DIVISOR;
 225   // end of frequency division state machine
 226
 227
 228
 229   /** various definitions */
 230
 231   // angle update variables
 232   static int8_t angle_electrical_previous_0 = 0; // previous electrical angle
 233
 234   static uint8_t angle_previous_sensors_0 = 0;     // previous state of sensors for angle calc
 235
 236   int8_t angle_electrical, diff;
 237   uint8_t sensors_0 = 0;
 238
 239
 240   // pwm update variables
 241   static uint16_t pwm_update_timer_0 = 1;
 242
 243
 244
 245     {
 246     /*********** Motor 0 angle update  **********/
 247
 248
 249     /** sensors acquisition part
 250     extraction of the sensor signals, and built up of a 3 bit ordened byte
 251     this is done every time */
 252
 253     if(bit_is_set(PIN(BRUSHLESS_0_SENSOR_1_PORT),BRUSHLESS_0_SENSOR_1_BIT))
 254       sensors_0 +=1;
 255     if(bit_is_set(PIN(BRUSHLESS_0_SENSOR_2_PORT),BRUSHLESS_0_SENSOR_2_BIT))
 256       sensors_0 +=2;
 257     if(bit_is_set(PIN(BRUSHLESS_0_SENSOR_3_PORT),BRUSHLESS_0_SENSOR_3_BIT))
 258       sensors_0 +=4;
 259
 260     #ifdef BRUSHLESS_0_SENSORS_INVERT
 261       sensors_0  = (~ sensors_0) & 0x7;  // inversion of the sensors
 262     #endif
 263
 264
 265
 266     /** the angle update part */
 267
 268     #ifndef LOADTEST
 269     // skipping if no change
 270     if(sensors_0 != angle_previous_sensors_0)
 271     #endif
 272       {
 273       angle_previous_sensors_0 = sensors_0;
 274
 275
 276       angle_electrical = g_brushless_angle[sensors_0];      // calculate electrical angle
 277
 278       diff = angle_electrical - angle_electrical_previous_0;  // convert to angle delta
 279       angle_electrical_previous_0 = angle_electrical;         // store for next time
 280
 281       // clipping
 282       if (diff > 3)
 283         diff -=6;
 284       else if (diff < -3)
 285         diff +=6;
 286
 287       #ifndef BRUSHLESS_0_INVERT
 288         diff *= -1;    // inversion of the angle reading
 289       #endif
 290
 291       // update of the absolute angle with the delta
 292       //IRQ_LOCK(); // not necessary coz we enable ints only after
 293       g_brushless_0.position -= (brushless_position)diff * BRUSHLESS_POSITION_PRECISION;
 294       //IRQ_UNLOCK();
 295
 296       }
 297
 298     /*********** END Motor 0 angle update  **********/
 299     }
 300
 301
 302   if(interrupt_pwm ==0)
 303     {
 304     interrupt_pwm =1;
 305
 306     sei();
 307
 308     /*********** Motor 0 PWM update  **********/
 309
 310     // frequency division state machine
 311     if (--pwm_update_timer_0 == 0)
 312       {
 313       uint8_t flags;
 314       IRQ_LOCK(flags);
 315       pwm_update_timer_0 = g_brushless_0_pwm_divider; // protected against glitches
 316       IRQ_UNLOCK(flags);
 317       // end of frequency division state machine
 318
 319       #ifndef LOADTEST
 320       // skipping if same as last time
 321       if(sensors_0 != pwm_previous_sensors_0)
 322       #endif
 323         {
 324         brushless_torque torque;
 325
 326         pwm_previous_sensors_0 = sensors_0;
 327
 328
 329         IRQ_LOCK(flags);
 330         torque = g_brushless_0_torque;
 331         IRQ_UNLOCK(flags);
 332
 333
 334
 335
 336         BRUSHLESS_0_PWM_SET_1(
 337                               PWM_MAX/2 +                         // offset 50%
 338                               ((int8_t)pgm_read_byte(g_brushless_phase1 + sensors_0)) * // conversion from sensors
 339                               torque /*>> 1*/ );      // torque must be divided by 2. this is now done in the acess function
 340         BRUSHLESS_0_PWM_SET_2(
 341                               PWM_MAX/2 +
 342                               ((int8_t)pgm_read_byte(g_brushless_phase2 + sensors_0)) *
 343                               torque /*>> 1*/ );
 344         BRUSHLESS_0_PWM_SET_3(
 345                               PWM_MAX/2 +
 346                               ((int8_t)pgm_read_byte(g_brushless_phase3 + sensors_0)) *
 347                               torque /*>> 1*/ );
 348         }
 349       }
 350
 351
 352     /*********** END Motor 0 PWM update  **********/
 353
 354     interrupt_pwm =0;
 355     }
 356
 357   /** speed update part */
 358
 359 #ifndef BRUSHLESS_MANAGE_EXTERNAL
 360   // speed update variables
 361   static uint16_t speed_division_timer = 1; // only one needed
 362   // frequency division state machine
 363   if (--speed_division_timer == 0)
 364     {
 365
 366     speed_division_timer = BRUSHLESS_SAMPLE_TO_EVENT_DIVISOR;
 367     // end of frequency division state machine
 368
 369     brushless_speed_update_manage((void *)0);
 370
 371     }
 372 #endif
 373
 374 }
 375
 376
 377 void brushless_speed_update_manage(void * dummy)
 378 {
 379
 380   uint8_t flags;
 381
 382   // speed calculation, protected against glitches
 383   IRQ_LOCK(flags);
 384   g_brushless_0.speed = g_brushless_0.position - g_brushless_0_position_previous;
 385   g_brushless_0_position_previous = g_brushless_0.position;
 386   IRQ_UNLOCK(flags);
 387
 388
 389   // event call, with no imbrication autorized !
 390     {
 391     void (*f)(brushless);
 392     static volatile uint8_t in_progress = 0;
 393
 394     if(in_progress ==0)
 395       {
 396       in_progress = 1;
 397
 398       IRQ_LOCK(flags);
 399       f = periodic_event_0;
 400       IRQ_UNLOCK(flags);
 401
 402       if(f)
 403         f(g_brushless_0);
 404
 405
 406       in_progress = 0;
 407       }
 408     }
 409 }
 410
 411
 412 brushless_speed speed_mem_0   = BRUSHLESS_MAX_SPEED;
 413 brushless_torque torque_mem_0 = 0;
 414
 415
 416 /** initialisation, also executes pwm_init */
 417 void brushless_init(void)
 418 {
 419
 420         pwm_init();
 421
 422         // pull up resistors enable, if feature enabled
 423 #ifdef BRUSHLESS_0_SENSORS_PULL_UP_RESISTORS
 424         sbi(BRUSHLESS_0_SENSOR_1_PORT,BRUSHLESS_0_SENSOR_1_BIT);
 425         sbi(BRUSHLESS_0_SENSOR_2_PORT,BRUSHLESS_0_SENSOR_2_BIT);
 426         sbi(BRUSHLESS_0_SENSOR_3_PORT,BRUSHLESS_0_SENSOR_3_BIT);
 427 #endif
 428
 429
 430         INIT_INT();
 431
 432 }
 433
 434
 435
 436
 437 /******** set parameters, two times *******************/
 438 void brushless_0_set_parameters(brushless_speed speed, brushless_torque torque)
 439 {
 440         uint16_t pwm_divider = 0;
 441         uint8_t flags;
 442
 443
 444         // memory of settings
 445         speed_mem_0 = speed;
 446         torque_mem_0 = torque;
 447
 448
 449         if(speed ==0)
 450                 torque = 0;
 451         else
 452                 pwm_divider =  BRUSHLESS_MAX_SPEED / speed ;
 453
 454         if (pwm_divider ==0)
 455                 pwm_divider =1;
 456
 457         torque /= 2; // division is made here instead of in int function
 458
 459         // inversion
 460 #ifdef BRUSHLESS_0_INVERT
 461         torque *= -1;
 462 #endif
 463
 464         IRQ_LOCK(flags);
 465         g_brushless_0_pwm_divider  = pwm_divider;
 466         g_brushless_0_torque = torque;
 467         IRQ_UNLOCK(flags);
 468
 469         pwm_previous_sensors_0 = 0; // force application of the torque
 470 }
 471
 472
 473
 474 /******** get current speed and position, two times *******************/
 475 brushless brushless_0_get_mesures(void)
 476 {
 477         brushless ret;
 478         uint8_t flags;
 479
 480         IRQ_LOCK(flags);
 481         ret = g_brushless_0;
 482         IRQ_UNLOCK(flags);
 483
 484         return ret;
 485 }
 486
 487
 488 /******** set the position counter, two times *******************/
 489 void brushless_0_set_position(brushless_position p)
 490 {
 491         uint8_t flags;
 492         IRQ_LOCK(flags);
 493         g_brushless_0_position_previous += (p - g_brushless_0.position); // avoids speed glitches  on pos change
 494         g_brushless_0.position = p;
 495         IRQ_UNLOCK(flags);
 496 }
 497
 498
 499
 500 void brushless_0_register_periodic_event(void (*f)(brushless))
 501 {
 502         uint8_t flags;
 503         IRQ_LOCK(flags);
 504         periodic_event_0 = f;
 505         IRQ_UNLOCK(flags);
 506 }
 507
 508
 509
 510 /** acess functions for the control system interface */
 511
 512
 513 /** get speed function, compatible with control_system. Argument not used. */
 514 int32_t brushless_get_speed(void * motor_num)
 515 {
 516         brushless retour;
 517
 518         retour = brushless_0_get_mesures();
 519
 520         return (int32_t)(retour.speed);
 521 }
 522
 523 /** get position function, compatible with control_system. Argument not used. */
 524 int32_t brushless_get_pos(void * motor_num)
 525 {
 526         brushless retour;
 527
 528         retour = brushless_0_get_mesures();
 529
 530         return (int32_t)(retour.position);
 531 }
 532
 533 /** set torque function, compatible with control_system. first argument not used. */
 534 void brushless_set_torque(void * motor_num, int32_t torque)
 535 {
 536         brushless_0_set_parameters(speed_mem_0, (brushless_torque) torque);
 537 }
 538
 539 /** set speed function, compatible with control_system. first argument not used. */
 540 void brushless_set_speed(void * motor_num, int32_t speed)
 541 {
 542         brushless_0_set_parameters((brushless_speed) speed, torque_mem_0);
 543 }