#include <avr/io.h>
#include <avr/interrupt.h>
#include <avr/sleep.h>
#include <util/delay.h>
#include "PMSM_tables.h"
#include <avr/fuse.h>
/* Hardware entsprechend AVR449
1 PB0 !OC1A UL Gate-Treiber U Low-Side
2 PB1 OC1A UH Gate-Treiber U High-Side
3 PB2 !OC1B VL Gate-Treiber V Low-Side
4 PB3 OC1B VL Gate-Treiber V Low-Side
5 Ucc
6 GND
7 PB4 !OC1D WL Gate-Treiber W Low-Side
8 PB5 OC1D WL Gate-Treiber W Low-Side
9 PB6 INT0/FP Fehler-Schutz (Überstromdetektor)
10 PB7 RESET -
11 PA7 ADC6 Geschwindigkeitsvorgabe (Poti)
12 PA6 AIN0 (alternativer Shunt)
13 PA5 AIN2
14 PA4 Debug-Ausgang
15 AUcc
16 AGND
17 PA3 Richtungsvorgabe
18 PA2 H3 Positionssignal vom Hallsensor
19 PA1 H2 Positionssignal vom Hallsensor
20 PA0 H1 Positionssignal vom Hallsensor
U,V,W = gängige Bezeichnung für Anschlüsse eines Dreiphasenmotors
*/
FUSES={
0b11000001, // HF PLL = 16 MHz, ungeteilt, schneller Hochlauf
0b01010100, // Kein RESET, EEPROM behalten, Brownout bei 4,3 V
0b11111111 // Kein SPM
};
#if SPEED_CONTROL_CLOSED_LOOP
#include "pid.h"
#endif
register uint16_t sineTableIncrement asm("r10");
register uint16_t sineTableIndex asm("r8");
register uint16_t commutationTicks asm("r6");
register uint16_t amplitude asm("r4");
register uint8_t sineTableNextSectorStart asm("r3");
register uint8_t speedControllerTimer asm("r2");
uint8_t advanceCommutationSteps;
#if SPEED_CONTROL_CLOSED_LOOP
//! Struct used to hold PID controller parameters and variables.
PID_DATA pidData;
#endif
static void PortsInit(void) {
#if (HALL_PULL_UP_ENABLE)
PORTA = 7<<H1_PIN;
#endif
DDRA = (1 << PA4);
}
// CKSEL fuses to 0001. Otherwise, the application will hang here forever.
static void PLLInit() {
while (!(PLLCSR & 1<<PLOCK)); //Wait for PLL lock
PLLCSR = 1<<PCKE;
}
static void PWMInit() {
//Start TCNT1 with prescaler 1.
TCCR1B = PWM_INVERT_OUTPUT<<PWM1X | 1<<CS10;
//Enable fault protection input on INT0 pin, falling edge.
TCCR1D = 1<<FPIE1 | 1<<FPEN1;
//Set dead-time.
DT1 = DEAD_TIME<<4 | DEAD_TIME;
//Set PWM top value.
TC1H = 3;
OCR1C= 0xFF;
}
static void ADCInit() {
//Use PA7 as ADC input, 2.56V internal voltage reference without bypass capacitor.
ADMUX = 1<<REFS1 | 1<<MUX2 | 1<<MUX1 ;
ADCSRA = 1<<ADEN | //Enable the ADC.
1<<ADSC | //Start the first converssion.
1<<ADPS2 | 1<<ADPS1 | 1<<ADPS0; // Prescaler CK/128.
ADCSRB = 1<<REFS2; //2.56V internal reference.
}
static void PinChangeInit() {
//Initialize hall change interrupt
PCMSK0 = 7<<H1_PIN;
//Clear all other pin change interrupts.
PCMSK1 = 0x00;
GIMSK |= 1<<PCIE1;
//Force the pin change interrupt to be triggered.
DDRA |= 7<<H1_PIN;
DDRA &= ~(7<<H1_PIN);
}
static void SpeedController(uint16_t speedReference) {
#if SPEED_CONTROL_CLOSED_LOOP
//Calculate an increment setpoint from the analog speed input.
int16_t incrementSetpoint = ((uint32_t)speedReference * SPEED_CONTROLLER_MAX_INCREMENT) / SPEED_CONTROLLER_MAX_INPUT;
//PID controller with feed forward from speed input.
int32_t outputValue;
outputValue = (uint32_t)speedReference;
outputValue += pidData.Controller(incrementSetpoint, (int16_t)sineTableIncrement);
if (outputValue < 0) outputValue = 0;
else if (outputValue > 0x3FF) outputValue = 0x3FF;
cli();
amplitude = outputValue;
sei();
#else
cli();
amplitude = speedReference;
sei();
#endif
}
inline void DisablePWMOutputs() { DDRB &=~PWM_PIN_MASK_PORTB;}
inline void EnablePWMOutputs() { DDRB |= PWM_PIN_MASK_PORTB;}
inline void TimerSetModeSinusoidal() {
//Set PWM pins to input (Hi-Z) while changing modes.
DisablePWMOutputs();
//Set Timer/counter1 in phase and frequency correct mode.
TCCR1A = 1<<COM1A0 | 1<<COM1B0 | 1<<PWM1A | 1<<PWM1B;
TCCR1C = 1<<COM1A0S | 1<<COM1B0S | 1<<COM1D0 | 1<<PWM1D;
TCCR1D &= ~(1<<WGM11 | 1<<WGM10);
TCCR1D |= 1<<WGM10;
OCR1D=OCR1B=OCR1A=TC1H=0;
GPIOR0|= 0x20;
GPIOR0&=~0x40; //WAVEFORM_SINUSOIDAL;
//Wait for the PWM cycle to complete.
TIFR = TIFR;
while (!(TIFR & 1<<TOV1));
//Change PWM pins to output again to allow PWM control.
EnablePWMOutputs();
}
inline void TimerSetModeBlockCommutation() {
//Set PWM pins to input (Hi-Z) while changing modes.
DisablePWMOutputs();
//Set Timer/counter1 in dual slope PWM6 mode, clear on upcounting, set on downcounting (non-inverting).
TCCR1A = (1 << COM1A1) | (0 << COM1A0) | (1 << COM1B1) | (0 << COM1B0) | (1 << PWM1A) | (1 << PWM1B);
TCCR1C = (1 << COM1A1S) | (0 << COM1A0S) | (1 << COM1B1S) | (0 << COM1B0S) | (1 << COM1D1) | (0 << COM1D0) | (1 << PWM1D);
TCCR1D |= (1 << WGM11) | (1 << WGM10);
TCCR1E = 0x00;
//Set output duty cycle to 0.
OCR1A=TC1H=0;
GPIOR0|= 0x20;
GPIOR0|= 0x40; //WAVEFORM_BLOCK_COMMUTATION;
//Change PWM pins to output again to allow PWM control.
EnablePWMOutputs();
}
#if TURN_MODE_BRAKE
inline void TimerSetModeBrake() {
//Set PWM pins to input (Hi-Z) while changing modes.
DisablePWMOutputs();
//Set Timer/counter1 in phase and frequency correct mode.
TCCR1A = 1<<COM1A0 | 1<<COM1B0 | 1<<PWM1A | 1<<PWM1B;
TCCR1C = 1<<COM1A0S | 1<<COM1B0S | 1<<COM1D0 | 1<<PWM1D;
TCCR1D &= ~(1<<WGM11 | 1<<WGM10);
TCCR1D |= 1<<WGM10;
//All output duty cycles at 0 creates a braking force. (Low side braking)
OCR1D=OCR1B=OCR1A=TC1H=0;
GPIOR0&=~0x20;
GPIOR0|= 0x40; // WAVEFORM_BRAKING;
//Change PWM pins to output again to allow PWM control.
EnablePWMOutputs();
}
#endif
inline void BlockCommutate(const uint8_t hall) {
TCCR1E = pgm_read_byte((GPIOR0&WANTDIR_REVERSE
?blockCommutationTableReverse
:blockCommutationTableForward)+hall);
}
inline uint8_t GetHall() { return (PINA>>H1_PIN)&7;}
inline void DesiredDirectionUpdate() {
if (PINA & 1<<DIRECTION_PIN) GPIOR0|=WANTDIR_REVERSE;
else GPIOR0&=~WANTDIR_REVERSE;
}
inline void ActualDirectionUpdate(uint8_t lastHall, const uint8_t newHall) {
//Make sure that lastHall is within bounds of table. If not, set to an
//illegal hall value, but legal table index.
if (lastHall > 7) lastHall = 0;
if (pgm_read_byte(HallSequenceForward+lastHall) == newHall) {
GPIOR0&=~DIR_REVERSE;
GPIOR0|= DIR_KNOWN;
}else if (pgm_read_byte(HallSequenceReverse+lastHall) == newHall) {
GPIOR0|= DIR_REVERSE;
GPIOR0|= DIR_KNOWN;
}else{
GPIOR0&=~DIR_KNOWN;
GPIOR0&=~MOTOR_SYNCHRONIZED;
}
}
inline uint16_t SineTableIncrementCalculate(const uint16_t ticks) {
return (uint16_t)(((SINE_TABLE_LENGTH/6) << 8) / ticks);
}
inline void AdjustSineTableIndex(const uint16_t increment) {
sineTableIndex += increment;
// If the table index is out of bounds, wrap the index around the table end
// to continue from the beginning. Also wrap the next sector start index.
if (sineTableIndex>>8 >= SINE_TABLE_LENGTH) {
sineTableIndex -= SINE_TABLE_LENGTH<<8;
sineTableNextSectorStart -= SINE_TABLE_LENGTH;
}
//Make copy of sineNextSectorStart to specify order of volatile access.
uint8_t nextSectorStart = sineTableNextSectorStart;
if ((sineTableIndex >> 8) > nextSectorStart) {
sineTableIndex = (nextSectorStart << 8);
}
}
inline void CommutationTicksUpdate() {
if (commutationTicks < COMMUTATION_TICKS_STOPPED) commutationTicks++;
else{
GPIOR0|= MOTOR_STOPPED;
GPIOR0&=~MOTOR_SYNCHRONIZED;
sineTableIncrement = 0;
if ((GPIOR0&0x60)!=WF_BLOCK_COMMUTATION) {
TimerSetModeBlockCommutation();
BlockCommutate(GetHall());
#if SPEED_CONTROL_CLOSED_LOOP
pidData.Reset_Integrator();
#endif
}
}
}
inline void MotorSynchronizedUpdate() {
static uint8_t synchCount = 0;
if (GPIOR0&DIR_KNOWN
&& !((GPIOR0>>2^GPIOR0)&DIR_REVERSE) /*desiredDirection == actualDirection*/
&& !(GPIOR0&MOTOR_STOPPED)
&& !(GPIOR0&MOTOR_SYNCHRONIZED)) {
synchCount++;
if (synchCount >= SYNCHRONIZATION_COUNT) GPIOR0|=MOTOR_SYNCHRONIZED;
}else synchCount = 0;
}
inline uint8_t SineTableSmallGetValue(uint8_t index) {
//The last 3rd of the table is zero.
if (index >= (SINE_TABLE_LENGTH * 2 / 3) ) return 0;
//The second third of the table is a mirror of the first third.
if (index >= (SINE_TABLE_LENGTH / 3) )
index = ((SINE_TABLE_LENGTH * 2 / 3) - 1) - index;
return pgm_read_byte(sineTable+index);
}
/*! Fast unsigned multiply of a 15 bit number with an 8 bit number with 15 bit result.
* 50 CPU clock cycles.
*/
inline unsigned int MultiplyUS15x8(const uint16_t m15, const uint8_t m8) {
unsigned int result = 0;
if (m8 & 1<<0) result += m15; result >>= 1;
if (m8 & 1<<1) result += m15; result >>= 1;
if (m8 & 1<<2) result += m15; result >>= 1;
if (m8 & 1<<3) result += m15; result >>= 1;
if (m8 & 1<<4) result += m15; result >>= 1;
if (m8 & 1<<5) result += m15; result >>= 1;
if (m8 & 1<<6) result += m15; result >>= 1;
if (m8 & 1<<7) result += m15; result >>= 1;
return result;
}
inline void SineOutputUpdate() {
#if SINE_TABLE_SIZE_SMALL
uint16_t temp;
uint8_t tempIndex;
//Calculate U phase output duty cycle.
tempIndex = (uint8_t)(sineTableIndex >> 8);
temp = SineTableSmallGetValue(tempIndex);
if (temp) temp = MultiplyUS15x8(amplitude, temp);
TC1H = uint8_t(temp>>8);
OCR1A=uint8_t(temp);
//Calculate U phase + 120 degree output duty cycle.
tempIndex += (SINE_TABLE_LENGTH / 3);
if (tempIndex >= SINE_TABLE_LENGTH) tempIndex -= SINE_TABLE_LENGTH;
temp = SineTableSmallGetValue(tempIndex);
if (temp) temp = MultiplyUS15x8(amplitude, temp);
TC1H = uint8_t(temp>>8);
if (GPIOR0&WANTDIR_REVERSE) OCR1B=uint8_t(temp);
else OCR1D=uint8_t(temp);
//Calculate U phase + 240 degree output duty cycle.
tempIndex += (SINE_TABLE_LENGTH / 3);
if (tempIndex >= SINE_TABLE_LENGTH) tempIndex -= SINE_TABLE_LENGTH;
temp = SineTableSmallGetValue(tempIndex);
if (temp) temp = MultiplyUS15x8(amplitude, temp);
TC1H = uint8_t(temp>>8);
if (GPIOR0&WANTDIR_REVERSE) OCR1D=uint8_t(temp);
else OCR1B=uint8_t(temp);
#else
uint8_t const*sineTablePtr = sineTable;
uint16_t temp;
//Add sine table offset to pointer. Must be multiplied by 3, since one
//value for each phase is stored in the table.
//sineTablePtr += (uint8_t)(sineTableIndex >> 8) * 3;
{
uint8_t tempIndex = (uint8_t)(sineTableIndex >> 8);
sineTablePtr += tempIndex;
sineTablePtr += tempIndex;
sineTablePtr += tempIndex;
}
//Calculate output duty cycles. Since one phase is always zero, the scaling
//is only performed on the two non-zero phases, saving one multiply.
//Calculate U phase output duty cycle.
temp = pgm_read_byte(sineTablePtr++);
if (temp) temp = MultiplyUS15x8(amplitude, temp);
TC1H = uint8_t(temp>>8);
OCR1A= uint8_t(temp);
//Calculate U + 240 degree phase output duty cycle.
temp = pgm_read_byte(sineTablePtr++);
if (temp) temp = MultiplyUS15x8(amplitude, temp);
TC1H = uint8_t(temp>>8);
if (GPIOR0&WANTDIR_REVERSE) OCR1D = uint8_t(temp);
else OCR1B = uint8_t(temp);
//Calculate U + 120 degree phase output duty cycle.
temp = pgm_read_byte(sineTablePtr++);
if (temp) temp = MultiplyUS15x8(amplitude, temp);
TC1H = uint8_t(temp>>8);
if (GPIOR0&WANTDIR_REVERSE) OCR1B = uint8_t(temp);
else OCR1D = uint8_t(temp);
#endif
}
/*! \brief Hall sensor change interrupt service routine.
*
* This ISR is run every time one of the hall sensor inputs changes value.
* The responsibilities of this ISR are:
* - Synchronize sine wave generation to hall sensors.
* - Block commutation.
* - Direction control.
* - Synchronization control.
* - Sine table increment calculation.
* - Stop detection.
*/
ISR(PCINT_vect) {
static uint8_t lastHall = 0xff;
uint8_t hall;
hall = GetHall();
//Make sure that the hall sensors really changed.
if (hall == lastHall) return;
MotorSynchronizedUpdate();
if (((GPIOR0&0x60)!=WF_SINUSOIDAL) && GPIOR0&MOTOR_SYNCHRONIZED) {
TimerSetModeSinusoidal();
}
//If sinusoidal driving is used, synchronize sine wave generation to the
//current hall sensor value. Advance commutation (lead angle) is also
//added in the process.
switch (GPIOR0&0x60) {
case WF_SINUSOIDAL: {
uint16_t tempIndex;
tempIndex = (pgm_read_byte((GPIOR0&WANTDIR_REVERSE
?CSOffsetsReverse
:CSOffsetsForward)+hall)
+ advanceCommutationSteps) << 8;
sineTableIndex = tempIndex;
//Adjust next sector start index. It might be set to a value larger than
//SINE_TABLE_LENGTH at this point. This is adjusted in AdjustSineTableIndex
//and should not be done here, as it will cause problems when advance
//commutation is used.
sineTableNextSectorStart = (tempIndex>>8) + SINE_TABLE_LENGTH/6;
}break;
//If block commutation is used. Commutate according to hall signal.
case WF_BLOCK_COMMUTATION: {
BlockCommutate(hall);
}break;
}
//Update the actual direction flag.
ActualDirectionUpdate(lastHall, hall);
lastHall = hall;
//Calculate new step size for sine wave generation and reset commutation
//timer.
sineTableIncrement = SineTableIncrementCalculate(commutationTicks);
commutationTicks = 0;
//Since the hall sensors are changing, the motor can not be stopped.
GPIOR0&=~MOTOR_STOPPED;
}
/*! \brief Timer/counter1 overflow interrupt service routine
*
* This ISR is run every time Timer/counter1 overflows. This is the same moment
* as the double buffered PWM outputs are updated.
*
* The responsibilities of this ISR are:
* - Sine wave modulation update.
* - Direction command input.
* - Stop detection.
* - Speed controller timing.
*/
ISR(TIMER1_OVF_vect) {
PORTA|= 1<<PA4;
switch (GPIOR0&0x60) {
case WF_SINUSOIDAL: {
AdjustSineTableIndex(sineTableIncrement);
SineOutputUpdate();
}break;
case WF_BLOCK_COMMUTATION: {
uint16_t blockCommutationDuty = amplitude * BLOCK_COMMUTATION_DUTY_MULTIPLIER;
if (blockCommutationDuty > 0x3FF) blockCommutationDuty = 0x3FF;
TC1H = uint8_t(blockCommutationDuty>>8);
OCR1A= uint8_t(blockCommutationDuty);
}break;
}
//Update desired direction flag.
DesiredDirectionUpdate();
static uint8_t lastDesiredDirection = 0xff;
if ((GPIOR0&WANTDIR_REVERSE)!=lastDesiredDirection) {
#if TURN_MODE_BRAKE
//Set motor in brake mode. The motor will automatically start once it is
//synchronized or stopped.
GPIOR0&=~MOTOR_SYNCHRONIZED;
if (GPIOR0&DIR_KNOWN)
TimerSetModeBrake(); // Automatically sets driveWaveform.
#else
//Disable driver signals to let motor coast. The motor will automatically
//start once it is synchronized or stopped.
DisablePWMOutputs();
GPIOR0&=~MOTOR_SYNCHRONIZED;
GPIOR0|= 0x20;
GPIOR0|= 0x40; // driveWaveform = WAVEFORM_UNDEFINED;
#endif
lastDesiredDirection = GPIOR0&WANTDIR_REVERSE;
}
CommutationTicksUpdate();
if (speedControllerTimer) --speedControllerTimer;
PORTA&=~(1<<PA4);
}
/*! \brief Hardware fault protection interrupt.
*
* This ISR will be run every time the hardware fault protection disables the
* PWM outputs. The required action to this event will be different from
* application to application. Here, a delay is inserted, before the PWM
* outputs are once again enabled.
*/
ISR(FAULT_PROTECTION_vect) {
_delay_ms(800);
TCCR1D |= 1<<FPEN1;
#if SPEED_CONTROL_CLOSED_LOOP
pidData.Reset_Integrator();
#endif
}
int main(void) {
PortsInit();
PLLInit();
PWMInit();
PinChangeInit();
ADCInit();
#if SPEED_CONTROL_CLOSED_LOOP
pidData.Init(PID_K_P,PID_K_I,PID_K_D);
#endif
speedControllerTimer = SPEED_CONTROLLER_TIME_BASE;
advanceCommutationSteps=0;
// Set motorStopped to FALSE at startup. This will make sure that the motor
// is not started if it is not really stopped. If it is stopped, this variable
// will quickly be updated.
// Set motorSyncronized to FALSE at startup. This will prevent the motor from being
// driven until the motor is in synch or stopped.
GPIOR0 = 0;
DesiredDirectionUpdate();
//Enable Timer/counter1 overflow.
TIMSK |= 1<<TOV1;
//Enable interrupts globally and let motor driver take over.
sei();
MCUCR=0x20; // Sleep Enable
for (;;) {
sleep_cpu(); // Schlafen bis Interrupt: Timer1, Hall-Sensoren, Fehlereingang
uint16_t speedReference=0;
if (!(ADCSRA & 1<<ADSC)) {
speedReference = ADC;
ADCSRA |= 1<<ADSC;
}
if (!speedControllerTimer) {
speedControllerTimer = SPEED_CONTROLLER_TIME_BASE;
SpeedController(speedReference);
}
}
}
| Detected encoding: UTF-8 | 0
|