Skip to content

Commit

Permalink
Phase corrected PWM
Browse files Browse the repository at this point in the history
  • Loading branch information
@suovula
suovula committed Oct 14, 2012
1 parent 33b16eb commit e672cbc
Showing 1 changed file with 100 additions and 77 deletions.
177 changes: 100 additions & 77 deletions driver/AirCore_4_spwm/AirCore_4_spwm.ino
View file
Expand Up @@ -2,12 +2,17 @@
#include <Wire.h> #include <Wire.h>


// This is ATmega328P specific implementation! // This is ATmega328P specific implementation!
const uint8_t I2CSlaveAddress = 0x04; // The 7-bit address, remember to change this! enum { BoardSelector = 0 }; // REMEMBER TO CHANGE THIS BEFORE PROGRAMMING!
const uint8_t I2CSlaveAddresses[5] = { 0x04, 0x05, 0x06, 0x07, 0x08 };
const uint8_t I2CSlaveAddress = I2CSlaveAddresses[BoardSelector];
const bool I2CMode = true;


volatile bool demoMode = true; // Starts in Demo mode where timer2 interrupt will automatically increment the gauge values volatile bool demoMode = true; // Starts in Demo mode where timer2 interrupt will automatically increment the gauge values


const uint8_t PwmFrequencySelector = 13; // 0..13..21 const uint8_t PwmScaler = 11; // 0..15!
// Selector Resolution Frequency Notes const uint16_t PwmPulseWidth = (1 << PwmScaler);

// PwmScaler Resolution Frequency Notes
// 0 2 16 MHz Too fast, doesn't even generate valid PWM // 0 2 16 MHz Too fast, doesn't even generate valid PWM
// 1 4 8 MHz // 1 4 8 MHz
// 2 8 2 MHz Too fast still, but the aircore already moving, very jerky // 2 8 2 MHz Too fast still, but the aircore already moving, very jerky
Expand All @@ -17,49 +22,51 @@ const uint8_t PwmFrequencySelector = 13; // 0..13..21
// 6 128 125 kHz // 6 128 125 kHz
// 7 256 63 kHz // 7 256 63 kHz
// 8 512 31 kHz Aircore moves, but still jerky // 8 512 31 kHz Aircore moves, but still jerky
// 9 1024 16 kHz // 9 1Ki 16 kHz
// 10 2048 7812 Hz // 10 2Ki 7812 Hz
// 11 4096 3906 Hz Aircore moves fine, some noise // 11 4Ki 3906 Hz Aircore moves fine, some noise
// 12 8192 1953 Hz Aircore moves smoothly, but noise is very irritating // 12 8Ki 1953 Hz WAT, now it works fine? // Aircore moves smoothly, but noise is very irritating
// 13 16386 976 Hz Optimal? // 13 16Ki 976 Hz Optimal?
// 14 32768 488 Hz Optimal? // 14 32Ki 488 Hz Optimal?
// 15 65536 244 Hz Aircore needle starts to vibrate a little bit at the quadrants. This could be minimized by tweaking the sine table -> TODO // 15 64Ki 244 Hz Aircore needle starts to vibrate a little bit at the quadrants. This could be minimized by tweaking the sine table -> TODO
// 16 16368 122 Hz
// 17 32768 61 Hz Too slow for aircores! const int GaugeCount = 8;
// 18 65536 31 Hz
// 19 16384 15 Hz
// 20 32768 7.7 Hz
// 21 65536 3.8 Hz Jeah, right, om nom nom

const uint8_t PwmTimerPrescaler[22] = { 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 2, 2, 2, 3, 3, 3, };
const uint16_t PwmTimerDuration[22] = { 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 8, 8, 8, 64, 64, 64, };
const uint32_t PwmPulseWidth[22] = { 2, 4, 8, 16, 32, 64, 128, 256, 512, 1024, 2048, 4096, 8192, 16384, 32768, 65536, 16368, 32768, 65536, 16384, 32768, 65536, };

