ThoriumMoltenSalt
New member
Hello there. So I've started a project working on a bldc esc based upon Teensy 4.0.
I've been trying to configure the analog comparators as I have a bemf sensing circuit which measures the voltages of the three phases compared to a common virtual neutral.
In order to switch at the correct time, I will be using the comparator to compare a given phase with the common virtual neutral. Zero-crossings need to be detected on either rising or falling edges depending on the state of the commutation of the motor.
So the important details is I have 4 pins, 21 (PhaseA), 22 (PhaseB), 23 (PhaseC) and 18 (common virtual neutral to all phases), then i need to configure 3 analog comparators such that their +ve side is either phase A,B or C and their -ve side is the common virtual neutral.
I need zero crossings to trigger events in the teensy such that I can commute to the next state.
Moreover to avoid electrical noise I have attempted to enable sample mode on the comparators, on trigger signal 4 from the pwm modules driving the motors. AKA when the power is off at this point in the pwm cycle ctr.
However I've not managed to get a single successful interrupt with a teensy 4.0, even when disabling the sample mode thus allowing for samples based on clock cycles not synced to the pwm power cycle, something which has worked on a teensy 3.6 before that I have code for (included)
I was wondering if someone could give me a pointer as to where I am going wrong, as I am a little stuck with what to try next.
Teensy 3.6 code:
Teensy 4.0 code:
I've been trying to configure the analog comparators as I have a bemf sensing circuit which measures the voltages of the three phases compared to a common virtual neutral.
In order to switch at the correct time, I will be using the comparator to compare a given phase with the common virtual neutral. Zero-crossings need to be detected on either rising or falling edges depending on the state of the commutation of the motor.
So the important details is I have 4 pins, 21 (PhaseA), 22 (PhaseB), 23 (PhaseC) and 18 (common virtual neutral to all phases), then i need to configure 3 analog comparators such that their +ve side is either phase A,B or C and their -ve side is the common virtual neutral.
I need zero crossings to trigger events in the teensy such that I can commute to the next state.
Moreover to avoid electrical noise I have attempted to enable sample mode on the comparators, on trigger signal 4 from the pwm modules driving the motors. AKA when the power is off at this point in the pwm cycle ctr.
However I've not managed to get a single successful interrupt with a teensy 4.0, even when disabling the sample mode thus allowing for samples based on clock cycles not synced to the pwm power cycle, something which has worked on a teensy 3.6 before that I have code for (included)
I was wondering if someone could give me a pointer as to where I am going wrong, as I am a little stuck with what to try next.
Teensy 3.6 code:
Code:
#define A_IN 2 // A High/low switch
#define A_SD 6 // A PWM
#define B_IN 7 // B High/low switch
#define B_SD 5// B PWM
#define C_IN 8 // C High/low switch
#define C_SD 10// C PWM
#define PWM_FREQUENCY 20000 // 20kHz
#define STARTUP_PWM 10
#define nBLDCCycles 11 // 22 poles
byte bldc_step = 0;
int motor_step = 0;
const byte motor_step_mod = nBLDCCycles * 6;
int setPWMDuty(byte duty) {
analogWrite(A_SD, duty);
analogWrite(B_SD, duty);
analogWrite(C_SD, duty);
return duty;
}
void setup() {
// put your setup code here, to run once:
pinMode(A_IN, OUTPUT);
pinMode(A_SD, OUTPUT);
pinMode(B_IN, OUTPUT);
pinMode(B_SD, OUTPUT);
pinMode(C_IN, OUTPUT);
pinMode(C_SD, OUTPUT);
digitalWriteFast(A_IN, LOW);
digitalWriteFast(B_IN, LOW);
digitalWriteFast(C_IN, LOW);
analogWriteFrequency(A_SD, PWM_FREQUENCY);
analogWriteFrequency(B_SD, PWM_FREQUENCY);
analogWriteFrequency(C_SD, PWM_FREQUENCY);
// setPWMDuty(STARTUP_PWM);
}
void setupComparators() {
SIM_SCGC4 |= SIM_SCGC4_CMP;
// CMPx_CR0
// 0 - [x - x - x] - 0 - 0 - [x - x]
// 0 - [FILTER_CNT] - 0 - 0 -[HYSTCTR]
CMP0_CR0 = 0B00000000; // no filtering 5mV hysteresis
CMP1_CR0 = 0B00000000; // no filtering 5mV hysteresis
CMP2_CR0 = 0B00000000; // no filtering 5mV hysteresis
// DACCR - disable
CMP0_DACCR = 0x00;
CMP1_DACCR = 0x00;
CMP2_DACCR = 0x00;
// CMPx_SCR
// 0 - [DMA] - 0 - [RISING INTERUPT] - [FALLING INTERUPT] - [RISE FLAG] - [FALL FLAG] - [ANALOG COMP OUTPUT]
CMP0_SCR = 0B00000000; // set to known state
CMP1_SCR = 0B00000000;
CMP2_SCR = 0B00000000;
// (CMPx_FPR)
// Specifies the sampling period, in bus clock cycles, of the comparator output filter, when CR1[SE]=0.
// Setting FILT_PER to 0x0 disables the filter.
CMP0_FPR = 0;
CMP1_FPR = 0;
CMP2_FPR = 0;
CMP0_MUXCR = 0B00000001; //set + to pin 11 (IN0) - to pin 12 (IN1) [phaseA - red]
CMP1_MUXCR = 0B00000001; //set + to pin 23 (IN0) - to pin 9 (IN1) [phaseB - yellow] WAS 18
CMP2_MUXCR = 0B00000001; //set + to pin 3 (IN0) - to pin 4 (IN1) [phaseC - black]
// CMPx_CR1
// x - x - x - x - x - x - x - x
// [SE] - [WE] - [TM] - [PMODE] - [INVERT] - [COS] - [OPE] - [EN]
CMP0_CR1 = 0B00010001; //set high speed and power on, window off
CMP1_CR1 = 0B00010001;
CMP2_CR1 = 0B00010001;
}
void AH_BL() {
// turn on PWM for B
analogWrite(B_SD, STARTUP_PWM);
digitalWriteFast(C_IN, LOW);
// turn on A_IN
digitalWriteFast(A_IN, HIGH);
}
void AH_CL() {
// keep A_IN on
// turn on PWM for C
digitalWriteFast(B_SD, LOW);
analogWrite(C_SD, STARTUP_PWM);
}
void BH_CL() {
// keep PWM on C on
// turn B_IN on
digitalWriteFast(A_IN, LOW);
digitalWriteFast(B_IN, HIGH);
}
void BH_AL() {
// keep B_IN on
// turn pwn for A
digitalWriteFast(C_SD, LOW);
analogWrite(A_SD, STARTUP_PWM);
}
void CH_AL() {
// turn C_IN on
digitalWriteFast(B_IN, LOW);
digitalWriteFast(C_IN, HIGH);
}
void CH_BL() {
// keep c_in high
// turn pwm on for B
digitalWriteFast(A_SD, LOW);
analogWrite(B_SD, STARTUP_PWM);
}
void BEMF_A_RISING() {
// enable a rising
CMP0_SCR |= 0B00010000;
// CMP0_CR1 |= 0x40; //set windowing on
}
void BEMF_A_FALLING() {
// enabling a falling
CMP0_SCR |= 0B00001000;
// CMP0_CR1 |= 0x40; //set windowing on
}
void BEMF_B_RISING() {
// enable b rising
CMP1_SCR |= 0B00010000;
// CMP1_CR1 |= 0x40; //set windowing on
}
void BEMF_B_FALLING() {
// enable b falling
CMP1_SCR |= 0B00001000;
// CMP1_CR1 |= 0x40; //set windowing on
}
void BEMF_C_RISING() {
// enable c rising
CMP2_SCR |= 0B00010000;
// CMP2_CR1 |= 0x40; //set windowing on
}
void BEMF_C_FALLING() {
// enable c falling
CMP2_SCR |= 0B00001000;
// CMP2_CR1 |= 0x40; //set windowing on
}
void bldc_move() { // BLDC motor commutation function
// Serial.println(bldc_step);
switch (bldc_step) {
case 0:
AH_BL();
BEMF_C_RISING();
break;
case 1:
AH_CL();
BEMF_B_FALLING();
break;
case 2:
BH_CL();
BEMF_A_RISING();
break;
case 3:
BH_AL();
BEMF_C_FALLING();
break;
case 4:
CH_AL();
BEMF_B_RISING();
break;
case 5:
CH_BL();
BEMF_A_FALLING();
break;
}
bldc_step++;
bldc_step %= 6;
motor_step++;
motor_step %= motor_step_mod;
}
bool startupMode = true;
void startup() {
int i, x = 0;
while (x < 1) {
i = 15000;
// Motor start
while (i > 20) {
if (startupMode == false) {
return;
}
delayMicroseconds(i);
bldc_move();
i = i - 20;
}
x++;
}
}
void loop() {
if (startupMode == true) {
setupComparators();
startup();
Serial.println("done startup");
NVIC_ENABLE_IRQ(IRQ_CMP0);
NVIC_ENABLE_IRQ(IRQ_CMP1);
NVIC_ENABLE_IRQ(IRQ_CMP2);
}
}
// 0 - [DMA] - 0 - [RISING INTERUPT] - [FALLING INTERUPT] - [RISE FLAG] - [FALL FLAG] - [ANALOG COMP OUTPUT]
auto fallingMask = 0B00000010; // falling flag and interrupt
auto risingMask = 0B00000100; // rising flag and interrupt
auto debounceDistance = 35; // ;
void cmp0_isr(void) {
startupMode = false;
// debounce
for (int i = 0; i < debounceDistance; i++) {
if (bldc_step & 1) {
// odd means falling
if ((CMP0_SCR & fallingMask) != fallingMask) {
i -= 1;
}
}
else {
//rising
if ((CMP0_SCR & risingMask) != risingMask) {
i -= 1;
}
}
}
Serial.print(0);Serial.print("\t");
CMP0_SCR &= 0x00; // turn off A falling or rising
// CMP0_CR1 &= 0xBF; // turn off windowing
bldc_move();
}
void cmp1_isr(void) {
startupMode = false;
// debounce
for (int i = 0; i < debounceDistance; i++) {
if (bldc_step & 1) {
// odd means falling
if ((CMP1_SCR & fallingMask) != fallingMask) {
i -= 1;
}
}
else {
//rising
if ((CMP1_SCR & risingMask) != risingMask) {
i -= 1;
}
}
// clear flag
}
Serial.print(1);Serial.print("\t");
CMP1_SCR &= 0x00;// turn off B falling or rising
// CMP1_CR1 &= 0xBF; // turn off windowing
bldc_move();
}
void cmp2_isr(void) {
startupMode = false;
// debounce
for (int i = 0; i < debounceDistance; i++) {
if (bldc_step & 1) {
// odd means falling
if ((CMP2_SCR & fallingMask) != fallingMask) {
i -= 1;
}
}
else {
//rising
if ((CMP2_SCR & risingMask) != risingMask) {
i -= 1;
}
}
}
Serial.print(2);Serial.print("\t");
// turn off C falling or rising
CMP2_SCR &= 0x00;
// CMP2_CR1 &= 0xBF; // turn off windowing
bldc_move();
}
Teensy 4.0 code:
Code:
// goal: 3 phase brushless motor commutation based on bemf cmp measurements.
#define DEBUG_MODE true
#include <Arduino.h>
#include "imxrt.h"
#include <ADC.h>
#define PI 3.1415926535897932384626433832795
#define A_IN 2 // A High/low switch
#define B_IN 9 // B High/low switch
#define C_IN 8 // C High/low switch
#define A_SD 1 // PWM A
#define B_SD 0 // PWM B
#define C_SD 7 // PWM C
#define LED_EN 13
/*
ACMP
21: PhaseA
22: PhaseB
23: PhaseC
18: Virtual Neutral
*/
#define PWM_FREQUENCY 2300// should be 20khz+ 1950 seems to atleast do something.
#define MIN_DUTY 50
#define MAX_DUTY 240
byte DUTY_TARGET = MIN_DUTY;
byte NEXT_DUTY = MIN_DUTY;
byte ELECTRICAL_STEP_CTR = 0;
bool anticlockwise = false;
boolean STARTUP_MODE = true;
// ACMP -----------------------------------------------------------------------------------------------------------
void initACMP() {
// 0 - [x - x - x] - 0 - 0 - [x - x]
// 0 - [FILTER_CNT] - 0 - 0 -[HYSTCTR]
CMP1_CR0 = 0B00000000; // no filtering 5mV hysteresis
CMP2_CR0 = 0B00000000; // no filtering 5mV hysteresis
CMP3_CR0 = 0B00000000; // no filtering 5mV hysteresis
// DACCR - disable
CMP1_DACCR = 0x00;
CMP2_DACCR = 0x00;
CMP3_DACCR = 0x00;
// CMPx_SCR
// 0 - [DMA] - 0 - [RISING INTERUPT] - [FALLING INTERUPT] - [RISE FLAG] - [FALL FLAG] - [ANALOG COMP OUTPUT]
CMP1_SCR = 0B00000000; // set to known state
CMP2_SCR = 0B00000000;
CMP3_SCR = 0B00000000;
// (CMPx_FPR)
// Specifies the sampling period, in bus clock cycles, of the comparator output filter, when CR1[SE]=0.
// Setting FILT_PER to 0x0 disables the filter.
CMP1_FPR = 0;
CMP2_FPR = 0;
CMP3_FPR = 0;
// MUX
// [0] - 0 - [PSEL] - [MSEL]
// 0 - 0 - [x - x - x] - [x - x - x]
/* MSEL / PSEL
000 IN0
001 IN1
010 IN2
011 IN3
100 IN4
101 IN5
110 IN6
111 IN7
*/
// so for negative [msel] we have the virtual netural pin 18 which is IN0 for each acmp
// x-x-x
// 0-0-0
// for positive [psel] we have phase A,B,C -> pin 21,22,23 ->
// pin 21 is acmp1_in6 110
CMP1_MUXCR = 0B00110000; //set + to pin 21 (IN6) - to pin 18 (IN0) [phaseA - red]
// pin 22 is acmp2_in5 101
CMP2_MUXCR = 0B00101000; //set + to pin 22 (IN5) - to pin 18 (IN0) [phaseB - yellow]
// pin 23 is acmp3_in5 101
CMP3_MUXCR = 0B00101000; //set + to pin 23 (IN5) - to pin 18 (IN0) [phaseC - black]
// CMPx_CR1
// 65.3.2 CMP Control Register 1 (CMPx_CR1)
// x - x - x - x - x - x - x - x
// [SE] - [WE] - [x] - [PMODE] - [INVERT] - [COS] - [OPE] - [EN]
/*
Try without sample mode... not working
CMP1_CR1 = 0B00010101; //set high speed and power on, sample mode disabled, coutA
CMP2_CR1 = 0B00010101;
CMP3_CR1 = 0B00010101;
*/
// try with sample enabled
CMP1_CR1 = 0B10010001; //set high speed and power on, sample mode enabled
CMP2_CR1 = 0B10010001;
CMP3_CR1 = 0B10010001;
}
void BEMF_A_RISING() {
// enable a rising
CMP1_SCR |= 0B00010000;
}
void BEMF_A_FALLING() {
// enabling a falling
CMP1_SCR |= 0B00001000;
}
void BEMF_B_RISING() {
// enable b rising
CMP2_SCR |= 0B00010000;
}
void BEMF_B_FALLING() {
// enable b falling
CMP2_SCR |= 0B00001000;
}
void BEMF_C_RISING() {
// enable c rising
CMP3_SCR |= 0B00010000;
}
void BEMF_C_FALLING() {
// enable c falling
CMP3_SCR |= 0B00001000;
}
// 0 - [DMA] - 0 - [RISING INTERUPT] - [FALLING INTERUPT] - [RISE FLAG] - [FALL FLAG] - [ANALOG COMP OUTPUT]
auto fallingMask = 0B00000010; // falling flag and interrupt
auto risingMask = 0B00000100; // rising flag and interrupt
auto debounceDistance = 1; // ;
void enableACMPInterrupts() {
attachInterruptVector(IRQ_ACMP1, &acmp1_isr );
attachInterruptVector(IRQ_ACMP2, &acmp2_isr );
attachInterruptVector(IRQ_ACMP3, &acmp3_isr );
NVIC_ENABLE_IRQ(IRQ_ACMP1);
NVIC_ENABLE_IRQ(IRQ_ACMP2);
NVIC_ENABLE_IRQ(IRQ_ACMP3);
}
void acmp1_isr(void) {
cli();
Serial.println("acmp1_isr");
sei();
// STARTUP_MODE = false;
// debounce
for (int i = 0; i < debounceDistance; i++) {
if (ELECTRICAL_STEP_CTR & 1) {
// odd means falling
if ((CMP1_SCR & fallingMask) != fallingMask) {
i -= 1;
}
}
else {
//rising
if ((CMP1_SCR & risingMask) != risingMask) {
i -= 1;
}
}
}
// Serial.print(0); Serial.print("\t");
CMP1_SCR &= 0x00; // turn off A falling or rising
}
void acmp2_isr(void) {
cli();
Serial.println("acmp2_isr");
sei();
// STARTUP_MODE = false;
// debounce
for (int i = 0; i < debounceDistance; i++) {
if (ELECTRICAL_STEP_CTR & 1) {
// odd means falling
if ((CMP2_SCR & fallingMask) != fallingMask) {
i -= 1;
}
}
else {
//rising
if ((CMP2_SCR & risingMask) != risingMask) {
i -= 1;
}
}
}
// Serial.print(1); Serial.print("\t");
CMP2_SCR &= 0x00;// turn off B falling or rising
}
void acmp3_isr(void) {
cli();
Serial.println("acmp3_isr");
sei();
// STARTUP_MODE = false;
// debounce
for (int i = 0; i < debounceDistance; i++) {
if (ELECTRICAL_STEP_CTR & 1) {
// odd means falling
if ((CMP3_SCR & fallingMask) != fallingMask) {
i -= 1;
}
}
else {
//rising
if ((CMP3_SCR & risingMask) != risingMask) {
i -= 1;
}
}
}
// Serial.print(2); Serial.print("\t");
// turn off C falling or rising
CMP3_SCR &= 0x00;
}
// END ACMP -----------------------------------------------------------------------------------------------------------
// XBAR ----------------------------------------------------------------------------------------------------------
void xbarConnect(unsigned int input, unsigned int output)
{
if (input >= 88)
return;
if (output >= 132)
return;
volatile uint16_t *xbar = &XBARA1_SEL0 + (output / 2);
uint16_t val = *xbar;
if (!(output & 1))
{
val = (val & 0xFF00) | input;
}
else
{
val = (val & 0x00FF) | (input << 8);
}
*xbar = val;
}
void xbarInit()
{
CCM_CCGR2 |= CCM_CCGR2_XBAR1(CCM_CCGR_ON); //turn clock on for xbara1
// cmp
xbarConnect(XBARA1_IN_FLEXPWM1_PWM1_OUT_TRIG0, XBARA1_OUT_ACMP1_SAMPLE);
xbarConnect(XBARA1_IN_FLEXPWM1_PWM2_OUT_TRIG0, XBARA1_OUT_ACMP2_SAMPLE);
xbarConnect(XBARA1_IN_FLEXPWM1_PWM4_OUT_TRIG0, XBARA1_OUT_ACMP3_SAMPLE);
}
// END XBAR ------------------------------------------------------------------------------------------------------
// MOTION ------------------------------------------------------------------------------------------------------
void AH_BL()
{
digitalWriteFast(C_IN, LOW);
digitalWriteFast(B_IN, LOW);
// turn on A_IN
digitalWriteFast(A_IN, HIGH);
digitalWriteFast(A_SD, LOW);
digitalWriteFast(C_SD, LOW);
// turn on / keep on PWM for B
analogWrite(B_SD, NEXT_DUTY);
} // A_IN HIGH B_SB PWM
void AH_CL()
{
digitalWriteFast(B_IN, LOW);
digitalWriteFast(C_IN, LOW);
// keep A_IN on
digitalWriteFast(A_IN, HIGH);
digitalWriteFast(A_SD, LOW);
digitalWriteFast(B_SD, LOW);
// turn on PWM for C
analogWrite(C_SD, NEXT_DUTY);
} // A_IN high C_SD PWM
void BH_CL()
{
digitalWriteFast(C_IN, LOW);
// turn B_IN on
digitalWriteFast(A_IN, LOW);
digitalWriteFast(B_IN, HIGH);
digitalWriteFast(A_SD, LOW);
digitalWriteFast(B_SD, LOW);
// keep PWM on for C
analogWrite(C_SD, NEXT_DUTY);
} // B_IN high C_SD PWM
void BH_AL()
{
digitalWriteFast(A_IN, LOW);
digitalWriteFast(C_IN, LOW);
// keep B_IN on
digitalWriteFast(B_IN, HIGH);
digitalWriteFast(B_SD, LOW);
digitalWriteFast(C_SD, LOW);
// turn pwn for A
analogWrite(A_SD, NEXT_DUTY);
} // B_IN high A_SD PWM
void CH_AL()
{
digitalWriteFast(A_IN, LOW);
// turn C_IN on
digitalWriteFast(B_IN, LOW);
digitalWriteFast(C_IN, HIGH);
digitalWriteFast(B_SD, LOW);
digitalWriteFast(C_SD, LOW);
// keep PWM on for A
analogWrite(A_SD, NEXT_DUTY);
} // C_IN high A_SD PWM
void CH_BL()
{
digitalWriteFast(A_IN, LOW);
digitalWriteFast(B_IN, LOW);
// keep c_in high
digitalWriteFast(C_IN, HIGH);
digitalWriteFast(C_SD, LOW);
// turn pwm on for B
digitalWriteFast(A_SD, LOW);
analogWrite(B_SD, NEXT_DUTY);
// C_IN HIGH B_SD PWM
}
void enforceCommutationState()
{
switch (ELECTRICAL_STEP_CTR)
{
case 0:
if (anticlockwise == false)
{
AH_BL(); // C RISING
BEMF_C_RISING();
}
else
{
CH_BL(); // A RISING
BEMF_A_RISING();
}
break;
case 1:
if (anticlockwise == false)
{
AH_CL(); // B FALLING
BEMF_B_FALLING();
}
else
{
CH_AL(); // B FALLING
BEMF_B_FALLING();
}
break;
case 2:
if (anticlockwise == false)
{
BH_CL(); // A RISING
BEMF_A_RISING();
}
else
{
BH_AL(); // C RISING
BEMF_C_RISING();
}
break;
case 3:
if (anticlockwise == false)
{
BH_AL(); // C FALLING
BEMF_C_FALLING();
}
else
{
BH_CL(); // A FALLING
BEMF_A_FALLING();
}
break;
case 4:
if (anticlockwise == false)
{
CH_AL(); // B RISING
BEMF_B_RISING();
}
else
{
AH_CL(); // B RISING
BEMF_B_RISING();
}
break;
case 5:
if (anticlockwise == false)
{
CH_BL(); // A FALLING
BEMF_A_FALLING();
}
else
{
AH_BL(); // C FALLING
BEMF_C_FALLING();
}
break;
}
}
// END MOTION -----------------------------------------------------------------------------------------------------
// PWM CTRL -----------------------------------------------------------------------------------------------------------
void pwmInit_1_0()
{ // this works ok pin 1
analogWriteFrequency(A_SD, PWM_FREQUENCY);
FLEXPWM1_SM0TCTRL = FLEXPWM_SMTCTRL_OUT_TRIG_EN(1 << 4);
}
// sync clocks of flexpwm1_SM0 and flexpwm1_SM1
/*
CLK_SEL
Clock Source Select
These read/write bits determine the source of the clock signal for this submodule.
00b - The IPBus clock is used as the clock for the local prescaler and counter.
01b - EXT_CLK is used as the clock for the local prescaler and counter.
10b - Submodule 0’s clock (AUX_CLK) is used as the source clock for the local prescaler and
counter. This setting should not be used in submodule 0 as it will force the clock to logic 0.
11b - reserved
*/
void pwmInit_1_1()
{ // this works ok pin 0
// analogWriteFrequency(B_SD, PWM_FREQUENCY);
FLEXPWM1_MCTRL |= FLEXPWM_MCTRL_CLDOK(0x0F); // Clear Load Okay LDOK(SM) -> no reload of PWM settings
FLEXPWM1_SM1CTRL2 = FLEXPWM_SMCTRL2_INDEP | FLEXPWM_SMCTRL2_CLK_SEL(2) | FLEXPWM_SMCTRL2_INIT_SEL(0); //A & B independant | sm0 chosen as clock
FLEXPWM1_MCTRL |= FLEXPWM_MCTRL_LDOK(0x0F); // Load Okay LDOK(SM) -> reload setting again
FLEXPWM1_SM1TCTRL = FLEXPWM_SMTCTRL_OUT_TRIG_EN(1 << 4);
}
void pwmInit_1_3()
{ // this works ok pin 0
// analogWriteFrequency(C_SD, PWM_FREQUENCY);
FLEXPWM1_MCTRL |= FLEXPWM_MCTRL_CLDOK(0x0F); // Clear Load Okay LDOK(SM) -> no reload of PWM settings
FLEXPWM1_SM3CTRL2 = FLEXPWM_SMCTRL2_INDEP | FLEXPWM_SMCTRL2_CLK_SEL(2) | FLEXPWM_SMCTRL2_INIT_SEL(0); //A & B independant | sm0 chosen as clock
FLEXPWM1_MCTRL |= FLEXPWM_MCTRL_LDOK(0x0F); // Load Okay LDOK(SM) -> reload setting again
FLEXPWM1_SM3TCTRL = FLEXPWM_SMTCTRL_OUT_TRIG_EN(1 << 4);
// | FLEXPWM_SMCTRL2_INIT_SEL(1)
}
void pwmInit_1_2()
{ // this is just here so in future i can use the event of this submodule to sync the adc or another motor
FLEXPWM1_MCTRL |= FLEXPWM_MCTRL_CLDOK(0x0F); // Clear Load Okay LDOK(SM) -> no reload of PWM settings
FLEXPWM1_SM2CTRL2 = FLEXPWM_SMCTRL2_INDEP | FLEXPWM_SMCTRL2_CLK_SEL(2) | FLEXPWM_SMCTRL2_INIT_SEL(0); //A & B independant | sm0 chosen as clock
FLEXPWM1_MCTRL |= FLEXPWM_MCTRL_LDOK(0x0F); // Load Okay LDOK(SM) -> reload setting again
FLEXPWM1_SM2TCTRL = FLEXPWM_SMTCTRL_OUT_TRIG_EN(1 << 4);
}
void pwmInit()
{
analogWriteRes(8);
pwmInit_1_0();
pwmInit_1_1();
pwmInit_1_2(); // just used for timing no pin out
pwmInit_1_3();
}
// END PWM CTRL -----------------------------------------------------------------------------------------------------------
void setup()
{
pinMode(A_IN, OUTPUT);
pinMode(B_IN, OUTPUT);
pinMode(C_IN, OUTPUT);
pinMode(A_SD, OUTPUT);
pinMode(B_SD, OUTPUT);
pinMode(C_SD, OUTPUT);
pinMode(LED_EN, OUTPUT);
DUTY_TARGET = MIN_DUTY;
NEXT_DUTY = MIN_DUTY;
pwmInit();
initACMP();
// force off everything
digitalWriteFast(A_IN, LOW);
digitalWriteFast(B_IN, LOW);
digitalWriteFast(C_IN, LOW);
analogWrite(A_SD, LOW);
analogWrite(B_SD, LOW);
analogWrite(C_SD, LOW);
STARTUP_MODE = true;
}
void loop()
{
delayMicroseconds(200);
if (STARTUP_MODE == true) {
delay(1000);
Serial.println("got past startup");
enableACMPInterrupts();
Serial.println("got past enableACMPInterrupts");
/* Try C rising
AH_BL(); // C RISING
Serial.println("got past AH_BL");
BEMF_C_RISING();
Serial.println("got past BEMF_C_RISING");
*/
// Try C falling:
BH_AL(); // C FALLING
BEMF_C_FALLING();
Serial.println("got past turn on pwm");
STARTUP_MODE = false;
}
}