2 * Copyright Droids Corporation, Microb Technology, Eirbot (2005)
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.
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.
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
18 * Revision : $Id: brushless.c,v 1.2.2.3 2007-05-23 17:18:12 zer0 Exp $
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 !!
27 The control value is a voltage. This can be assimiled to a torque at low speeds.
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.
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.
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 !)
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.*/
53 #include <avr/signal.h>
61 #include <avr/pgmspace.h>
63 #include <brushless.h>
65 #include "brushless_3phase_digital_hall_double_config.h"
67 #if (BRUSHLESS_TYPE != BRUSHLESS_DIGITAL_DOUBLE)
68 #error mauvais fichier de config "brushless_double_config.h"
74 /** calculating the event to use, based on PWM definition of the phase 1
75 The two motors function on the same timer! */
77 #if (BRUSHLESS_TIMER == 0)
78 #define INIT_INT() sbi(TIMSK, TOIE0)
79 #define INT SIG_OVERFLOW0
81 #elif (BRUSHLESS_TIMER == 1)
82 #define INIT_INT() sbi(TIMSK, TOIE1)
83 #define INT SIG_OVERFLOW1
85 #elif (BRUSHLESS_TIMER == 2)
86 #define INIT_INT() sbi(TIMSK, TOIE2)
87 #define INT SIG_OVERFLOW2
89 #elif (BRUSHLESS_TIMER == 3)
90 #define INIT_INT() sbi(ETIMSK, TOIE3) /* extended timsk for this one ! */
91 #define INT SIG_OVERFLOW3
97 sensors > relative electric angle
98 sensors > phase information
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 */
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};
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};
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 !
118 void brushless_speed_update_manage(void * dummy);
121 // mem of previous position for speed calc.
122 brushless_position g_brushless_0_position_previous;
123 brushless_position g_brushless_1_position_previous;
125 // memory for current measurements
126 brushless g_brushless_0;
127 brushless g_brushless_1;
129 // pseudo torque to use
130 brushless_torque g_brushless_0_torque;
131 brushless_torque g_brushless_1_torque;
133 // given with speed limit
134 uint16_t g_brushless_0_pwm_divider = 1;
135 uint16_t g_brushless_1_pwm_divider = 1;
138 // function pointer definition for event
139 void (*periodic_event_0)(brushless) = 0; //NULL;
140 void (*periodic_event_1)(brushless) = 0; //NULL;
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
151 Typically, PWM update is slower than angle update, in order to not skip angle counting.
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.
158 examples for a pretty fast motor :
160 - angle update at 10 kHz
161 - PWM update at 5 kHz or lower, depending on speed setting
162 - speed update at 100 Hz
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
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 !!)
180 void INT (void) __attribute__ ((naked)); // no register save, and no exit !!
185 /* division, done very early, for optimization */
189 "LDS R1,pwm_div \n\t"
192 "STS pwm_div,R1 \n\t" /*we store only if negative. if positive, it will be done in the C code*/
195 /* early go out of int */
202 /* restoring context, double work with the following interrupt function, but no other way */
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)
214 #else // the same code in C
217 /** PWM division part */
218 if (--pwm_division_timer != 0)
225 pwm_division_timer = BRUSHLESS_PWM_TO_SAMPLE_DIVISOR;
226 // end of frequency division state machine
230 /** various definitions */
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;
241 // pwm update variables
242 static uint16_t pwm_update_timer_0 = 1;
243 static uint16_t pwm_update_timer_1 = 1;
247 /*********** Motor 0 angle update **********/
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 */
254 if(bit_is_set(PIN(BRUSHLESS_0_SENSOR_1_PORT),BRUSHLESS_0_SENSOR_1_BIT))
256 if(bit_is_set(PIN(BRUSHLESS_0_SENSOR_2_PORT),BRUSHLESS_0_SENSOR_2_BIT))
258 if(bit_is_set(PIN(BRUSHLESS_0_SENSOR_3_PORT),BRUSHLESS_0_SENSOR_3_BIT))
261 #ifdef BRUSHLESS_0_SENSORS_INVERT
262 sensors_0 = (~ sensors_0) & 0x7; // inversion of the sensors
267 /** the angle update part */
270 // skipping if no change
271 if(sensors_0 != angle_previous_sensors_0)
274 angle_previous_sensors_0 = sensors_0;
277 angle_electrical = g_brushless_angle[sensors_0]; // calculate electrical angle
279 diff = angle_electrical - angle_electrical_previous_0; // convert to angle delta
280 angle_electrical_previous_0 = angle_electrical; // store for next time
288 #ifndef BRUSHLESS_0_INVERT
289 diff *= -1; // inversion of the angle reading
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;
299 /*********** END Motor 0 angle update **********/
301 /*********** Motor 1 angle update **********/
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 */
308 if(bit_is_set(PIN(BRUSHLESS_1_SENSOR_1_PORT),BRUSHLESS_1_SENSOR_1_BIT))
310 if(bit_is_set(PIN(BRUSHLESS_1_SENSOR_2_PORT),BRUSHLESS_1_SENSOR_2_BIT))
312 if(bit_is_set(PIN(BRUSHLESS_1_SENSOR_3_PORT),BRUSHLESS_1_SENSOR_3_BIT))
315 #ifdef BRUSHLESS_1_SENSORS_INVERT
316 sensors_1 = (~ sensors_1) & 0x7; // inversion of the sensors
321 /** the angle update part */
324 // skipping if no change
325 if(sensors_1 != angle_previous_sensors_1)
328 angle_previous_sensors_1 = sensors_1;
331 angle_electrical = g_brushless_angle[sensors_1]; // calculate electrical angle
333 diff = angle_electrical - angle_electrical_previous_1; // convert to angle delta
334 angle_electrical_previous_1 = angle_electrical; // store for next time
342 #ifndef BRUSHLESS_1_INVERT
343 diff *= -1; // inversion of the angle reading
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;
353 /*********** END Motor 1 angle update **********/
357 if(interrupt_pwm ==0)
363 /*********** Motor 0 PWM update **********/
365 // frequency division state machine
366 if (--pwm_update_timer_0 == 0)
370 pwm_update_timer_0 = g_brushless_0_pwm_divider; // protected against glitches
372 // end of frequency division state machine
375 // skipping if same as last time
376 if(sensors_0 != pwm_previous_sensors_0)
379 brushless_torque torque;
381 pwm_previous_sensors_0 = sensors_0;
385 torque = g_brushless_0_torque;
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(
397 ((int8_t)pgm_read_byte(g_brushless_phase2 + sensors_0)) *
399 BRUSHLESS_0_PWM_SET_3(
401 ((int8_t)pgm_read_byte(g_brushless_phase3 + sensors_0)) *
407 /*********** END Motor 0 PWM update **********/
410 /*********** Motor 1 PWM update **********/
412 // frequency division state machine
413 if (--pwm_update_timer_1 == 0)
417 pwm_update_timer_1 = g_brushless_1_pwm_divider; // protected against glitches
419 // end of frequency division state machine
422 // skipping if same as last time
423 if(sensors_1 != pwm_previous_sensors_1)
426 brushless_torque torque;
428 pwm_previous_sensors_1 = sensors_1;
432 torque = g_brushless_1_torque;
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(
444 ((int8_t)pgm_read_byte(g_brushless_phase2 + sensors_1)) *
446 BRUSHLESS_1_PWM_SET_3(
448 ((int8_t)pgm_read_byte(g_brushless_phase3 + sensors_1)) *
452 /*********** END Motor 1 PWM update **********/
456 /** speed update part */
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)
465 speed_division_timer = BRUSHLESS_SAMPLE_TO_EVENT_DIVISOR;
466 // end of frequency division state machine
468 brushless_speed_update_manage((void *)0);
475 void brushless_speed_update_manage(void * dummy)
480 // speed calculation, protected against glitches
482 g_brushless_0.speed = g_brushless_0.position - g_brushless_0_position_previous;
483 g_brushless_0_position_previous = g_brushless_0.position;
487 g_brushless_1.speed = g_brushless_1.position - g_brushless_1_position_previous;
488 g_brushless_1_position_previous = g_brushless_1.position;
491 // event call, with no imbrication autorized !
493 void (*f)(brushless);
494 static volatile uint8_t in_progress = 0;
496 if(in_progress ==0) {
500 f = periodic_event_0;
508 f = periodic_event_1;
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;
527 /** initialisation, also executes pwm_init */
528 void brushless_init(void)
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);
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);
554 /******** set parameters, two times *******************/
555 void brushless_0_set_parameters(brushless_speed speed, brushless_torque torque)
557 uint16_t pwm_divider = 0;
560 // memory of settings
562 torque_mem_0 = torque;
568 pwm_divider = BRUSHLESS_MAX_SPEED / speed ;
573 torque /= 2; // division is made here instead of in int function
576 #ifdef BRUSHLESS_0_INVERT
581 g_brushless_0_pwm_divider = pwm_divider;
582 g_brushless_0_torque = torque;
585 pwm_previous_sensors_0 = 0; // force application of the torque
588 void brushless_1_set_parameters(brushless_speed speed, brushless_torque torque)
590 uint16_t pwm_divider = 0;
593 // memory of settings
595 torque_mem_1 = torque;
601 pwm_divider = BRUSHLESS_MAX_SPEED / speed ;
606 torque /= 2; // division is made here instead of in int function
609 #ifdef BRUSHLESS_1_INVERT
614 g_brushless_1_pwm_divider = pwm_divider;
615 g_brushless_1_torque = torque;
618 pwm_previous_sensors_1 = 0; // force application of the torque
622 /******** get current speed and position, two times *******************/
623 brushless brushless_0_get_mesures(void)
634 brushless brushless_1_get_mesures(void)
647 /******** set the position counter, two times *******************/
648 void brushless_0_set_position(brushless_position p)
652 g_brushless_0_position_previous += p - g_brushless_0.position; // avoids speed glitches on pos change
653 g_brushless_0.position = p;
656 void brushless_1_set_position(brushless_position p)
660 g_brushless_1_position_previous += p - g_brushless_1.position; // avoids speed glitches on pos change
661 g_brushless_1.position = p;
666 void brushless_0_register_periodic_event(void (*f)(brushless))
670 periodic_event_0 = f;
673 void brushless_1_register_periodic_event(void (*f)(brushless))
677 periodic_event_1 = f;
683 /** acess functions for the control system interface */
686 /** get speed function, compatible with control_system. Argument not used. */
687 int32_t brushless_get_speed(void * motor_num)
692 retour = brushless_1_get_mesures();
694 retour = brushless_0_get_mesures();
696 return (int32_t)(retour.speed);
699 /** get position function, compatible with control_system. Argument not used. */
700 int32_t brushless_get_pos(void * motor_num)
705 retour = brushless_1_get_mesures();
707 retour = brushless_0_get_mesures();
709 return (int32_t)(retour.position);
712 /** set torque function, compatible with control_system. first argument not used. */
713 void brushless_set_torque(void * motor_num, int32_t torque)
716 brushless_1_set_parameters(speed_mem_1, (brushless_torque) torque);
718 brushless_0_set_parameters(speed_mem_0, (brushless_torque) torque);
721 /** set speed function, compatible with control_system. first argument not used. */
722 void brushless_set_speed(void * motor_num, int32_t speed)
725 brushless_1_set_parameters((brushless_speed) speed, torque_mem_1);
727 brushless_0_set_parameters((brushless_speed) speed, torque_mem_0);