// There are two pins per gauge, and they are allocated from ports D, C and B. I2C mode assumed!
const uint8_t GaugeCountPortD = 4;
const uint8_t GaugePinSinPortD[GaugeCountPortD] = { B00000001, B00000100, B00010000, B01000000, };
const uint8_t GaugePinCosPortD[GaugeCountPortD] = { B00000010, B00001000, B00100000, B10000000, };
const uint8_t PortMaskD = B11111111;

const uint8_t GaugeCountPortC = 2;
const uint8_t GaugePinSinPortC[GaugeCountPortC] = { B00000001, B00000100 };
const uint8_t GaugePinCosPortC[GaugeCountPortC] = { B00000010, B00001000 };
const uint8_t PortMaskC = B00001111;

const uint8_t GaugeCountPortB = 2;
const uint8_t GaugePinSinPortB[GaugeCountPortB] = { B00000001, B00000100 };
const uint8_t GaugePinCosPortB[GaugeCountPortB] = { B00000010, B00001000 };
const uint8_t PortMaskB = B00001111;

const uint8_t GaugeCount = GaugeCountPortD + GaugeCountPortC + GaugeCountPortB; // Should be 8!

// The actual gauge values. As bytes they can be read or write with interrups enabled. For read-modify-write disable interrupts! // The actual gauge values. As bytes they can be read or write with interrups enabled. For read-modify-write disable interrupts!
volatile uint8_t gaugeValue[GaugeCount] = { 0 }; // angle = 0..255 volatile uint8_t gaugeValue[GaugeCount] = { 0 }; // angle = 0..255


// This takes 1024 bytes of RAM // This takes 1024 bytes of RAM
uint16_t sin256Table[256] = { 0 }; uint16_t sin256Table[256] = { 0 };


// Timer2 interrupt, called 244.140625 times per second const uint8_t PortMaskD = I2CMode ? B11111111 : B11111100;
const uint8_t PortMaskC = B00001111;
const uint8_t PortMaskB = I2CMode ? B00001111 : B00111111;


// Main clock, Timer1, 32-bit
volatile uint16_t tick_counter_31to16 = 0; // Incremented ~488 times per second
ISR(TIMER1_OVF_vect, ISR_BLOCK) { // interrupt service routine that wraps a user defined function supplied by attachInterrupt
++tick_counter_31to16;
}

inline uint32_t getTickCount32() {
cli();
uint16_t timer = TCNT1; // (1a) Read HW timer
if (TIFR1 & (1 << TOV1)) { // INTFLAGS[0] X OVFIF: Overflow/Underflow Interrupt Flag
// Handle it straight here, instead of the interrupt handler
TIFR1 = (1 << TOV1);; // Clear the pending interrupt
timer = TCNT1;; // (1b) Overflow occurred concurrently, read timer again to get new value after overflow
++tick_counter_31to16;
}

uint32_t ticks = uint32_t(tick_counter_31to16) << 16 | timer; // (2) Read volatile overflow counter

sei();

return ticks;
}


