Main Page | File List | Globals

main.c

Go to the documentation of this file.
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 

Generated on Tue Oct 11 10:57:02 2005 for Sensorless control of 3-phase brushless DC motors by  doxygen 1.4.4