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_config.h"
67 #if (BRUSHLESS_TYPE != BRUSHLESS_DIGITAL)
68 #error mauvais fichier de config "brushless_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 !
119 void brushless_speed_update_manage(void * dummy);
121 // mem of previous position for speed calc.
122 brushless_position g_brushless_0_position_previous;
125 // memory for current measurements
126 brushless g_brushless_0;
129 // pseudo torque to use
130 brushless_torque g_brushless_0_torque;
133 // given with speed limit
134 uint16_t g_brushless_0_pwm_divider = 1;
138 // function pointer definition for event
139 void (*periodic_event_0)(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
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 !!)
179 void INT (void) __attribute__ ((naked)); // no register save, and no exit !!
184 /* division, done very early, for optimization */
188 "LDS R1,pwm_div \n\t"
191 "STS pwm_div,R1 \n\t" /*we store only if negative. if positive, it will be done in the C code*/
194 /* early go out of int */
201 /* restoring context, double work with the following interrupt function, but no other way */
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)
213 #else // the same code in C
216 /** PWM division part */
217 if (--pwm_division_timer != 0)
224 pwm_division_timer = BRUSHLESS_PWM_TO_SAMPLE_DIVISOR;
225 // end of frequency division state machine
229 /** various definitions */
231 // angle update variables
232 static int8_t angle_electrical_previous_0 = 0; // previous electrical angle
234 static uint8_t angle_previous_sensors_0 = 0; // previous state of sensors for angle calc
236 int8_t angle_electrical, diff;
237 uint8_t sensors_0 = 0;
240 // pwm update variables
241 static uint16_t pwm_update_timer_0 = 1;
246 /*********** Motor 0 angle update **********/
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 */
253 if(bit_is_set(PIN(BRUSHLESS_0_SENSOR_1_PORT),BRUSHLESS_0_SENSOR_1_BIT))
255 if(bit_is_set(PIN(BRUSHLESS_0_SENSOR_2_PORT),BRUSHLESS_0_SENSOR_2_BIT))
257 if(bit_is_set(PIN(BRUSHLESS_0_SENSOR_3_PORT),BRUSHLESS_0_SENSOR_3_BIT))
260 #ifdef BRUSHLESS_0_SENSORS_INVERT
261 sensors_0 = (~ sensors_0) & 0x7; // inversion of the sensors
266 /** the angle update part */
269 // skipping if no change
270 if(sensors_0 != angle_previous_sensors_0)
273 angle_previous_sensors_0 = sensors_0;
276 angle_electrical = g_brushless_angle[sensors_0]; // calculate electrical angle
278 diff = angle_electrical - angle_electrical_previous_0; // convert to angle delta
279 angle_electrical_previous_0 = angle_electrical; // store for next time
287 #ifndef BRUSHLESS_0_INVERT
288 diff *= -1; // inversion of the angle reading
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;
298 /*********** END Motor 0 angle update **********/
302 if(interrupt_pwm ==0)
308 /*********** Motor 0 PWM update **********/
310 // frequency division state machine
311 if (--pwm_update_timer_0 == 0)
315 pwm_update_timer_0 = g_brushless_0_pwm_divider; // protected against glitches
317 // end of frequency division state machine
320 // skipping if same as last time
321 if(sensors_0 != pwm_previous_sensors_0)
324 brushless_torque torque;
326 pwm_previous_sensors_0 = sensors_0;
330 torque = g_brushless_0_torque;
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(
342 ((int8_t)pgm_read_byte(g_brushless_phase2 + sensors_0)) *
344 BRUSHLESS_0_PWM_SET_3(
346 ((int8_t)pgm_read_byte(g_brushless_phase3 + sensors_0)) *
352 /*********** END Motor 0 PWM update **********/
357 /** speed update part */
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)
366 speed_division_timer = BRUSHLESS_SAMPLE_TO_EVENT_DIVISOR;
367 // end of frequency division state machine
369 brushless_speed_update_manage((void *)0);
377 void brushless_speed_update_manage(void * dummy)
382 // speed calculation, protected against glitches
384 g_brushless_0.speed = g_brushless_0.position - g_brushless_0_position_previous;
385 g_brushless_0_position_previous = g_brushless_0.position;
389 // event call, with no imbrication autorized !
391 void (*f)(brushless);
392 static volatile uint8_t in_progress = 0;
399 f = periodic_event_0;
412 brushless_speed speed_mem_0 = BRUSHLESS_MAX_SPEED;
413 brushless_torque torque_mem_0 = 0;
416 /** initialisation, also executes pwm_init */
417 void brushless_init(void)
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);
437 /******** set parameters, two times *******************/
438 void brushless_0_set_parameters(brushless_speed speed, brushless_torque torque)
440 uint16_t pwm_divider = 0;
444 // memory of settings
446 torque_mem_0 = torque;
452 pwm_divider = BRUSHLESS_MAX_SPEED / speed ;
457 torque /= 2; // division is made here instead of in int function
460 #ifdef BRUSHLESS_0_INVERT
465 g_brushless_0_pwm_divider = pwm_divider;
466 g_brushless_0_torque = torque;
469 pwm_previous_sensors_0 = 0; // force application of the torque
474 /******** get current speed and position, two times *******************/
475 brushless brushless_0_get_mesures(void)
488 /******** set the position counter, two times *******************/
489 void brushless_0_set_position(brushless_position p)
493 g_brushless_0_position_previous += (p - g_brushless_0.position); // avoids speed glitches on pos change
494 g_brushless_0.position = p;
500 void brushless_0_register_periodic_event(void (*f)(brushless))
504 periodic_event_0 = f;
510 /** acess functions for the control system interface */
513 /** get speed function, compatible with control_system. Argument not used. */
514 int32_t brushless_get_speed(void * motor_num)
518 retour = brushless_0_get_mesures();
520 return (int32_t)(retour.speed);
523 /** get position function, compatible with control_system. Argument not used. */
524 int32_t brushless_get_pos(void * motor_num)
528 retour = brushless_0_get_mesures();
530 return (int32_t)(retour.position);
533 /** set torque function, compatible with control_system. first argument not used. */
534 void brushless_set_torque(void * motor_num, int32_t torque)
536 brushless_0_set_parameters(speed_mem_0, (brushless_torque) torque);
539 /** set speed function, compatible with control_system. first argument not used. */
540 void brushless_set_speed(void * motor_num, int32_t speed)
542 brushless_0_set_parameters((brushless_speed) speed, torque_mem_0);