00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00052 #include <avr/io.h>
00053 #include <avr/signal.h>
00054
00055
00056 #include <pwm.h>
00057
00058 #include <utils.h>
00059
00060
00061 #include <avr/pgmspace.h>
00062
00063 #include <brushless.h>
00064
00065 #include "brushless_3phase_digital_hall_config.h"
00066
00067 #if (BRUSHLESS_TYPE != BRUSHLESS_DIGITAL)
00068 #error mauvais fichier de config "brushless_config.h"
00069 #endif
00070
00071
00072
00073
00077 #if (BRUSHLESS_TIMER == 0)
00078 #define INIT_INT() sbi(TIMSK, TOIE0)
00079 #define INT SIG_OVERFLOW0
00080
00081 #elif (BRUSHLESS_TIMER == 1)
00082 #define INIT_INT() sbi(TIMSK, TOIE1)
00083 #define INT SIG_OVERFLOW1
00084
00085 #elif (BRUSHLESS_TIMER == 2)
00086 #define INIT_INT() sbi(TIMSK, TOIE2)
00087 #define INT SIG_OVERFLOW2
00088
00089 #elif (BRUSHLESS_TIMER == 3)
00090 #define INIT_INT() sbi(ETIMSK, TOIE3)
00091 #define INT SIG_OVERFLOW3
00092
00093 #endif
00094
00095
00105
00106
00107
00108 const int8_t g_brushless_angle[]= {0, 1, 5, 0, 3, 2, 4, 0};
00109
00110
00111 const int8_t PROGMEM g_brushless_phase1[]= {0, 1, -1, 0, 0, 1, -1, 0};
00112 const int8_t PROGMEM g_brushless_phase2[]= {0, 0, 1, 1, -1, -1, 0, 0};
00113 const int8_t PROGMEM g_brushless_phase3[]= {0, -1, 0, -1, 1, 0, 1, 0};
00114
00115
00116
00117
00118
00119 void brushless_speed_update_manage(void * dummy);
00120
00121
00122 brushless_position g_brushless_0_position_previous;
00123
00124
00125
00126 brushless g_brushless_0;
00127
00128
00129
00130 brushless_torque g_brushless_0_torque;
00131
00132
00133
00134 uint16_t g_brushless_0_pwm_divider = 1;
00135
00136
00137
00138
00139 void (*periodic_event_0)(brushless) = 0;
00140
00141
00142
00167 volatile uint8_t pwm_division_timer asm("pwm_div") = 1;
00168 volatile uint8_t interrupt_pwm;
00169 volatile uint8_t pwm_previous_sensors_0 = 0;
00170
00171
00172
00173 #ifdef ASMHEADER
00174
00179 void INT (void) __attribute__ ((naked));
00180 SIGNAL(INT)
00181 {
00182 asm volatile(
00183
00184
00185 "PUSH R1 \n\t"
00186 "IN R1,0x3F \n\t"
00187 "PUSH R1 \n\t"
00188 "LDS R1,pwm_div \n\t"
00189 "DEC R1 \n\t"
00190 "BREQ continue \n\t"
00191 "STS pwm_div,R1 \n\t"
00192
00193
00194
00195 "go_out: \n\t"
00196 "POP R1 \n\t"
00197 "OUT 0x3F,R1 \n\t"
00198 "POP R1 \n\t"
00199 "RETI \n\t"
00200
00201
00202 "continue: \n\t"
00203 "POP R1 \n\t"
00204 "OUT 0x3F,R1 \n\t"
00205 "POP R1 \n\t"
00206
00207 ::);
00208
00209 }
00210 SIGNAL(DUMMY)
00211 {
00212
00213 #else // the same code in C
00214 SIGNAL(INT)
00215 {
00217 if (--pwm_division_timer != 0)
00218 return;
00219
00220
00221 #endif //common code
00222
00223
00224 pwm_division_timer = BRUSHLESS_PWM_TO_SAMPLE_DIVISOR;
00225
00226
00227
00228
00231
00232 static int8_t angle_electrical_previous_0 = 0;
00233
00234 static uint8_t angle_previous_sensors_0 = 0;
00235
00236 int8_t angle_electrical, diff;
00237 uint8_t sensors_0 = 0;
00238
00239
00240
00241 static uint16_t pwm_update_timer_0 = 1;
00242
00243
00244
00245 {
00246
00247
00248
00253 if(bit_is_set(PIN(BRUSHLESS_0_SENSOR_1_PORT),BRUSHLESS_0_SENSOR_1_BIT))
00254 sensors_0 +=1;
00255 if(bit_is_set(PIN(BRUSHLESS_0_SENSOR_2_PORT),BRUSHLESS_0_SENSOR_2_BIT))
00256 sensors_0 +=2;
00257 if(bit_is_set(PIN(BRUSHLESS_0_SENSOR_3_PORT),BRUSHLESS_0_SENSOR_3_BIT))
00258 sensors_0 +=4;
00259
00260 #ifdef BRUSHLESS_0_SENSORS_INVERT
00261 sensors_0 = (~ sensors_0) & 0x7;
00262 #endif
00263
00264
00265
00268 #ifndef LOADTEST
00269
00270 if(sensors_0 != angle_previous_sensors_0)
00271 #endif
00272 {
00273 angle_previous_sensors_0 = sensors_0;
00274
00275
00276 angle_electrical = g_brushless_angle[sensors_0];
00277
00278 diff = angle_electrical - angle_electrical_previous_0;
00279 angle_electrical_previous_0 = angle_electrical;
00280
00281
00282 if (diff > 3)
00283 diff -=6;
00284 else if (diff < -3)
00285 diff +=6;
00286
00287 #ifndef BRUSHLESS_0_INVERT
00288 diff *= -1;
00289 #endif
00290
00291
00292
00293 g_brushless_0.position -= (brushless_position)diff * BRUSHLESS_POSITION_PRECISION;
00294
00295
00296 }
00297
00298
00299 }
00300
00301
00302 if(interrupt_pwm ==0)
00303 {
00304 interrupt_pwm =1;
00305
00306 sei();
00307
00308
00309
00310
00311 if (--pwm_update_timer_0 == 0)
00312 {
00313 uint8_t flags;
00314 IRQ_LOCK(flags);
00315 pwm_update_timer_0 = g_brushless_0_pwm_divider;
00316 IRQ_UNLOCK(flags);
00317
00318
00319 #ifndef LOADTEST
00320
00321 if(sensors_0 != pwm_previous_sensors_0)
00322 #endif
00323 {
00324 brushless_torque torque;
00325
00326 pwm_previous_sensors_0 = sensors_0;
00327
00328
00329 IRQ_LOCK(flags);
00330 torque = g_brushless_0_torque;
00331 IRQ_UNLOCK(flags);
00332
00333
00334
00335
00336 BRUSHLESS_0_PWM_SET_1(
00337 PWM_MAX/2 +
00338 ((int8_t)pgm_read_byte(g_brushless_phase1 + sensors_0)) *
00339 torque );
00340 BRUSHLESS_0_PWM_SET_2(
00341 PWM_MAX/2 +
00342 ((int8_t)pgm_read_byte(g_brushless_phase2 + sensors_0)) *
00343 torque );
00344 BRUSHLESS_0_PWM_SET_3(
00345 PWM_MAX/2 +
00346 ((int8_t)pgm_read_byte(g_brushless_phase3 + sensors_0)) *
00347 torque );
00348 }
00349 }
00350
00351
00352
00353
00354 interrupt_pwm =0;
00355 }
00356
00359 #ifndef BRUSHLESS_MANAGE_EXTERNAL
00360
00361 static uint16_t speed_division_timer = 1;
00362
00363 if (--speed_division_timer == 0)
00364 {
00365
00366 speed_division_timer = BRUSHLESS_SAMPLE_TO_EVENT_DIVISOR;
00367
00368
00369 brushless_speed_update_manage((void *)0);
00370
00371 }
00372 #endif
00373
00374 }
00375
00376
00377 void brushless_speed_update_manage(void * dummy)
00378 {
00379
00380 uint8_t flags;
00381
00382
00383 IRQ_LOCK(flags);
00384 g_brushless_0.speed = g_brushless_0.position - g_brushless_0_position_previous;
00385 g_brushless_0_position_previous = g_brushless_0.position;
00386 IRQ_UNLOCK(flags);
00387
00388
00389
00390 {
00391 void (*f)(brushless);
00392 static volatile uint8_t in_progress = 0;
00393
00394 if(in_progress ==0)
00395 {
00396 in_progress = 1;
00397
00398 IRQ_LOCK(flags);
00399 f = periodic_event_0;
00400 IRQ_UNLOCK(flags);
00401
00402 if(f)
00403 f(g_brushless_0);
00404
00405
00406 in_progress = 0;
00407 }
00408 }
00409 }
00410
00411
00412 brushless_speed speed_mem_0 = BRUSHLESS_MAX_SPEED;
00413 brushless_torque torque_mem_0 = 0;
00414
00415
00417 void brushless_init(void)
00418 {
00419
00420 pwm_init();
00421
00422
00423 #ifdef BRUSHLESS_0_SENSORS_PULL_UP_RESISTORS
00424 sbi(BRUSHLESS_0_SENSOR_1_PORT,BRUSHLESS_0_SENSOR_1_BIT);
00425 sbi(BRUSHLESS_0_SENSOR_2_PORT,BRUSHLESS_0_SENSOR_2_BIT);
00426 sbi(BRUSHLESS_0_SENSOR_3_PORT,BRUSHLESS_0_SENSOR_3_BIT);
00427 #endif
00428
00429
00430 INIT_INT();
00431
00432 }
00433
00434
00435
00436
00437
00438 void brushless_0_set_parameters(brushless_speed speed, brushless_torque torque)
00439 {
00440 uint16_t pwm_divider = 0;
00441 uint8_t flags;
00442
00443
00444
00445 speed_mem_0 = speed;
00446 torque_mem_0 = torque;
00447
00448
00449 if(speed ==0)
00450 torque = 0;
00451 else
00452 pwm_divider = BRUSHLESS_MAX_SPEED / speed ;
00453
00454 if (pwm_divider ==0)
00455 pwm_divider =1;
00456
00457 torque /= 2;
00458
00459
00460 #ifdef BRUSHLESS_0_INVERT
00461 torque *= -1;
00462 #endif
00463
00464 IRQ_LOCK(flags);
00465 g_brushless_0_pwm_divider = pwm_divider;
00466 g_brushless_0_torque = torque;
00467 IRQ_UNLOCK(flags);
00468
00469 pwm_previous_sensors_0 = 0;
00470 }
00471
00472
00473
00474
00475 brushless brushless_0_get_mesures(void)
00476 {
00477 brushless ret;
00478 uint8_t flags;
00479
00480 IRQ_LOCK(flags);
00481 ret = g_brushless_0;
00482 IRQ_UNLOCK(flags);
00483
00484 return ret;
00485 }
00486
00487
00488
00489 void brushless_0_set_position(brushless_position p)
00490 {
00491 uint8_t flags;
00492 IRQ_LOCK(flags);
00493 g_brushless_0_position_previous += (p - g_brushless_0.position);
00494 g_brushless_0.position = p;
00495 IRQ_UNLOCK(flags);
00496 }
00497
00498
00499
00500 void brushless_0_register_periodic_event(void (*f)(brushless))
00501 {
00502 uint8_t flags;
00503 IRQ_LOCK(flags);
00504 periodic_event_0 = f;
00505 IRQ_UNLOCK(flags);
00506 }
00507
00508
00509
00514 int32_t brushless_get_speed(void * motor_num)
00515 {
00516 brushless retour;
00517
00518 retour = brushless_0_get_mesures();
00519
00520 return (int32_t)(retour.speed);
00521 }
00522
00524 int32_t brushless_get_pos(void * motor_num)
00525 {
00526 brushless retour;
00527
00528 retour = brushless_0_get_mesures();
00529
00530 return (int32_t)(retour.position);
00531 }
00532
00534 void brushless_set_torque(void * motor_num, int32_t torque)
00535 {
00536 brushless_0_set_parameters(speed_mem_0, (brushless_torque) torque);
00537 }
00538
00540 void brushless_set_speed(void * motor_num, int32_t speed)
00541 {
00542 brushless_0_set_parameters((brushless_speed) speed, torque_mem_0);
00543 }