// Timer2 interrupt, called 244.140625 times per second in Demo mode
ISR(TIMER2_OVF_vect) { ISR(TIMER2_OVF_vect) {
if (demoMode) { if (demoMode) {
for (uint8_t i = 0; i < GaugeCount; ++i) { for (uint8_t i = 0; i < GaugeCount; ++i) {
Expand All @@ -73,35 +80,34 @@ ISR(TIMER2_OVF_vect) {
void setup() { void setup() {


pinMode(13, OUTPUT); // Status LED pinMode(13, OUTPUT); // Status LED
digitalWrite(13, HIGH);


cli(); { cli(); {
// Set Timer1 to X MHz // Set Timer1 to 16 MHz
TCCR1A = 0; // No PWM is used. So set this to zero. - 15.11.1 TCCR1A – Timer/Counter1 Control Register A TCCR1A = 0; // Normal mode, no PWM
TCCR1B = PwmTimerPrescaler[PwmFrequencySelector]; // See 15.11.2 TCCR1B – Timer/Counter1 Control Register B TCCR1B = B000; // 16000000 Hz = ~244 Hz overflow
TIMSK1 = 0 << TOIE1; // Bit 0 – TOIE1: Timer/Counter1, Overflow Interrupt Enable - // 15.11.8 TIMSK1 – Timer/Counter1 Interrupt Mask Register TIMSK1 = B1; // Overflow Interrupt Enable
TCNT1 = 0x0000; // Reset the conuter - 15.11.4 TCNT1H and TCNT1L – Timer/Counter1 TCNT1 = 0x0000; // Reset the counter
// Now TCNT1 wraps around 244.140625 times per second and can count maximum of 4.095 ms durations
} sei(); } sei();


cli(); { cli(); {
// Set Timer2 to X MHz // Set Timer2 to ~15 kHz
TCCR2A = 0; // Normal mode, no PWM TCCR2A = 0; // Normal mode, no PWM
TCCR2B = B00000111; // 16000000 Hz / 1024 = 15625 Hz / 256 = ~61 Hz overflow TCCR2B = B00000111; // 16000000 Hz / 1024 = 15625 Hz / 256 = ~61 Hz overflow
TIMSK2 = B00000001; // Overflow Interrupt Enable TIMSK2 = B00000001; // Overflow Interrupt Enable
TCNT2 = 0x00; // Reset the counter TCNT2 = 0x00; // Reset the counter
} sei(); } sei();


// Set I2C for (uint8_t i = 0; i < GaugeCount; ++i) {
Wire.begin(I2CSlaveAddress); // Join i2c bus with address #I2C_SLAVE_ADDR gaugeValue[i] = 13*i;
Wire.onReceive(receiveEvent); // Register event handler }
digitalWrite(A4, HIGH); // Enable the pull-up for SDA
digitalWrite(A5, HIGH); // Enable the pull-up for SCL


// Generate initial values with difference phase if (I2CMode) {
if (demoMode) { // Set I2C
for (uint8_t i = 0; i < GaugeCount; ++i) { Wire.begin(I2CSlaveAddress); // Join i2c bus with address #I2C_SLAVE_ADDR
gaugeValue[i] = 13*i; Wire.onReceive(receiveEvent); // Register event handler
} digitalWrite(A4, HIGH); // Enable the pull-up for SDA
digitalWrite(A5, HIGH); // Enable the pull-up for SCL
} }


// Enable driving pins // Enable driving pins
Expand All @@ -111,14 +117,14 @@ void setup() {


// Precalculate sine table // Precalculate sine table
for (uint16_t i = 0; i < 256; ++i) { for (uint16_t i = 0; i < 256; ++i) {
const float scalehalf = float(PwmPulseWidth[PwmFrequencySelector] - 1)*0.5f; // 255 => 127.5, 4095 => 2047.5 const float scalehalf = float(PwmPulseWidth - 1)*0.5f; // 255 => 127.5, 4095 => 2047.5
float sinehalf = sin(2*M_PI * ((float)i)/256); float sinehalf = sin(2*M_PI * ((float)i)/256);
sin256Table[i] = (int16_t)(scalehalf * (1.0f*sinehalf) + scalehalf + 0.5); sin256Table[i] = (int16_t)(scalehalf * (1.0f*sinehalf) + scalehalf + 0.5);
} }




// Status ok // Status ok
digitalWrite(13, HIGH); digitalWrite(13, LOW);
} }


// TODO: Check that we have at least two bytes before going on and supposing first one is register... // TODO: Check that we have at least two bytes before going on and supposing first one is register...
Expand All @@ -141,39 +147,56 @@ void receiveEvent(int howMany)
} }




inline bool sinPwm(uint16_t pwmTime, uint8_t angle) {
return sin256Table[uint8_t(angle + 0)] > pwmTime;
}

inline bool cosPwm(uint16_t pwmTime, uint8_t angle) {
return sin256Table[uint8_t(angle + 64)] > pwmTime;
}



void loop() { void loop() {


cli(); { cli(); {
// Read current time // Read current time
uint16_t pwmTime = TCNT1 % PwmPulseWidth[PwmFrequencySelector]; uint16_t pwmTime = getTickCount32() & (PwmPulseWidth*2 - 1);
if (pwmTime > (PwmPulseWidth - 1)) { pwmTime = PwmPulseWidth*2 - pwmTime; } // Phase corrected PWM


uint8_t portBitsD = PORTD & ~PortMaskD; uint8_t portBitsD = PORTD & ~PortMaskD;
uint8_t portBitsC = PORTC & ~PortMaskC; uint8_t portBitsC = PORTC & ~PortMaskC;
uint8_t portBitsB = PORTB & ~PortMaskB; uint8_t portBitsB = PORTB & ~PortMaskB;


uint8_t gauge = 0; uint8_t gauge = 0;


// Generate PWM-signals with different phase shift on each gauges // Port D
for (uint8_t i = 0; i < GaugeCountPortD; ++i) { if (I2CMode) {
uint8_t angle = gaugeValue[gauge++]; if (sinPwm(pwmTime, gaugeValue[gauge ])) { portBitsD |= B00000001; }
if (sin256Table[uint8_t(angle + 0)] < pwmTime) { portBitsD |= GaugePinSinPortD[i]; } if (cosPwm(pwmTime, gaugeValue[gauge++])) { portBitsD |= B00000010; }
if (sin256Table[uint8_t(angle + 64)] < pwmTime) { portBitsD |= GaugePinCosPortD[i]; }
} }

if (sinPwm(pwmTime, gaugeValue[gauge ])) { portBitsD |= B00000100; }
for (uint8_t i = 0; i < GaugeCountPortC; ++i) { if (cosPwm(pwmTime, gaugeValue[gauge++])) { portBitsD |= B00001000; }
uint8_t angle = gaugeValue[gauge++]; if (sinPwm(pwmTime, gaugeValue[gauge ])) { portBitsD |= B00010000; }
if (sin256Table[uint8_t(angle + 0)] < pwmTime) { portBitsC |= GaugePinSinPortC[i]; } if (cosPwm(pwmTime, gaugeValue[gauge++])) { portBitsD |= B00100000; }
if (sin256Table[uint8_t(angle + 64)] < pwmTime) { portBitsC |= GaugePinCosPortC[i]; } if (sinPwm(pwmTime, gaugeValue[gauge ])) { portBitsD |= B01000000; }
if (cosPwm(pwmTime, gaugeValue[gauge++])) { portBitsD |= B10000000; }

// Port C
if (sinPwm(pwmTime, gaugeValue[gauge ])) { portBitsC |= B00000001; }
if (cosPwm(pwmTime, gaugeValue[gauge++])) { portBitsC |= B00000010; }
if (sinPwm(pwmTime, gaugeValue[gauge ])) { portBitsC |= B00000100; }
if (cosPwm(pwmTime, gaugeValue[gauge++])) { portBitsC |= B00001000; }

// Port B
if (sinPwm(pwmTime, gaugeValue[gauge ])) { portBitsB |= B00000001; }
if (cosPwm(pwmTime, gaugeValue[gauge++])) { portBitsB |= B00000010; }
if (sinPwm(pwmTime, gaugeValue[gauge ])) { portBitsB |= B00000100; }
if (cosPwm(pwmTime, gaugeValue[gauge++])) { portBitsB |= B00001000; }
if (!I2CMode) {
if (sinPwm(pwmTime, gaugeValue[gauge ])) { portBitsB |= B00010000; }
if (cosPwm(pwmTime, gaugeValue[gauge++])) { portBitsB |= B00100000; }
} }


for (uint8_t i = 0; i < GaugeCountPortB; ++i) {
uint8_t angle = gaugeValue[gauge++];
if (sin256Table[uint8_t(angle + 0)] < pwmTime) { portBitsB |= GaugePinSinPortB[i]; }
if (sin256Table[uint8_t(angle + 64)] < pwmTime) { portBitsB |= GaugePinCosPortB[i]; }
}


// Write patterns // Write patterns
PORTD = portBitsD; PORTD = portBitsD;
PORTC = portBitsC; PORTC = portBitsC;
Expand Down

0 comments on commit e672cbc

Please sign in to comment.