00001 // This file has been prepared for Doxygen automatic documentation generation. 00023 #include "BLDC.h" 00024 00025 #include <ioavr.h> 00026 #include <inavr.h> 00027 00029 unsigned char driveTable[6]; 00030 00032 unsigned char ADMUXTable[6]; 00033 00035 unsigned int startupDelays[STARTUP_NUM_COMMUTATIONS]; 00036 00042 __regvar __no_init volatile unsigned int filteredTimeSinceCommutation @14; 00043 00050 __regvar __no_init volatile unsigned char nextDrivePattern @13; 00051 00057 __regvar __no_init volatile unsigned char zcPolarity @ 12; 00058 00065 __regvar __no_init volatile unsigned char nextCommutationStep @11; 00066 00068 volatile unsigned char speedReferenceADC; 00069 00071 volatile unsigned char shuntVoltageADC = 0; 00072 00074 volatile unsigned char referenceVoltageADC; 00075 00077 volatile unsigned char speedUpdated = FALSE; 00078 00080 volatile unsigned char currentUpdated = FALSE; 00081 00082 00088 void main(void) 00089 { 00090 // Initialize all sub-systems. 00091 ResetHandler(); 00092 InitPorts(); 00093 InitTimers(); 00094 InitADC(); 00095 MakeTables(); 00096 InitAnalogComparator(); 00097 00098 // Run startup procedure. 00099 StartMotor(); 00100 00101 // Turn on watchdog for stall-detection. 00102 WatchdogTimerEnable(); 00103 __enable_interrupt(); 00104 00105 for(;;) 00106 { 00107 PWMControl(); 00108 } 00109 } 00110 00111 00124 static void ResetHandler(void) 00125 { 00126 __eeprom unsigned static int restartAttempts; 00127 // Temporary variable to avoid unnecessary reading of volatile register MCUSR. 00128 unsigned char tempMCUSR; 00129 00130 tempMCUSR = MCUSR; 00131 MCUSR = tempMCUSR & ~((1 << WDRF) | (1 << BORF) | (1 << EXTRF) | (1 << PORF)); 00132 00133 // Reset watchdog to avoid false stall detection before the motor has started. 00134 __disable_interrupt(); 00135 __watchdog_reset(); 00136 WDTCSR |= (1 << WDCE) | (1 << WDE); 00137 WDTCSR = 0x00; 00138 00139 // Examine the reset source and take action. 00140 switch (tempMCUSR & ((1 << WDRF) | (1 << BORF) | (1 << EXTRF) | (1 << PORF))) 00141 { 00142 case (1 << WDRF): 00143 restartAttempts++; 00144 if (restartAttempts >= MAX_RESTART_ATTEMPTS) 00145 { 00146 // Do something here. E.g. wait for a button to be pressed. 00147 for (;;) 00148 { 00149 00150 } 00151 } 00152 00153 // Put watchdog reset handler here. 00154 break; 00155 case (1 << BORF): 00156 //Put brownout reset handler here. 00157 break; 00158 case (1 << EXTRF): 00159 restartAttempts = 0; 00160 // Put external reset handler here. 00161 break; 00162 case (1 << PORF): 00163 restartAttempts = 0; 00164 // Put power-on reset handler here. 00165 break; 00166 } 00167 } 00168 00169 00174 static void InitPorts(void) 00175 { 00176 // Init DRIVE_DDR for motor driving. 00177 DRIVE_DDR = (1 << UL) | (1 << UH) | (1 << VL) | (1 << VH) | (1 << WL) | (1 << WH); 00178 00179 // Init PORTD for PWM on PD5. 00180 DDRD = (1 << PD5); 00181 00182 // Disable digital input buffers on ADC channels. 00183 DIDR0 = (1 << ADC4D) | (1 << ADC3D) | (1 << ADC2D) | (1 << ADC1D) | (1 << ADC0D); 00184 } 00185 00186 00192 static void InitTimers(void) 00193 { 00194 // Set up Timer/counter0 for PWM, output on OCR0B, OCR0A as TOP value, prescaler = 1. 00195 TCCR0A = (0 << COM0A1) | (0 << COM0A0) | (1 << COM0B1) | (0 << COM0B0) | (0 << WGM01) | (1 << WGM00); 00196 TCCR0B = (1 << WGM02) | (0 << CS02) | (0 << CS01) | (1 << CS00); 00197 OCR0A = PWM_TOP_VALUE; 00198 TIFR0 = TIFR0; 00199 TIMSK0 = (0 << TOIE0); 00200 00201 // Set up Timer/counter1 for commutation timing, prescaler = 8. 00202 TCCR1B = (1 << CS11) | (0 << CS10); 00203 } 00204 00205 00211 static void InitADC(void) 00212 { 00213 // First make a measurement of the external reference voltage. 00214 ADMUX = ADMUX_REF_VOLTAGE; 00215 ADCSRA = (1 << ADEN) | (1 << ADSC) | (1 << ADIF) | (ADC_PRESCALER_16); 00216 while (ADCSRA & (1 << ADSC)) 00217 { 00218 00219 } 00220 referenceVoltageADC = ADCH; 00221 00222 // Initialize the ADC for autotriggered operation on PWM timer overflow. 00223 ADCSRA = (1 << ADEN) | (0 << ADSC) | (1 << ADATE) | (1 << ADIF) | (0 << ADIE) | ADC_PRESCALER_8; 00224 ADCSRB = ADC_TRIGGER_SOURCE; 00225 } 00226 00227 00232 static void InitAnalogComparator(void) 00233 { 00234 #ifdef ANALOG_COMPARATOR_ENABLE 00235 // Enable analog comparator interrupt on rising edge. 00236 ACSR = (0 << ACBG) | (1 << ACI) | (1 << ACIE) | (1 << ACIS1) | (1 << ACIS0); 00237 #endif 00238 } 00239 00240 00245 static void WatchdogTimerEnable(void) 00246 { 00247 __disable_interrupt(); 00248 __watchdog_reset(); 00249 00250 WDTCSR |= (1 << WDCE) | (1 << WDE); 00251 00252 WDTCSR = (1 << WDIF) | (1 << WDIE) | (1 << WDE) | (1 << WDP2); 00253 __enable_interrupt(); 00254 } 00255 00256 00262 static void MakeTables(void) 00263 { 00264 #if DIRECTION_OF_ROTATION == CCW 00265 driveTable[0] = DRIVE_PATTERN_STEP1_CCW; 00266 driveTable[1] = DRIVE_PATTERN_STEP2_CCW; 00267 driveTable[2] = DRIVE_PATTERN_STEP3_CCW; 00268 driveTable[3] = DRIVE_PATTERN_STEP4_CCW; 00269 driveTable[4] = DRIVE_PATTERN_STEP5_CCW; 00270 driveTable[5] = DRIVE_PATTERN_STEP6_CCW; 00271 00272 ADMUXTable[0] = ADMUX_W; 00273 ADMUXTable[1] = ADMUX_V; 00274 ADMUXTable[2] = ADMUX_U; 00275 ADMUXTable[3] = ADMUX_W; 00276 ADMUXTable[4] = ADMUX_V; 00277 ADMUXTable[5] = ADMUX_U; 00278 #else 00279 driveTable[0] = DRIVE_PATTERN_STEP1_CW; 00280 driveTable[1] = DRIVE_PATTERN_STEP2_CW; 00281 driveTable[2] = DRIVE_PATTERN_STEP3_CW; 00282 driveTable[3] = DRIVE_PATTERN_STEP4_CW; 00283 driveTable[4] = DRIVE_PATTERN_STEP5_CW; 00284 driveTable[5] = DRIVE_PATTERN_STEP6_CW; 00285 00286 ADMUXTable[0] = ADMUX_U; 00287 ADMUXTable[1] = ADMUX_V; 00288 ADMUXTable[2] = ADMUX_W; 00289 ADMUXTable[3] = ADMUX_U; 00290 ADMUXTable[4] = ADMUX_V; 00291 ADMUXTable[5] = ADMUX_W; 00292 00293 #endif 00294 00295 startupDelays[0] = 200; 00296 startupDelays[1] = 150; 00297 startupDelays[2] = 100; 00298 startupDelays[3] = 80; 00299 startupDelays[4] = 70; 00300 startupDelays[5] = 65; 00301 startupDelays[6] = 60; 00302 startupDelays[7] = 55; 00303 } 00304 00305 00311 static void StartMotor(void) 00312 { 00313 unsigned char i; 00314 00315 SET_PWM_COMPARE_VALUE(STARTUP_PWM_COMPARE_VALUE); 00316 00317 nextCommutationStep = 0; 00318 00319 //Preposition. 00320 DRIVE_PORT = driveTable[nextCommutationStep]; 00321 StartupDelay(STARTUP_LOCK_DELAY); 00322 nextCommutationStep++; 00323 nextDrivePattern = driveTable[nextCommutationStep]; 00324 00325 for (i = 0; i < STARTUP_NUM_COMMUTATIONS; i++) 00326 { 00327 DRIVE_PORT = nextDrivePattern; 00328 StartupDelay(startupDelays[i]); 00329 00330 ADMUX = ADMUXTable[nextCommutationStep]; 00331 00332 // Use LSB of nextCommutationStep to determine zero crossing polarity. 00333 zcPolarity = nextCommutationStep & 0x01; 00334 00335 nextCommutationStep++; 00336 if (nextCommutationStep >= 6) 00337 { 00338 nextCommutationStep = 0; 00339 } 00340 nextDrivePattern = driveTable[nextCommutationStep]; 00341 } 00342 00343 // Switch to sensorless commutation. 00344 TCNT1 = 0; 00345 TIMSK1 = (1 << OCIE1A); 00346 00347 // Set filteredTimeSinceCommutation to the time to the next commutation. 00348 filteredTimeSinceCommutation = startupDelays[STARTUP_NUM_COMMUTATIONS - 1] * (STARTUP_DELAY_MULTIPLIER / 2); 00349 } 00350 00351 00363 #pragma vector=TIMER0_OVF_vect 00364 __interrupt void MotorPWMBottom() 00365 { 00366 unsigned char temp; 00367 00368 // Disable ADC auto-triggering. This must be done here to avoid wrong channel being sampled on manual samples later. 00369 ADCSRA &= ~((1 << ADATE) | (1 << ADIE)); 00370 00371 // Wait for auto-triggered ADC sample to complete. 00372 while (!(ADCSRA & (1 << ADIF))) 00373 { 00374 00375 } 00376 temp = ADCH; 00377 if (((zcPolarity == EDGE_RISING) && (temp > ADC_ZC_THRESHOLD)) || ((zcPolarity == EDGE_FALLING) && (temp < ADC_ZC_THRESHOLD))) 00378 { 00379 unsigned int timeSinceCommutation; 00380 00381 // Find time since last commutation 00382 timeSinceCommutation = TCNT1; 00383 TCNT1 = COMMUTATION_CORRECTION; 00384 00385 // Filter the current ZC detection with earlier measurements through an IIR filter. 00386 filteredTimeSinceCommutation = (COMMUTATION_TIMING_IIR_COEFF_A * timeSinceCommutation 00387 + COMMUTATION_TIMING_IIR_COEFF_B * filteredTimeSinceCommutation) 00388 / (COMMUTATION_TIMING_IIR_COEFF_A + COMMUTATION_TIMING_IIR_COEFF_B); 00389 OCR1A = filteredTimeSinceCommutation; 00390 00391 speedUpdated = TRUE; 00392 00393 SET_TIMER1_INT_COMMUTATION; 00394 CLEAR_ALL_TIMER1_INT_FLAGS; 00395 00396 // Disable Timer/Counter0 overflow ISR. 00397 DISABLE_ALL_TIMER0_INTS; 00398 00399 // Read speed reference. 00400 00401 // Make sure that a sample is not in progress. 00402 while (ADCSRA & (1 << ADSC)) 00403 { 00404 00405 } 00406 // Change channel 00407 ADMUX = ADMUX_SPEED_REF; 00408 00409 // Start conversion manually. 00410 ADCSRA |= (1 << ADSC); 00411 00412 // Wait for conversion to complete. 00413 while((ADCSRA & (1 << ADSC))) 00414 { 00415 00416 } 00417 speedReferenceADC = ADCH; 00418 00419 // Read voltage reference. 00420 // Change ADC channel. 00421 ADMUX = ADMUX_REF_VOLTAGE; 00422 // Start conversion manually. 00423 ADCSRA |= (1 << ADSC); 00424 // Wait for conversion to complete. 00425 while((ADCSRA & (1 << ADSC))) 00426 { 00427 00428 } 00429 referenceVoltageADC = ADCH; 00430 00431 // Enable current measurements in ADC ISR. 00432 ADMUX = ADMUX_CURRENT; 00433 ADCSRA |= (1 << ADATE) | (1 << ADIE) | ADC_PRESCALER; 00434 } 00435 else 00436 { 00437 unsigned char tempADMUX; 00438 00439 tempADMUX = ADMUX; 00440 // Read current 00441 00442 // Make sure that a sample is not in progress 00443 while (ADCSRA & (1 << ADSC)) 00444 { 00445 00446 } 00447 00448 // Change channel 00449 ADMUX = ADMUX_CURRENT; 00450 00451 // Start conversion manually. 00452 ADCSRA |= (1 << ADSC); 00453 // Wait for conversion to complete. 00454 while((ADCSRA & (1 << ADSC))) 00455 { 00456 00457 } 00458 00459 shuntVoltageADC = ADCH; 00460 currentUpdated = TRUE; 00461 00462 // Restore ADC channel. 00463 ADMUX = tempADMUX; 00464 ADCSRA |= (1 << ADATE) | (1 << ADIE) | ADC_PRESCALER; 00465 } 00466 } 00467 00468 00481 #pragma vector=TIMER1_COMPA_vect 00482 __interrupt void Commutate() 00483 { 00484 // Commutate and clear commutation timer. 00485 DRIVE_PORT = nextDrivePattern; 00486 TCNT1 = 0; 00487 00488 zcPolarity = nextCommutationStep & 0x01; 00489 00490 // Set zero-cross detection holdoff time. 00491 CLEAR_ALL_TIMER1_INT_FLAGS; 00492 OCR1B = ZC_DETECTION_HOLDOFF_TIME_US; 00493 SET_TIMER1_INT_HOLDOFF; 00494 00495 __watchdog_reset(); 00496 } 00497 00498 00506 #pragma vector=TIMER1_COMPB_vect 00507 __interrupt void EnableZCDetection() 00508 { 00509 // Enable TCNT0 overflow ISR. 00510 CLEAR_ALL_TIMER0_INT_FLAGS; 00511 CLEAR_ALL_TIMER1_INT_FLAGS; 00512 SET_TIMER0_INT_ZC_DETECTION; 00513 DISABLE_ALL_TIMER1_INTS; 00514 00515 // Set up ADC for zero-cross detection 00516 ADMUX = ADMUXTable[nextCommutationStep]; 00517 00518 // Wait for ADC to complete 00519 while (!(ADCSRA & (1 << ADIF))) 00520 { 00521 00522 } 00523 ADCSRA &= ~(1 << ADIE); 00524 ADCSRA |= (1 << ADSC) | (1 << ADATE); 00525 00526 // Rotate commutation step counter. 00527 nextCommutationStep++; 00528 if (nextCommutationStep >= 6) 00529 { 00530 nextCommutationStep = 0; 00531 } 00532 nextDrivePattern = driveTable[nextCommutationStep]; 00533 } 00534 00535 00536 /* \brief ADC complete interrupt service routine, used for current measurements. 00537 * 00538 * This interrupt service routine is only enabled when current measurements are 00539 * auto-triggered by the PWM counter overflow. The measured value is simply 00540 * copied to \ref shuntVoltageADC, the \ref currentUpdated flag is set and 00541 * Timer0 (PWM timer) interrupt flags are cleared. 00542 */ 00543 #pragma vector=ADC_vect 00544 __interrupt void CurrentMeasurementComplete() 00545 { 00546 shuntVoltageADC = ADCH; 00547 currentUpdated = TRUE; 00548 CLEAR_ALL_TIMER0_INT_FLAGS; 00549 } 00550 00551 00558 #pragma vector=WDT_vect 00559 __interrupt void WatchdogISR() 00560 { 00561 DISABLE_DRIVING; 00562 for(;;) 00563 { 00564 ; 00565 } 00566 } 00567 00573 #ifdef ANALOG_COMPARATOR_ENABLE 00574 #pragma vector=ANA_COMP_vect 00575 __interrupt void OverCurrentISR() 00576 { 00577 DISABLE_DRIVING; 00578 for(;;) 00579 { 00580 ; 00581 } 00582 } 00583 #endif 00584 00585 00593 void StartupDelay(unsigned int delay) 00594 { 00595 CLEAR_ALL_TIMER1_INT_FLAGS; 00596 do 00597 { 00598 TCNT1 = 0xffff - STARTUP_DELAY_MULTIPLIER; 00599 // Wait for timer to overflow. 00600 while (!(TIFR1 & (1 << TOV1))) 00601 { 00602 00603 } 00604 00605 CLEAR_ALL_TIMER1_INT_FLAGS; 00606 delay--; 00607 } while (delay); 00608 } 00609 00610 00611 00612 #ifdef SPEED_CONTROL_CLOSED_LOOP 00613 00619 static void PWMControl(void) 00620 { 00621 signed int speedCompensation; 00622 static unsigned char currentCompensation = 0; 00623 static signed int duty = STARTUP_PWM_COMPARE_VALUE; 00624 00625 // Run speed control only if a new speed measurement is available. 00626 if (speedUpdated) 00627 { 00628 speedCompensation = SpeedControl(); 00629 speedUpdated = FALSE; 00630 duty += speedCompensation; 00631 } 00632 00633 // Run current control only if a new current measurement is available. 00634 if (currentUpdated) 00635 { 00636 currentCompensation = CurrentControl(); 00637 currentUpdated = FALSE; 00638 } 00639 00640 // Keep duty cycle within limits. 00641 if (duty < MIN_PWM_COMPARE_VALUE) 00642 { 00643 duty = MIN_PWM_COMPARE_VALUE; 00644 } 00645 if (duty > (MAX_PWM_COMPARE_VALUE - currentCompensation)) 00646 { 00647 duty = MAX_PWM_COMPARE_VALUE - currentCompensation; 00648 } 00649 00650 SET_PWM_COMPARE_VALUE((unsigned char)duty); 00651 } 00652 #endif 00653 00654 #ifdef SPEED_CONTROL_OPEN_LOOP 00655 static void PWMControl(void) 00656 { 00657 // Only update duty cycle if a new speed reference measurement has been made. (Done right after speed measurement is ready) 00658 if (speedUpdated) 00659 { 00660 // Calculate duty cycle from speed reference value. 00661 SET_PWM_COMPARE_VALUE(MIN_PWM_COMPARE_VALUE + speedReferenceADC * (MAX_PWM_COMPARE_VALUE - MIN_PWM_COMPARE_VALUE) / ADC_RESOLUTION); 00662 } 00663 } 00664 #endif 00665 00666 00672 static unsigned long CalculateSpeed() 00673 { 00674 // Copy used to minimize period where interrupts are disabled. 00675 unsigned int filteredTimeSinceCommutationCopy; 00676 unsigned long rotationPeriod; 00677 unsigned long speed; 00678 00679 /* 00680 Disable interrupts to ensure that \ref filteredTimeSinceCommutation is accessed in 00681 an atomic operation. 00682 */ 00683 __disable_interrupt(); 00684 filteredTimeSinceCommutationCopy = filteredTimeSinceCommutation; 00685 __enable_interrupt(); 00686 00687 /* 00688 filteredTimeSinceCommutation is one half commutation time. Must be multiplied by 12 to get 00689 one full rotation. 00690 */ 00691 rotationPeriod = (unsigned long)filteredTimeSinceCommutationCopy * 12; 00692 speed = (TICKS_PER_MINUTE / rotationPeriod); 00693 00694 return speed; 00695 } 00696 00697 00707 static unsigned long CalculateSpeedSetpoint() 00708 { 00709 return (MIN_SPEED + ((MAX_SPEED - MIN_SPEED) * (unsigned int)speedReferenceADC) / ADC_RESOLUTION); 00710 } 00711 00712 00719 static unsigned int CalculateCurrent() 00720 { 00721 unsigned long ADCref; 00722 unsigned int current; 00723 00724 // Calculate the voltage at AREF pin (scaled down motor supply voltage), 00725 // using the known reference voltage. (In milliVolts) 00726 ADCref = EXTERNAL_REF_VOLTAGE * 256UL / referenceVoltageADC; 00727 00728 // Calculate the current through the shunt. (In milliAmperes) 00729 current = (unsigned int)((shuntVoltageADC * ADCref * 1000UL / 256UL) / SHUNT_RESISTANCE); 00730 00731 return current; 00732 } 00733 00734 00742 static signed int SpeedControl(void) 00743 { 00744 unsigned long speedSetpoint; 00745 unsigned long currentSpeed; 00746 signed long speedError; 00747 signed long dutyChange; 00748 00749 00750 00751 speedSetpoint = CalculateSpeedSetpoint(); 00752 currentSpeed = CalculateSpeed(); 00753 speedError = (speedSetpoint - currentSpeed); 00754 dutyChange = speedError * P_REG_K_P / P_REG_SCALING; 00755 00756 return dutyChange; 00757 } 00758 00759 00767 static unsigned char CurrentControl(void) 00768 { 00769 unsigned int current; 00770 unsigned int overCurrentCorrection = 0; 00771 00772 current = CalculateCurrent(); 00773 00774 // Cut power to motor if current is critically high. 00775 if (current > CURRENT_LIMITER_CRITICAL) 00776 { 00777 DRIVE_PORT = 0x00; 00778 for (;;) 00779 { 00780 // Stop and let watchdog timer reset part. 00781 } 00782 } 00783 00784 if (current > CURRENT_LIMITER_START) 00785 { 00786 overCurrentCorrection = (current - CURRENT_LIMITER_START) * CURRENT_LIMITER_FACTOR; 00787 } 00788 00789 if (overCurrentCorrection > 255) 00790 { 00791 return 255; 00792 } 00793 00794 return overCurrentCorrection; 00795 } 00796