/*
This program tests the initialization of complementary output.
FlexTimer0 hardware is configured by directly writing to the registers
CH2 and CH3 of FTM0 are used, corresponding to pins 9 and 10 of the Teensy3.0
PWM Frequency can be set using the #define.
Dead-time is set for 1uS
*/
const int ledPin = 13;
const int buttonPin = 27;
const int NOT_RESET_AND_ENABLE = 8;
const int PHASE1_PWM_H = 9; // FTM0 - CH2
const int PHASE1_PWM_L = 10; // FTM0 - CH3
const int PHASE2_PWM_H = 20; // FTM0 - CH2
const int PHASE2_PWM_L = 21; // FTM0 - CH3
const int PHASE3_PWM_H = 22; // FTM0 - CH2
const int PHASE3_PWM_L = 23;
static int commStep = 0;
// Change these to adjust PWM
#define PWM_FREQ 20000 // PWM frequency [Hz]
#define PWM_AMPLITUDE 1500
#define TPM_C 60000000 // core clock, for calculation only
#define MODULO (TPM_C / PWM_FREQ) // calculation the modulo for FTM0
#define DEBUG_LOOP_SPEED_MICROSECONDS 5000//5000
int PWMvalue = 0;
bool countDir = 1;
int buttonPressed = 0;
int bounceTimer = 0;
unsigned long previousMicros = 0;
unsigned long pulseTimer = 0;
unsigned long lastPressTime = 0;
void setup()
{
pinMode(ledPin, OUTPUT);
pinMode(buttonPin, INPUT_PULLUP);
pinMode(NOT_RESET_AND_ENABLE, OUTPUT);
/*
pinMode(PHASE1_PWM_H, OUTPUT);
pinMode(PHASE1_PWM_L, OUTPUT);
pinMode(PHASE2_PWM_H, OUTPUT);
pinMode(PHASE2_PWM_L, OUTPUT);
pinMode(PHASE3_PWM_H, OUTPUT);
pinMode(PHASE3_PWM_L, OUTPUT);*/
Serial.begin(9600);
digitalWriteFast(NOT_RESET_AND_ENABLE, LOW);
init_FTM0();
}
void loop()
{
unsigned long lastDebugLoopTime = micros();
while(true)
{
unsigned long debugLoopDiff = micros() - lastDebugLoopTime;
//FTM0_C3V = PWMvalue;
FTM0_SYNC |= 0x80;
if (debugLoopDiff >= DEBUG_LOOP_SPEED_MICROSECONDS)
{
//FTM0_SYNC |= 0x80;
debugLoop();
lastDebugLoopTime = micros();
}
}
}
void init_FTM0()
{
FTM0_POL = 0; // Positive Polarity
FTM0_OUTMASK = 0b11111111; // Use mask to disable outputs
FTM0_SC = 0x08; // set system clock as source for FTM0
FTM0_MOD = MODULO; // Period register
FTM0_CNTIN = 0; // Counter initial value
//FTM0_COMBINE = 0x00003300; // COMBINE=1, COMP=1, DTEN=1, SYNCEN=1
FTM0_COMBINE = 0b00110011001100110011001100110011;
//FTM0_COMBINE = 0b11001100110011001100110011001100;
FTM0_MODE = 0x01; // Enable FTM0
FTM0_SYNC = 0x02; // PWM sync @ max loading point enable
FTM0_DEADTIME = 0x80; // DeadTimer prescale systemClk/4
FTM0_DEADTIME |= 12; // 1uS DeadTime, max of 63 counts of 48Mhz clock
/*
FTM0_C0V = 0; // Combine mode, pulse-width controlled by...
FTM0_C1V = 0;
FTM0_C2V = 0; // Combine mode, pulse-width controlled by...
FTM0_C3V = 0; // odd channel.
FTM0_C4V = 0;
FTM0_C5V = 0;
FTM0_C6V = 0;
FTM0_C7V = 0;*/
FTM0_SYNC |= 0x80; // set PWM value
FTM0_C0SC = 0x28; // PWM output, edge aligned, positive signal
FTM0_C1SC = 0x28; // PWM output, edge aligned, positive signal
FTM0_C2SC = 0x28; // PWM output, edge aligned, positive signal
FTM0_C3SC = 0x28; // PWM output, edge aligned, positive signal
FTM0_C4SC = 0x28; // PWM output, edge aligned, positive signal
FTM0_C5SC = 0x28; // PWM output, edge aligned, positive signal
FTM0_C6SC = 0x28; // PWM output, edge aligned, positive signal
FTM0_C7SC = 0x28; // PWM output, edge aligned, positive signal
CORE_PIN5_CONFIG = PORT_PCR_MUX(4) | PORT_PCR_DSE | PORT_PCR_SRE; //config teensy output port pins
CORE_PIN6_CONFIG = PORT_PCR_MUX(4) | PORT_PCR_DSE | PORT_PCR_SRE; //config teensy output port pins
CORE_PIN9_CONFIG = PORT_PCR_MUX(4) | PORT_PCR_DSE | PORT_PCR_SRE; //config teensy output port pins
CORE_PIN10_CONFIG = PORT_PCR_MUX(4) | PORT_PCR_DSE | PORT_PCR_SRE; //config teensy output port pins
CORE_PIN20_CONFIG = PORT_PCR_MUX(4) | PORT_PCR_DSE | PORT_PCR_SRE; //config teensy output port pins
CORE_PIN21_CONFIG = PORT_PCR_MUX(4) | PORT_PCR_DSE | PORT_PCR_SRE; //config teensy output port pins
//CORE_PIN22_CONFIG = PORT_PCR_MUX(4) | PORT_PCR_DSE | PORT_PCR_SRE; //config teensy output port pins
//CORE_PIN23_CONFIG = PORT_PCR_MUX(4) | PORT_PCR_DSE | PORT_PCR_SRE; //config teensy output port pins
FTM0_OUTMASK = 0b00000000; // Turn on PWM all channels
}
void debugLoop()
{
// Read the user button
buttonPressed = !digitalRead(buttonPin);
if(buttonPressed)
{
digitalWriteFast(ledPin, HIGH);
digitalWriteFast(NOT_RESET_AND_ENABLE, HIGH);
//PWMvalue = PWM_AMPLITUDE;
commStep++;
if(commStep > 5) commStep = 0;
if(commStep == 0)
{
//PWMvalue = PWM_AMPLITUDE;
//FTM0_OUTMASK = 0b11001111; //channel 2, 3 PWM...channel
//FTM0_C2V = PWM_AMPLITUDE;
FTM0_C3V = PWM_AMPLITUDE;
//FTM0_C4V = 0;
FTM0_C5V = 0;
//FTM0_C0V = 0;
FTM0_C1V = 0;
FTM0_C7V = 0;
}
else if(commStep == 1)
{
//PWMvalue = 222;
//FTM0_OUTMASK = 0b11000011;
//FTM0_C2V = PWM_AMPLITUDE;
FTM0_C3V = PWM_AMPLITUDE/2;
//FTM0_C4V = 4095;
FTM0_C5V = PWM_AMPLITUDE/2;
//FTM0_C6V = 0;
FTM0_C1V = 0;
FTM0_C7V = 0;
}
else if(commStep == 2)
{
//FTM0_OUTMASK = 0b11110011; // Turns on PWM output
//FTM0_C2V = 0;
FTM0_C3V = 0;
//FTM0_C4V = 4095;
FTM0_C5V = PWM_AMPLITUDE;
//FTM0_C6V = 0;
FTM0_C1V = 0;
FTM0_C7V = 0;
}
else if(commStep == 3)
{
//FTM0_OUTMASK = 0b11110000; // Turns on PWM output
//FTM0_C2V = 0;
FTM0_C3V = 0;
//FTM0_C4V = 4095;
FTM0_C5V = PWM_AMPLITUDE/2;
//FTM0_C6V = 4095;
FTM0_C1V = PWM_AMPLITUDE/2;
FTM0_C7V = PWM_AMPLITUDE/2;
}
else if(commStep == 4)
{
//FTM0_OUTMASK = 0b11111100; // Turns on PWM output
//FTM0_C2V = 0;
FTM0_C3V = 0;
//FTM0_C4V = 0;
FTM0_C5V = 0;
//FTM0_C6V = 4095;
FTM0_C1V = PWM_AMPLITUDE;
FTM0_C7V = PWM_AMPLITUDE;
}
else if(commStep == 5)
{
//FTM0_OUTMASK = 0b11001100; // Turns on PWM output
//FTM0_C2V = 0;
FTM0_C3V = PWM_AMPLITUDE/2;
//FTM0_C4V = 0;
FTM0_C5V = 0;
//FTM0_C6V = 4095;
FTM0_C1V = PWM_AMPLITUDE/2;
FTM0_C7V = PWM_AMPLITUDE/2;
}
}
else
{
digitalWriteFast(ledPin, LOW);
digitalWriteFast(NOT_RESET_AND_ENABLE, LOW);
//PWMvalue = 0;
//FTM0_C2V = MODULO/2;
FTM0_C3V = 0;
//FTM0_C4V = MODULO/2;
FTM0_C5V = 0;
//FTM0_C6V = MODULO/2;
FTM0_C7V = 0;
FTM0_C1V = 0;
}
Serial.print(commStep);
Serial.print(",");
Serial.print(FTM0_C2V);
Serial.print(",");
Serial.print(FTM0_C3V);
Serial.print(",");
Serial.print(FTM0_C4V);
Serial.print(",");
Serial.print(FTM0_C5V);
Serial.print(",");
Serial.print(FTM0_C0V);
Serial.print(",");
Serial.println(FTM0_C1V);
}