00001
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043 #include "config.h"
00044 #include "mc_drv.h"
00045
00046
00047
00048
00049
00050
00051
00052 #ifdef __ICCAVR__
00053 code U8
00054 #elif __GNUC__
00055 U8
00056 #endif
00057
00058 ADMUXTableForward[6] =
00059 {
00060 ADMUX_V,
00061 ADMUX_U,
00062 ADMUX_W,
00063 ADMUX_V,
00064 ADMUX_U,
00065 ADMUX_W
00066 };
00067
00068
00069 #ifdef __ICCAVR__
00070 code U8
00071 #elif __GNUC__
00072 U8
00073 #endif
00074
00075 ADMUXTableReverse[6] =
00076 {
00077 ADMUX_V,
00078 ADMUX_W,
00079 ADMUX_U,
00080 ADMUX_V,
00081 ADMUX_W,
00082 ADMUX_U
00083 };
00084
00085
00086 #ifdef __ICCAVR__
00087 code U8
00088 #elif __GNUC__
00089 U8
00090 #endif
00091
00092 commTableForward[6] = {
00093 DRIVE_PATTERN_STEP2_CW,
00094 DRIVE_PATTERN_STEP1_CW,
00095 DRIVE_PATTERN_STEP6_CW,
00096 DRIVE_PATTERN_STEP5_CW,
00097 DRIVE_PATTERN_STEP4_CW,
00098 DRIVE_PATTERN_STEP3_CW
00099 };
00100
00101
00102
00103 #ifdef __ICCAVR__
00104 code U8
00105 #elif __GNUC__
00106 U8
00107 #endif
00108
00109 commTableReverse[6] = {
00110 DRIVE_PATTERN_STEP5_CCW,
00111 DRIVE_PATTERN_STEP4_CCW,
00112 DRIVE_PATTERN_STEP3_CCW,
00113 DRIVE_PATTERN_STEP2_CCW,
00114 DRIVE_PATTERN_STEP1_CCW,
00115 DRIVE_PATTERN_STEP6_CCW
00116 };
00117
00118
00119 #ifdef __ICCAVR__
00120 code U8
00121 #elif __GNUC__
00122 U8
00123 #endif
00124
00125 zcTableForward[6] = {
00126 EDGE_RISING,
00127 EDGE_FALLING,
00128 EDGE_RISING,
00129 EDGE_FALLING,
00130 EDGE_RISING,
00131 EDGE_FALLING
00132 };
00133
00134
00135
00136 #ifdef __ICCAVR__
00137 code U8
00138 #elif __GNUC__
00139 U8
00140 #endif
00141
00142 zcTableReverse[6] = {
00143 EDGE_FALLING,
00144 EDGE_RISING,
00145 EDGE_FALLING,
00146 EDGE_RISING,
00147 EDGE_FALLING,
00148 EDGE_RISING
00149 };
00156 volatile U8 nextDrivePattern;
00157
00163 volatile U8 zcPolarity ;
00164
00171 volatile U8 nextCommutationStep ;
00172
00174 volatile U16 speedReferenceADC;
00175
00177 volatile U8 speedUpdated = FALSE;
00178
00179 void Timer0Init(void)
00180 {
00181
00182 TCCR0B = ((0<<CS02)|(1<<CS01) |(0<<CS00));
00183 }
00184
00185 void PLLInit(void)
00186 {
00187
00188 PLLCSR = (1 << PCKE);
00189 }
00190
00191
00192 void PWMInit(void)
00193 {
00194
00195
00196 TCCR1A = (1 << COM1A1) | (0 << COM1A0) | (1 << PWM1A);
00197
00198
00199 TCCR1D = (1 << WGM11) | (1 << WGM10);
00200
00201
00202 TC1_WRITE_10_BIT_REGISTER(OCR1C, PWM_TOP_VALUE);
00203
00204
00205 TCCR1B = (1 << CS10);
00206
00207 }
00208
00209
00210 void ADCInit(void)
00211 {
00212 ADMUX = ADMUX_REF_VOLTAGE ;
00213
00214 ADCSRA = (1 << ADEN) | (1 << ADSC) | (1 << ADIF) | (ADC_PRESCALER_16);
00215 while (ADCSRA & (1 << ADSC))
00216 {
00217
00218 }
00219 speedReferenceADC = ADC;
00220
00221
00222 ADCSRA = (1 << ADEN) | (0 << ADSC) | (1 << ADATE) | (1 << ADIF) | (0 << ADIE) | ADC_PRESCALER_16;
00223 ADCSRB = ADC_TRIGGER_SOURCE;
00224 }
00225
00226
00227 void PortsInit(void)
00228 {
00229
00230 DDRB = (1 << PB0) | (1 << PB1) | (1 << PB2) | (1 << PB3) | (1 << PB4) | (1 << PB5)| (1 << PB6) ;
00231
00232
00233 DIDR0 = (1 << ADC1D) | (1 << ADC2D) | (1 << ADC3D) | (1 << ADC4D) ;
00234 }
00235
00238 void mc_init_HW(void)
00239 {
00240 Clear_prescaler();
00241 PLLInit();
00242 PWMInit();
00243 ADCInit();
00244
00245
00246 Timer0Init();
00247
00248
00249 PortsInit();
00250
00251 }
00252
00253 #ifndef __GNUC__
00254 #pragma optimize=none
00255 void Set_cpu_prescaler(U8 x)
00256 {
00257 U8 save_int=SREG&0x80;
00258 Disable_interrupt();
00259 CLKPR=(1<<CLKPCE);
00260 CLKPR=x;
00261 if(save_int) { Enable_interrupt(); }
00262 }
00263 #endif
00264
00276 #ifdef __GNUC__
00277 ISR(TIMER1_OVF_vect)
00278 #else
00279 #pragma vector = TIM1_OVF_vect
00280 __interrupt void MotorPWMBottom()
00281 #endif
00282 {
00283
00284 U16 temp;
00285
00286 CLEAR_ALL_TIMER1_INT_FLAGS;
00287
00288
00289 ADCSRA = (1 << ADEN) | (1 << ADSC) | (1 << ADIF) | (ADC_PRESCALER_16);
00290 while (ADCSRA & (1 << ADSC))
00291 {
00292 }
00293 temp= ADC;
00294
00295 if (((zcPolarity == EDGE_RISING) && (temp > ADC_ZC_THRESHOLD)) || ((zcPolarity == EDGE_FALLING) && (temp < ADC_ZC_THRESHOLD)))
00296 {
00297 U16 timeSinceCommutation;
00298
00299
00300 TC0_READ_TCNT0(timeSinceCommutation);
00301
00302
00303 TC0_WRITE_TCNT0(0);
00304
00305 OCR0A = timeSinceCommutation;
00306
00307 speedUpdated = TRUE;
00308
00309 SET_TIMER0_INT_COMMUTATION;
00310 CLEAR_ALL_TIMER0_INT_FLAGS;
00311
00312
00313 DISABLE_ALL_TIMER1_INTS;
00314
00315
00316
00317
00318 while (ADCSRA & (1 << ADSC))
00319 {
00320
00321 }
00322
00323 ADMUX = ADMUX_SPEED_REF;
00324
00325
00326 ADCSRA |= (1 << ADSC);
00327
00328
00329 while((ADCSRA & (1 << ADSC)))
00330 {
00331
00332 }
00333 speedReferenceADC = ADC;
00334
00335 ADCSRA |= (1 << ADATE) | (0 << ADIE) | ADC_PRESCALER;
00336 }
00337
00338 }
00339
00340
00353 #ifdef __GNUC__
00354 ISR(TIMER0_COMPA_vect)
00355 #else
00356 #pragma vector = TIM0_COMPA_vect
00357 __interrupt void Commutate()
00358 #endif
00359 {
00360 CLEAR_ALL_TIMER0_INT_FLAGS;
00361
00362 #if (DIRECTION_OF_ROTATION == CCW)
00363
00364 TCCR1E = commTableReverse[nextCommutationStep];
00365 #else
00366
00367 TCCR1E = commTableForward[nextCommutationStep];
00368 #endif
00369
00370
00371 TC0_WRITE_TCNT0(0);
00372
00373 #if (DIRECTION_OF_ROTATION == CCW)
00374 zcPolarity = zcTableReverse[nextCommutationStep];
00375 ADMUX = ADMUXTableReverse[nextCommutationStep];
00376 #else
00377 zcPolarity = zcTableForward[nextCommutationStep];
00378 ADMUX = ADMUXTableForward[nextCommutationStep];
00379 #endif
00380
00381 nextCommutationStep ++;
00382
00383 if (nextCommutationStep > 5)
00384 {
00385 nextCommutationStep = 0;
00386 }
00387
00388 DISABLE_ALL_TIMER0_INTS;
00389 SET_TIMER1_INT_ZC_DETECTION;
00390
00391 }