I am using a teensy 4.1 microcontroller with Emax BLheli125 Oneshot ESC for drones, is my code right for Emax BLheli125 Oneshot ESC? My ESCs not run expected at higher loops above 1Khz, please help me to implement the right code. thanks.
#include <Arduino.h>
class ESC
{
private:
/* data */
public:
int pin_m1;
int pin_m2;
int pin_m3;
int pin_m4;
int wentLow;
int32_t pulseStart, timer;
int flagM1;
int flagM2;
int flagM3;
int flagM4;
int MOTOR_IDEL_SPEED = (int)1200 / 8;
ESC(int pinM1=3, int pinM2=4, int pinM3=5, int pinM4=6);
void attachMototrs();
void commandMotors(int m1_cmd, int m2_cmd, int m3_cmd, int m4_cmd);
};
ESC::ESC(int pinM1, int pinM2, int pinM3, int pinM4)
{
this->pin_m1 = pinM1;
this->pin_m2 = pinM2;
this->pin_m3 = pinM3;
this->pin_m4 = pinM4;
}
void ESC::attachMototrs()
{
pinMode(this->pin_m1, OUTPUT);
pinMode(this->pin_m2, OUTPUT);
pinMode(this->pin_m3, OUTPUT);
pinMode(this->pin_m4, OUTPUT);
}
void ESC::commandMotors(int M1_cmd, int M2_cmd, int M3_cmd, int M4_cmd)
{
/*
RADIO FREQUENCY ARE 1000 TO 2000 AND MOTOR RUN AT 125 TO 250 SO WE NEED TO RADIO SIGNAL TO FECTOR OF 8.
EX. MOTOR 1 = CHANNEL 3 / 8;
MOTOR 1 = FRONT TIGHT
MOTOR 2 = BOTTOM RIGHT
MOTOR 3 = BOTTOM LEFT
MOTOR 4 = FRONT LEFT
*/
wentLow = 0;
flagM1 = 0;
flagM2 = 0;
flagM3 = 0;
flagM4 = 0;
// Write all motor pins high
digitalWrite(this->pin_m1, HIGH);
digitalWrite(this->pin_m2, HIGH);
digitalWrite(this->pin_m3, HIGH);
digitalWrite(this->pin_m4, HIGH);
pulseStart = micros();
// Write each motor pin low as correct pulse length is reached
while (wentLow < 4)
{ // keep going until final (4th) pulse is finished, then done
timer = micros();
if ((M1_cmd <= timer - pulseStart) && (flagM1 == 0))
{
digitalWrite(this->pin_m1, LOW);
wentLow = wentLow + 1;
flagM1 = 1;
}
if ((M2_cmd <= timer - pulseStart) && (flagM2 == 0))
{
digitalWrite(this->pin_m2, LOW);
wentLow = wentLow + 1;
flagM2 = 1;
}
if ((M3_cmd <= timer - pulseStart) && (flagM3 == 0))
{
digitalWrite(this->pin_m3, LOW);
wentLow = wentLow + 1;
flagM3 = 1;
}
if ((M4_cmd <= timer - pulseStart) && (flagM4 == 0))
{
digitalWrite(this->pin_m4, LOW);
wentLow = wentLow + 1;
flagM4 = 1;
}
}
}
