From 241e8c04dbd4ee8ea3476b166d6e0684b1cd08e1 Mon Sep 17 00:00:00 2001 From: dekutree64 <54822734+dekutree64@users.noreply.github.com> Date: Sat, 13 Jan 2024 20:25:13 -0600 Subject: [PATCH 01/57] LinearHall improvements and move to main repository Changes compared to the original pull request in the drivers repository https://github.com/simplefoc/Arduino-FOC-drivers/pull/12 1. Added a version of init which turns the motor one revolution to find the center values of the sensors. 2. Moved the calls to analogRead into a weakly bound function ReadLinearHalls so it can be overridden with custom ADC code on platforms with poor analogRead performance. 3. Commented out the pinMode calls in init, which makes it possible to pass in ADC channel numbers for custom ReadLinearHalls to use without having to remap them every update. 4. Changed to use the much faster _atan2 function that was added to foc_utils recently. 5. Added examples. --- .../angle_control/angle_control.ino | 117 ++++++++++++++++++ .../voltage_control/voltage_control.ino | 96 ++++++++++++++ .../velocity_control/velocity_control.ino | 109 ++++++++++++++++ src/SimpleFOC.h | 1 + src/sensors/LinearHall.cpp | 109 ++++++++++++++++ src/sensors/LinearHall.h | 45 +++++++ 6 files changed, 477 insertions(+) create mode 100644 examples/motion_control/position_motion_control/linear_hall/angle_control/angle_control.ino create mode 100644 examples/motion_control/torque_control/linear_hall/voltage_control/voltage_control.ino create mode 100644 examples/motion_control/velocity_motion_control/linear_hall/velocity_control/velocity_control.ino create mode 100644 src/sensors/LinearHall.cpp create mode 100644 src/sensors/LinearHall.h diff --git a/examples/motion_control/position_motion_control/linear_hall/angle_control/angle_control.ino b/examples/motion_control/position_motion_control/linear_hall/angle_control/angle_control.ino new file mode 100644 index 00000000..8179008d --- /dev/null +++ b/examples/motion_control/position_motion_control/linear_hall/angle_control/angle_control.ino @@ -0,0 +1,117 @@ +/** + * + * Position/angle motion control example + * Steps: + * 1) Configure the motor and hall sensor + * 2) Run the code + * 3) Set the target angle (in radians) from serial terminal + */ +#include + +// BLDC motor & driver instance +BLDCMotor motor = BLDCMotor(11); +BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8); +// Stepper motor & driver instance +//StepperMotor motor = StepperMotor(50); +//StepperDriver4PWM driver = StepperDriver4PWM(9, 5, 10, 6, 8); + +// hall sensor instance +LinearHall sensor = LinearHall(A0, A1, 11); + +// angle set point variable +float target_angle = 0; +// instantiate the commander +Commander command = Commander(Serial); +void doTarget(char* cmd) { command.scalar(&target_angle, cmd); } + +void setup() { + + // driver config + // power supply voltage [V] + driver.voltage_power_supply = 12; + driver.init(); + // link the motor and the driver + motor.linkDriver(&driver); + + + // aligning voltage [V] + motor.voltage_sensor_align = 3; + // index search velocity [rad/s] + motor.velocity_index_search = 3; + + // set motion control loop to be used + motor.controller = MotionControlType::angle; + + // contoller configuration + // default parameters in defaults.h + + // velocity PI controller parameters + motor.PID_velocity.P = 0.2f; + motor.PID_velocity.I = 2; + motor.PID_velocity.D = 0; + // default voltage_power_supply + motor.voltage_limit = 6; + // jerk control using voltage voltage ramp + // default value is 300 volts per sec ~ 0.3V per millisecond + motor.PID_velocity.output_ramp = 1000; + + // velocity low pass filtering time constant + motor.LPF_velocity.Tf = 0.01f; + + // angle P controller + motor.P_angle.P = 20; + // maximal velocity of the position control + motor.velocity_limit = 4; + + + // use monitoring with serial + Serial.begin(115200); + // comment out if not needed + motor.useMonitoring(Serial); + + // initialize motor + motor.init(); + // initialize sensor hardware. This moves the motor to find the min/max sensor readings and + // averages them to get the center values. The motor can't move until motor.init is called, and + // motor.initFOC can't do its calibration until the sensor is intialized, so this must be done inbetween. + // You can then take the values printed to the serial monitor and pass them to sensor.init to + // avoid having to move the motor every time. In that case it doesn't matter whether sensor.init + // is called before or after motor.init. + sensor.init(&motor); + Serial.print("LinearHall centerA: "); + Serial.print(sensor.centerA); + Serial.print(", centerB: "); + Serial.println(sensor.centerB); + // link the motor to the sensor + motor.linkSensor(&sensor); + // align sensor and start FOC + motor.initFOC(); + + // add target command T + command.add('T', doTarget, "target angle"); + + Serial.println(F("Motor ready.")); + Serial.println(F("Set the target angle using serial terminal:")); + _delay(1000); +} + +void loop() { + // main FOC algorithm function + // the faster you run this function the better + // Arduino UNO loop ~1kHz + // Bluepill loop ~10kHz + motor.loopFOC(); + + // Motion control function + // velocity, position or voltage (defined in motor.controller) + // this function can be run at much lower frequency than loopFOC() function + // You can also use motor.move() and set the motor.target in the code + motor.move(target_angle); + + // function intended to be used with serial plotter to monitor motor variables + // significantly slowing the execution down!!!! + // motor.monitor(); + + // user communication + command.run(); +} diff --git a/examples/motion_control/torque_control/linear_hall/voltage_control/voltage_control.ino b/examples/motion_control/torque_control/linear_hall/voltage_control/voltage_control.ino new file mode 100644 index 00000000..e646849a --- /dev/null +++ b/examples/motion_control/torque_control/linear_hall/voltage_control/voltage_control.ino @@ -0,0 +1,96 @@ +/** + * + * Torque control example using voltage control loop. + * + * Most of the low-end BLDC driver boards doesn't have current measurement therefore SimpleFOC offers + * you a way to control motor torque by setting the voltage to the motor instead of the current. + * + * This makes the BLDC motor effectively a DC motor, and you can use it in a same way. + */ +#include + + +// BLDC motor & driver instance +BLDCMotor motor = BLDCMotor(11); +BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8); +// Stepper motor & driver instance +//StepperMotor motor = StepperMotor(50); +//StepperDriver4PWM driver = StepperDriver4PWM(9, 5, 10, 6, 8); + +// hall sensor instance +LinearHall sensor = LinearHall(A0, A1, 11); + + +// voltage set point variable +float target_voltage = 2; +// instantiate the commander +Commander command = Commander(Serial); +void doTarget(char* cmd) { command.scalar(&target_voltage, cmd); } + +void setup() { + + // driver config + // power supply voltage [V] + driver.voltage_power_supply = 12; + driver.init(); + // link driver + motor.linkDriver(&driver); + + // aligning voltage + motor.voltage_sensor_align = 3; + + // choose FOC modulation (optional) + motor.foc_modulation = FOCModulationType::SpaceVectorPWM; + + // set motion control loop to be used + motor.controller = MotionControlType::torque; + + // use monitoring with serial + Serial.begin(115200); + // comment out if not needed + motor.useMonitoring(Serial); + + // initialize motor + motor.init(); + // initialize sensor hardware. This moves the motor to find the min/max sensor readings and + // averages them to get the center values. The motor can't move until motor.init is called, and + // motor.initFOC can't do its calibration until the sensor is intialized, so this must be done inbetween. + // You can then take the values printed to the serial monitor and pass them to sensor.init to + // avoid having to move the motor every time. In that case it doesn't matter whether sensor.init + // is called before or after motor.init. + sensor.init(&motor); + Serial.print("LinearHall centerA: "); + Serial.print(sensor.centerA); + Serial.print(", centerB: "); + Serial.println(sensor.centerB); + // link the motor to the sensor + motor.linkSensor(&sensor); + // align sensor and start FOC + motor.initFOC(); + + // add target command T + command.add('T', doTarget, "target voltage"); + + Serial.println(F("Motor ready.")); + Serial.println(F("Set the target voltage using serial terminal:")); + _delay(1000); +} + + +void loop() { + + // main FOC algorithm function + // the faster you run this function the better + // Arduino UNO loop ~1kHz + // Bluepill loop ~10kHz + motor.loopFOC(); + + // Motion control function + // velocity, position or voltage (defined in motor.controller) + // this function can be run at much lower frequency than loopFOC() function + // You can also use motor.move() and set the motor.target in the code + motor.move(target_voltage); + + // user communication + command.run(); +} \ No newline at end of file diff --git a/examples/motion_control/velocity_motion_control/linear_hall/velocity_control/velocity_control.ino b/examples/motion_control/velocity_motion_control/linear_hall/velocity_control/velocity_control.ino new file mode 100644 index 00000000..870a0899 --- /dev/null +++ b/examples/motion_control/velocity_motion_control/linear_hall/velocity_control/velocity_control.ino @@ -0,0 +1,109 @@ +/** + * + * Velocity motion control example + * Steps: + * 1) Configure the motor and sensor + * 2) Run the code + * 3) Set the target velocity (in radians per second) from serial terminal + */ +#include + +// BLDC motor & driver instance +BLDCMotor motor = BLDCMotor(11); +BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8); +// Stepper motor & driver instance +//StepperMotor motor = StepperMotor(50); +//StepperDriver4PWM driver = StepperDriver4PWM(9, 5, 10, 6, 8); + +// hall sensor instance +LinearHall sensor = LinearHall(A0, A1, 11); + +// velocity set point variable +float target_velocity = 0; +// instantiate the commander +Commander command = Commander(Serial); +void doTarget(char* cmd) { command.scalar(&target_velocity, cmd); } + +void setup() { + + // driver config + // power supply voltage [V] + driver.voltage_power_supply = 12; + driver.init(); + // link the motor and the driver + motor.linkDriver(&driver); + + // aligning voltage [V] + motor.voltage_sensor_align = 3; + + // set motion control loop to be used + motor.controller = MotionControlType::velocity; + + // contoller configuration + // default parameters in defaults.h + + // velocity PI controller parameters + motor.PID_velocity.P = 0.2f; + motor.PID_velocity.I = 2; + motor.PID_velocity.D = 0; + // default voltage_power_supply + motor.voltage_limit = 6; + // jerk control using voltage voltage ramp + // default value is 300 volts per sec ~ 0.3V per millisecond + motor.PID_velocity.output_ramp = 1000; + + // velocity low pass filtering time constant + motor.LPF_velocity.Tf = 0.01f; + + // use monitoring with serial + Serial.begin(115200); + // comment out if not needed + motor.useMonitoring(Serial); + + // initialize motor + motor.init(); + // initialize sensor hardware. This moves the motor to find the min/max sensor readings and + // averages them to get the center values. The motor can't move until motor.init is called, and + // motor.initFOC can't do its calibration until the sensor is intialized, so this must be done inbetween. + // You can then take the values printed to the serial monitor and pass them to sensor.init to + // avoid having to move the motor every time. In that case it doesn't matter whether sensor.init + // is called before or after motor.init. + sensor.init(&motor); + Serial.print("LinearHall centerA: "); + Serial.print(sensor.centerA); + Serial.print(", centerB: "); + Serial.println(sensor.centerB); + // link the motor to the sensor + motor.linkSensor(&sensor); + // align sensor and start FOC + motor.initFOC(); + + // add target command T + command.add('T', doTarget, "target voltage"); + + Serial.println(F("Motor ready.")); + Serial.println(F("Set the target velocity using serial terminal:")); + _delay(1000); +} + + +void loop() { + // main FOC algorithm function + // the faster you run this function the better + // Arduino UNO loop ~1kHz + // Bluepill loop ~10kHz + motor.loopFOC(); + + // Motion control function + // velocity, position or voltage (defined in motor.controller) + // this function can be run at much lower frequency than loopFOC() function + // You can also use motor.move() and set the motor.target in the code + motor.move(target_velocity); + + // function intended to be used with serial plotter to monitor motor variables + // significantly slowing the execution down!!!! + // motor.monitor(); + + // user communication + command.run(); +} \ No newline at end of file diff --git a/src/SimpleFOC.h b/src/SimpleFOC.h index 4e6815e5..9e1c80c6 100644 --- a/src/SimpleFOC.h +++ b/src/SimpleFOC.h @@ -104,6 +104,7 @@ void loop() { #include "sensors/MagneticSensorAnalog.h" #include "sensors/MagneticSensorPWM.h" #include "sensors/HallSensor.h" +#include "sensors/LinearHall.h" #include "sensors/GenericSensor.h" #include "drivers/BLDCDriver3PWM.h" #include "drivers/BLDCDriver6PWM.h" diff --git a/src/sensors/LinearHall.cpp b/src/sensors/LinearHall.cpp new file mode 100644 index 00000000..36303aaa --- /dev/null +++ b/src/sensors/LinearHall.cpp @@ -0,0 +1,109 @@ +#include "LinearHall.h" + +// This function can be overridden with custom ADC code on platforms with poor analogRead performance. +__attribute__((weak)) void ReadLinearHalls(int hallA, int hallB, int *a, int *b) +{ + *a = analogRead(hallA); + *b = analogRead(hallB); +} + +LinearHall::LinearHall(int _hallA, int _hallB, int _pp){ + centerA = 512; + centerB = 512; + pinA = _hallA; + pinB = _hallB; + pp = _pp; +} + +float LinearHall::getSensorAngle() { + ReadLinearHalls(pinA, pinB, &lastA, &lastB); + //offset readings using center values, then compute angle + float reading = _atan2(lastA - centerA, lastB - centerB); + + //handle rollover logic between each electrical revolution of the motor + if (reading > prev_reading) { + if (reading - prev_reading >= PI) { + if (electrical_rev - 1 < 0) { + electrical_rev = pp - 1; + } else { + electrical_rev = electrical_rev - 1; + } + } + } else if (reading < prev_reading) { + if (prev_reading - reading >= PI) { + if (electrical_rev + 1 >= pp) { + electrical_rev = 0; + } else { + electrical_rev = electrical_rev + 1; + } + } + } + + //convert result from electrical angle and electrical revolution count to shaft angle in radians + float result = (reading + PI) / _2PI; + result = _2PI * (result + electrical_rev) / pp; + + //update previous reading for rollover handling + prev_reading = reading; + return result; +} + +void LinearHall::init(int _centerA, int _centerB) { + // Skip configuring the pins here because they normally default to input anyway, and + // this makes it possible to use ADC channel numbers instead of pin numbers when using + // custom implementation of ReadLinearHalls, to avoid having to remap them every update. + // If pins do need to be configured, it can be done by user code before calling init. + //pinMode(pinA, INPUT); + //pinMode(pinB, INPUT); + + centerA = _centerA; + centerB = _centerB; + + //establish initial reading for rollover handling + electrical_rev = 0; + ReadLinearHalls(pinA, pinB, &lastA, &lastB); + prev_reading = _atan2(lastA - centerA, lastB - centerB); +} + +void LinearHall::init(FOCMotor *motor) { + if (!motor->enabled) { + SIMPLEFOC_DEBUG("LinearHall::init failed. Call after motor.init, but before motor.initFOC."); + return; + } + + //pinMode(pinA, INPUT); + //pinMode(pinB, INPUT); + + int minA, maxA, minB, maxB; + + ReadLinearHalls(pinA, pinB, &lastA, &lastB); + minA = maxA = centerA = lastA; + minB = maxB = centerB = lastB; + + // move one mechanical revolution forward + for (int i = 0; i <= 2000; i++) + { + float angle = _3PI_2 + _2PI * i * pp / 2000.0f; + motor->setPhaseVoltage(motor->voltage_sensor_align, 0, angle); + + ReadLinearHalls(pinA, pinB, &lastA, &lastB); + + if (lastA < minA) + minA = lastA; + if (lastA > maxA) + maxA = lastA; + centerA = (minA + maxA) / 2; + + if (lastB < minB) + minB = lastB; + if (lastB > maxB) + maxB = lastB; + centerB = (minB + maxB) / 2; + + _delay(2); + } + + //establish initial reading for rollover handling + electrical_rev = 0; + prev_reading = _atan2(lastA - centerA, lastB - centerB); +} diff --git a/src/sensors/LinearHall.h b/src/sensors/LinearHall.h new file mode 100644 index 00000000..f05b7b2b --- /dev/null +++ b/src/sensors/LinearHall.h @@ -0,0 +1,45 @@ +#ifndef LINEAR_HALL_SENSOR_LIB_H +#define LINEAR_HALL_SENSOR_LIB_H + +#include + +class FOCMotor; +void ReadLinearHalls(int hallA, int hallB, int *a, int *b); + +/** + * This sensor class is for two linear hall effect sensors such as 49E, which are + * positioned 90 electrical degrees apart (if one is centered on a rotor magnet, + * the other is half way between rotor magnets). + * It can also be used for a single magnet mounted to the motor shaft (set pp to 1). + * + * For more information, see this forum thread and PDF + * https://community.simplefoc.com/t/40-cent-magnetic-angle-sensing-technique/1959 + * https://gist.github.com/nanoparticle/00030ea27c59649edbed84f0a957ebe1 + */ +class LinearHall: public Sensor{ + public: + LinearHall(int hallA, int hallB, int pp); + + void init(int centerA, int centerB); // Initialize without moving motor + void init(FOCMotor *motor); // Move motor to find center values + + // Get current shaft angle from the sensor hardware, and + // return it as a float in radians, in the range 0 to 2PI. + // - This method is pure virtual and must be implemented in subclasses. + // Calling this method directly does not update the base-class internal fields. + // Use update() when calling from outside code. + float getSensorAngle() override; + + int centerA; + int centerB; + int lastA, lastB; + + private: + int pinA; + int pinB; + int pp; + int electrical_rev; + float prev_reading; +}; + +#endif From aeb923824dd2366ca341f2cabcc1cabf063d814c Mon Sep 17 00:00:00 2001 From: dekutree64 <54822734+dekutree64@users.noreply.github.com> Date: Sat, 13 Jan 2024 20:40:04 -0600 Subject: [PATCH 02/57] Minor formatting/comment edits --- src/sensors/LinearHall.cpp | 5 +++-- src/sensors/LinearHall.h | 4 ++-- 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/src/sensors/LinearHall.cpp b/src/sensors/LinearHall.cpp index 36303aaa..96cfae3e 100644 --- a/src/sensors/LinearHall.cpp +++ b/src/sensors/LinearHall.cpp @@ -3,8 +3,8 @@ // This function can be overridden with custom ADC code on platforms with poor analogRead performance. __attribute__((weak)) void ReadLinearHalls(int hallA, int hallB, int *a, int *b) { - *a = analogRead(hallA); - *b = analogRead(hallB); + *a = analogRead(hallA); + *b = analogRead(hallB); } LinearHall::LinearHall(int _hallA, int _hallB, int _pp){ @@ -71,6 +71,7 @@ void LinearHall::init(FOCMotor *motor) { return; } + // See comment in other version of init for why these are commented out //pinMode(pinA, INPUT); //pinMode(pinB, INPUT); diff --git a/src/sensors/LinearHall.h b/src/sensors/LinearHall.h index f05b7b2b..d17035e4 100644 --- a/src/sensors/LinearHall.h +++ b/src/sensors/LinearHall.h @@ -3,7 +3,7 @@ #include -class FOCMotor; +// This function can be overridden with custom ADC code on platforms with poor analogRead performance. void ReadLinearHalls(int hallA, int hallB, int *a, int *b); /** @@ -21,7 +21,7 @@ class LinearHall: public Sensor{ LinearHall(int hallA, int hallB, int pp); void init(int centerA, int centerB); // Initialize without moving motor - void init(FOCMotor *motor); // Move motor to find center values + void init(class FOCMotor *motor); // Move motor to find center values // Get current shaft angle from the sensor hardware, and // return it as a float in radians, in the range 0 to 2PI. From d7d44887737f6dd949c2fc06bb7a153d9b026b36 Mon Sep 17 00:00:00 2001 From: Candas1 Date: Fri, 9 Feb 2024 13:00:50 +0100 Subject: [PATCH 03/57] optimize --- src/sensors/HallSensor.cpp | 29 +++++++++++++++++++---------- src/sensors/HallSensor.h | 1 + 2 files changed, 20 insertions(+), 10 deletions(-) diff --git a/src/sensors/HallSensor.cpp b/src/sensors/HallSensor.cpp index d9e5ec83..64c5e48c 100644 --- a/src/sensors/HallSensor.cpp +++ b/src/sensors/HallSensor.cpp @@ -42,22 +42,21 @@ void HallSensor::handleC() { * Updates the state and sector following an interrupt */ void HallSensor::updateState() { - long new_pulse_timestamp = _micros(); - int8_t new_hall_state = C_active + (B_active << 1) + (A_active << 2); // glitch avoidance #1 - sometimes we get an interrupt but pins haven't changed - if (new_hall_state == hall_state) { - return; - } + if (new_hall_state == hall_state) return; + + long new_pulse_timestamp = _micros(); hall_state = new_hall_state; int8_t new_electric_sector = ELECTRIC_SECTORS[hall_state]; - if (new_electric_sector - electric_sector > 3) { + int8_t electric_sector_dif = new_electric_sector - electric_sector; + if (electric_sector_dif > 3) { //underflow direction = Direction::CCW; electric_rotations += direction; - } else if (new_electric_sector - electric_sector < (-3)) { + } else if (electric_sector_dif < (-3)) { //overflow direction = Direction::CW; electric_rotations += direction; @@ -96,11 +95,19 @@ void HallSensor::attachSectorCallback(void (*_onSectorChange)(int sector)) { // Sensor update function. Safely copy volatile interrupt variables into Sensor base class state variables. void HallSensor::update() { // Copy volatile variables in minimal-duration blocking section to ensure no interrupts are missed - noInterrupts(); + if (use_interrupt){ + noInterrupts(); + }else{ + A_active = digitalRead(pinA); + B_active = digitalRead(pinB); + C_active = digitalRead(pinC); + updateState(); + } + angle_prev_ts = pulse_timestamp; long last_electric_rotations = electric_rotations; int8_t last_electric_sector = electric_sector; - interrupts(); + if (use_interrupt) interrupts(); angle_prev = ((float)((last_electric_rotations * 6 + last_electric_sector) % cpr) / (float)cpr) * _2PI ; full_rotations = (int32_t)((last_electric_rotations * 6 + last_electric_sector) / cpr); } @@ -150,7 +157,7 @@ void HallSensor::init(){ } // init hall_state - A_active= digitalRead(pinA); + A_active = digitalRead(pinA); B_active = digitalRead(pinB); C_active = digitalRead(pinC); updateState(); @@ -169,4 +176,6 @@ void HallSensor::enableInterrupts(void (*doA)(), void(*doB)(), void(*doC)()){ if(doA != nullptr) attachInterrupt(digitalPinToInterrupt(pinA), doA, CHANGE); if(doB != nullptr) attachInterrupt(digitalPinToInterrupt(pinB), doB, CHANGE); if(doC != nullptr) attachInterrupt(digitalPinToInterrupt(pinC), doC, CHANGE); + + use_interrupt = true; } diff --git a/src/sensors/HallSensor.h b/src/sensors/HallSensor.h index 78e1b416..94053e0f 100644 --- a/src/sensors/HallSensor.h +++ b/src/sensors/HallSensor.h @@ -47,6 +47,7 @@ class HallSensor: public Sensor{ int pinA; //!< HallSensor hardware pin A int pinB; //!< HallSensor hardware pin B int pinC; //!< HallSensor hardware pin C + int use_interrupt; //!< True if interrupts have been attached // HallSensor configuration Pullup pullup; //!< Configuration parameter internal or external pullups From 41de6b673ebb6528e90155f793019a72f47d8435 Mon Sep 17 00:00:00 2001 From: dekutree64 <54822734+dekutree64@users.noreply.github.com> Date: Wed, 14 Feb 2024 03:38:50 -0600 Subject: [PATCH 04/57] Moving LinearHall back to drivers repository As requested by runger --- src/sensors/LinearHall.cpp | 110 ------------------------------------- src/sensors/LinearHall.h | 45 --------------- 2 files changed, 155 deletions(-) delete mode 100644 src/sensors/LinearHall.cpp delete mode 100644 src/sensors/LinearHall.h diff --git a/src/sensors/LinearHall.cpp b/src/sensors/LinearHall.cpp deleted file mode 100644 index 96cfae3e..00000000 --- a/src/sensors/LinearHall.cpp +++ /dev/null @@ -1,110 +0,0 @@ -#include "LinearHall.h" - -// This function can be overridden with custom ADC code on platforms with poor analogRead performance. -__attribute__((weak)) void ReadLinearHalls(int hallA, int hallB, int *a, int *b) -{ - *a = analogRead(hallA); - *b = analogRead(hallB); -} - -LinearHall::LinearHall(int _hallA, int _hallB, int _pp){ - centerA = 512; - centerB = 512; - pinA = _hallA; - pinB = _hallB; - pp = _pp; -} - -float LinearHall::getSensorAngle() { - ReadLinearHalls(pinA, pinB, &lastA, &lastB); - //offset readings using center values, then compute angle - float reading = _atan2(lastA - centerA, lastB - centerB); - - //handle rollover logic between each electrical revolution of the motor - if (reading > prev_reading) { - if (reading - prev_reading >= PI) { - if (electrical_rev - 1 < 0) { - electrical_rev = pp - 1; - } else { - electrical_rev = electrical_rev - 1; - } - } - } else if (reading < prev_reading) { - if (prev_reading - reading >= PI) { - if (electrical_rev + 1 >= pp) { - electrical_rev = 0; - } else { - electrical_rev = electrical_rev + 1; - } - } - } - - //convert result from electrical angle and electrical revolution count to shaft angle in radians - float result = (reading + PI) / _2PI; - result = _2PI * (result + electrical_rev) / pp; - - //update previous reading for rollover handling - prev_reading = reading; - return result; -} - -void LinearHall::init(int _centerA, int _centerB) { - // Skip configuring the pins here because they normally default to input anyway, and - // this makes it possible to use ADC channel numbers instead of pin numbers when using - // custom implementation of ReadLinearHalls, to avoid having to remap them every update. - // If pins do need to be configured, it can be done by user code before calling init. - //pinMode(pinA, INPUT); - //pinMode(pinB, INPUT); - - centerA = _centerA; - centerB = _centerB; - - //establish initial reading for rollover handling - electrical_rev = 0; - ReadLinearHalls(pinA, pinB, &lastA, &lastB); - prev_reading = _atan2(lastA - centerA, lastB - centerB); -} - -void LinearHall::init(FOCMotor *motor) { - if (!motor->enabled) { - SIMPLEFOC_DEBUG("LinearHall::init failed. Call after motor.init, but before motor.initFOC."); - return; - } - - // See comment in other version of init for why these are commented out - //pinMode(pinA, INPUT); - //pinMode(pinB, INPUT); - - int minA, maxA, minB, maxB; - - ReadLinearHalls(pinA, pinB, &lastA, &lastB); - minA = maxA = centerA = lastA; - minB = maxB = centerB = lastB; - - // move one mechanical revolution forward - for (int i = 0; i <= 2000; i++) - { - float angle = _3PI_2 + _2PI * i * pp / 2000.0f; - motor->setPhaseVoltage(motor->voltage_sensor_align, 0, angle); - - ReadLinearHalls(pinA, pinB, &lastA, &lastB); - - if (lastA < minA) - minA = lastA; - if (lastA > maxA) - maxA = lastA; - centerA = (minA + maxA) / 2; - - if (lastB < minB) - minB = lastB; - if (lastB > maxB) - maxB = lastB; - centerB = (minB + maxB) / 2; - - _delay(2); - } - - //establish initial reading for rollover handling - electrical_rev = 0; - prev_reading = _atan2(lastA - centerA, lastB - centerB); -} diff --git a/src/sensors/LinearHall.h b/src/sensors/LinearHall.h deleted file mode 100644 index d17035e4..00000000 --- a/src/sensors/LinearHall.h +++ /dev/null @@ -1,45 +0,0 @@ -#ifndef LINEAR_HALL_SENSOR_LIB_H -#define LINEAR_HALL_SENSOR_LIB_H - -#include - -// This function can be overridden with custom ADC code on platforms with poor analogRead performance. -void ReadLinearHalls(int hallA, int hallB, int *a, int *b); - -/** - * This sensor class is for two linear hall effect sensors such as 49E, which are - * positioned 90 electrical degrees apart (if one is centered on a rotor magnet, - * the other is half way between rotor magnets). - * It can also be used for a single magnet mounted to the motor shaft (set pp to 1). - * - * For more information, see this forum thread and PDF - * https://community.simplefoc.com/t/40-cent-magnetic-angle-sensing-technique/1959 - * https://gist.github.com/nanoparticle/00030ea27c59649edbed84f0a957ebe1 - */ -class LinearHall: public Sensor{ - public: - LinearHall(int hallA, int hallB, int pp); - - void init(int centerA, int centerB); // Initialize without moving motor - void init(class FOCMotor *motor); // Move motor to find center values - - // Get current shaft angle from the sensor hardware, and - // return it as a float in radians, in the range 0 to 2PI. - // - This method is pure virtual and must be implemented in subclasses. - // Calling this method directly does not update the base-class internal fields. - // Use update() when calling from outside code. - float getSensorAngle() override; - - int centerA; - int centerB; - int lastA, lastB; - - private: - int pinA; - int pinB; - int pp; - int electrical_rev; - float prev_reading; -}; - -#endif From 3a840213de36eddf5219e691b1f8482d5147c490 Mon Sep 17 00:00:00 2001 From: dekutree64 <54822734+dekutree64@users.noreply.github.com> Date: Wed, 14 Feb 2024 03:53:46 -0600 Subject: [PATCH 05/57] Moving LinearHall back to drivers repository --- .../angle_control/angle_control.ino | 117 ------------------ .../voltage_control/voltage_control.ino | 96 -------------- .../velocity_control/velocity_control.ino | 109 ---------------- 3 files changed, 322 deletions(-) delete mode 100644 examples/motion_control/position_motion_control/linear_hall/angle_control/angle_control.ino delete mode 100644 examples/motion_control/torque_control/linear_hall/voltage_control/voltage_control.ino delete mode 100644 examples/motion_control/velocity_motion_control/linear_hall/velocity_control/velocity_control.ino diff --git a/examples/motion_control/position_motion_control/linear_hall/angle_control/angle_control.ino b/examples/motion_control/position_motion_control/linear_hall/angle_control/angle_control.ino deleted file mode 100644 index 8179008d..00000000 --- a/examples/motion_control/position_motion_control/linear_hall/angle_control/angle_control.ino +++ /dev/null @@ -1,117 +0,0 @@ -/** - * - * Position/angle motion control example - * Steps: - * 1) Configure the motor and hall sensor - * 2) Run the code - * 3) Set the target angle (in radians) from serial terminal - */ -#include - -// BLDC motor & driver instance -BLDCMotor motor = BLDCMotor(11); -BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8); -// Stepper motor & driver instance -//StepperMotor motor = StepperMotor(50); -//StepperDriver4PWM driver = StepperDriver4PWM(9, 5, 10, 6, 8); - -// hall sensor instance -LinearHall sensor = LinearHall(A0, A1, 11); - -// angle set point variable -float target_angle = 0; -// instantiate the commander -Commander command = Commander(Serial); -void doTarget(char* cmd) { command.scalar(&target_angle, cmd); } - -void setup() { - - // driver config - // power supply voltage [V] - driver.voltage_power_supply = 12; - driver.init(); - // link the motor and the driver - motor.linkDriver(&driver); - - - // aligning voltage [V] - motor.voltage_sensor_align = 3; - // index search velocity [rad/s] - motor.velocity_index_search = 3; - - // set motion control loop to be used - motor.controller = MotionControlType::angle; - - // contoller configuration - // default parameters in defaults.h - - // velocity PI controller parameters - motor.PID_velocity.P = 0.2f; - motor.PID_velocity.I = 2; - motor.PID_velocity.D = 0; - // default voltage_power_supply - motor.voltage_limit = 6; - // jerk control using voltage voltage ramp - // default value is 300 volts per sec ~ 0.3V per millisecond - motor.PID_velocity.output_ramp = 1000; - - // velocity low pass filtering time constant - motor.LPF_velocity.Tf = 0.01f; - - // angle P controller - motor.P_angle.P = 20; - // maximal velocity of the position control - motor.velocity_limit = 4; - - - // use monitoring with serial - Serial.begin(115200); - // comment out if not needed - motor.useMonitoring(Serial); - - // initialize motor - motor.init(); - // initialize sensor hardware. This moves the motor to find the min/max sensor readings and - // averages them to get the center values. The motor can't move until motor.init is called, and - // motor.initFOC can't do its calibration until the sensor is intialized, so this must be done inbetween. - // You can then take the values printed to the serial monitor and pass them to sensor.init to - // avoid having to move the motor every time. In that case it doesn't matter whether sensor.init - // is called before or after motor.init. - sensor.init(&motor); - Serial.print("LinearHall centerA: "); - Serial.print(sensor.centerA); - Serial.print(", centerB: "); - Serial.println(sensor.centerB); - // link the motor to the sensor - motor.linkSensor(&sensor); - // align sensor and start FOC - motor.initFOC(); - - // add target command T - command.add('T', doTarget, "target angle"); - - Serial.println(F("Motor ready.")); - Serial.println(F("Set the target angle using serial terminal:")); - _delay(1000); -} - -void loop() { - // main FOC algorithm function - // the faster you run this function the better - // Arduino UNO loop ~1kHz - // Bluepill loop ~10kHz - motor.loopFOC(); - - // Motion control function - // velocity, position or voltage (defined in motor.controller) - // this function can be run at much lower frequency than loopFOC() function - // You can also use motor.move() and set the motor.target in the code - motor.move(target_angle); - - // function intended to be used with serial plotter to monitor motor variables - // significantly slowing the execution down!!!! - // motor.monitor(); - - // user communication - command.run(); -} diff --git a/examples/motion_control/torque_control/linear_hall/voltage_control/voltage_control.ino b/examples/motion_control/torque_control/linear_hall/voltage_control/voltage_control.ino deleted file mode 100644 index e646849a..00000000 --- a/examples/motion_control/torque_control/linear_hall/voltage_control/voltage_control.ino +++ /dev/null @@ -1,96 +0,0 @@ -/** - * - * Torque control example using voltage control loop. - * - * Most of the low-end BLDC driver boards doesn't have current measurement therefore SimpleFOC offers - * you a way to control motor torque by setting the voltage to the motor instead of the current. - * - * This makes the BLDC motor effectively a DC motor, and you can use it in a same way. - */ -#include - - -// BLDC motor & driver instance -BLDCMotor motor = BLDCMotor(11); -BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8); -// Stepper motor & driver instance -//StepperMotor motor = StepperMotor(50); -//StepperDriver4PWM driver = StepperDriver4PWM(9, 5, 10, 6, 8); - -// hall sensor instance -LinearHall sensor = LinearHall(A0, A1, 11); - - -// voltage set point variable -float target_voltage = 2; -// instantiate the commander -Commander command = Commander(Serial); -void doTarget(char* cmd) { command.scalar(&target_voltage, cmd); } - -void setup() { - - // driver config - // power supply voltage [V] - driver.voltage_power_supply = 12; - driver.init(); - // link driver - motor.linkDriver(&driver); - - // aligning voltage - motor.voltage_sensor_align = 3; - - // choose FOC modulation (optional) - motor.foc_modulation = FOCModulationType::SpaceVectorPWM; - - // set motion control loop to be used - motor.controller = MotionControlType::torque; - - // use monitoring with serial - Serial.begin(115200); - // comment out if not needed - motor.useMonitoring(Serial); - - // initialize motor - motor.init(); - // initialize sensor hardware. This moves the motor to find the min/max sensor readings and - // averages them to get the center values. The motor can't move until motor.init is called, and - // motor.initFOC can't do its calibration until the sensor is intialized, so this must be done inbetween. - // You can then take the values printed to the serial monitor and pass them to sensor.init to - // avoid having to move the motor every time. In that case it doesn't matter whether sensor.init - // is called before or after motor.init. - sensor.init(&motor); - Serial.print("LinearHall centerA: "); - Serial.print(sensor.centerA); - Serial.print(", centerB: "); - Serial.println(sensor.centerB); - // link the motor to the sensor - motor.linkSensor(&sensor); - // align sensor and start FOC - motor.initFOC(); - - // add target command T - command.add('T', doTarget, "target voltage"); - - Serial.println(F("Motor ready.")); - Serial.println(F("Set the target voltage using serial terminal:")); - _delay(1000); -} - - -void loop() { - - // main FOC algorithm function - // the faster you run this function the better - // Arduino UNO loop ~1kHz - // Bluepill loop ~10kHz - motor.loopFOC(); - - // Motion control function - // velocity, position or voltage (defined in motor.controller) - // this function can be run at much lower frequency than loopFOC() function - // You can also use motor.move() and set the motor.target in the code - motor.move(target_voltage); - - // user communication - command.run(); -} \ No newline at end of file diff --git a/examples/motion_control/velocity_motion_control/linear_hall/velocity_control/velocity_control.ino b/examples/motion_control/velocity_motion_control/linear_hall/velocity_control/velocity_control.ino deleted file mode 100644 index 870a0899..00000000 --- a/examples/motion_control/velocity_motion_control/linear_hall/velocity_control/velocity_control.ino +++ /dev/null @@ -1,109 +0,0 @@ -/** - * - * Velocity motion control example - * Steps: - * 1) Configure the motor and sensor - * 2) Run the code - * 3) Set the target velocity (in radians per second) from serial terminal - */ -#include - -// BLDC motor & driver instance -BLDCMotor motor = BLDCMotor(11); -BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8); -// Stepper motor & driver instance -//StepperMotor motor = StepperMotor(50); -//StepperDriver4PWM driver = StepperDriver4PWM(9, 5, 10, 6, 8); - -// hall sensor instance -LinearHall sensor = LinearHall(A0, A1, 11); - -// velocity set point variable -float target_velocity = 0; -// instantiate the commander -Commander command = Commander(Serial); -void doTarget(char* cmd) { command.scalar(&target_velocity, cmd); } - -void setup() { - - // driver config - // power supply voltage [V] - driver.voltage_power_supply = 12; - driver.init(); - // link the motor and the driver - motor.linkDriver(&driver); - - // aligning voltage [V] - motor.voltage_sensor_align = 3; - - // set motion control loop to be used - motor.controller = MotionControlType::velocity; - - // contoller configuration - // default parameters in defaults.h - - // velocity PI controller parameters - motor.PID_velocity.P = 0.2f; - motor.PID_velocity.I = 2; - motor.PID_velocity.D = 0; - // default voltage_power_supply - motor.voltage_limit = 6; - // jerk control using voltage voltage ramp - // default value is 300 volts per sec ~ 0.3V per millisecond - motor.PID_velocity.output_ramp = 1000; - - // velocity low pass filtering time constant - motor.LPF_velocity.Tf = 0.01f; - - // use monitoring with serial - Serial.begin(115200); - // comment out if not needed - motor.useMonitoring(Serial); - - // initialize motor - motor.init(); - // initialize sensor hardware. This moves the motor to find the min/max sensor readings and - // averages them to get the center values. The motor can't move until motor.init is called, and - // motor.initFOC can't do its calibration until the sensor is intialized, so this must be done inbetween. - // You can then take the values printed to the serial monitor and pass them to sensor.init to - // avoid having to move the motor every time. In that case it doesn't matter whether sensor.init - // is called before or after motor.init. - sensor.init(&motor); - Serial.print("LinearHall centerA: "); - Serial.print(sensor.centerA); - Serial.print(", centerB: "); - Serial.println(sensor.centerB); - // link the motor to the sensor - motor.linkSensor(&sensor); - // align sensor and start FOC - motor.initFOC(); - - // add target command T - command.add('T', doTarget, "target voltage"); - - Serial.println(F("Motor ready.")); - Serial.println(F("Set the target velocity using serial terminal:")); - _delay(1000); -} - - -void loop() { - // main FOC algorithm function - // the faster you run this function the better - // Arduino UNO loop ~1kHz - // Bluepill loop ~10kHz - motor.loopFOC(); - - // Motion control function - // velocity, position or voltage (defined in motor.controller) - // this function can be run at much lower frequency than loopFOC() function - // You can also use motor.move() and set the motor.target in the code - motor.move(target_velocity); - - // function intended to be used with serial plotter to monitor motor variables - // significantly slowing the execution down!!!! - // motor.monitor(); - - // user communication - command.run(); -} \ No newline at end of file From 14f67265a2a73b985afc83fab8b036b2f295fd06 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Tue, 30 Apr 2024 12:05:09 +0200 Subject: [PATCH 06/57] fix RP2040 compile problems with earlehillpower --- src/drivers/hardware_specific/rp2040/rp2040_mcu.cpp | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/src/drivers/hardware_specific/rp2040/rp2040_mcu.cpp b/src/drivers/hardware_specific/rp2040/rp2040_mcu.cpp index f187fbef..6afbf345 100644 --- a/src/drivers/hardware_specific/rp2040/rp2040_mcu.cpp +++ b/src/drivers/hardware_specific/rp2040/rp2040_mcu.cpp @@ -13,13 +13,16 @@ #pragma message("SimpleFOC: compiling for RP2040") #pragma message("") - +#if !defined(SIMPLEFOC_DEBUG_RP2040) #define SIMPLEFOC_DEBUG_RP2040 +#endif #include "../../hardware_api.h" #include "hardware/pwm.h" #include "hardware/clocks.h" +#if defined(USE_ARDUINO_PINOUT) #include +#endif #define _PWM_FREQUENCY 24000 #define _PWM_FREQUENCY_MAX 66000 @@ -35,7 +38,11 @@ uint16_t wrapvalues[NUM_PWM_SLICES]; // TODO add checks which channels are already used... void setupPWM(int pin_nr, long pwm_frequency, bool invert, RP2040DriverParams* params, uint8_t index) { + #if defined(USE_ARDUINO_PINOUT) uint pin = (uint)digitalPinToPinName(pin_nr); // we could check for -DBOARD_HAS_PIN_REMAP ? + #else + uint pin = (uint)pin_nr; + #endif gpio_set_function(pin, GPIO_FUNC_PWM); uint slice = pwm_gpio_to_slice_num(pin); uint chan = pwm_gpio_to_channel(pin); From 3b7fa90b1e39d033138cc5bcd5a2140456957c5d Mon Sep 17 00:00:00 2001 From: askuric Date: Mon, 20 May 2024 18:01:31 +0200 Subject: [PATCH 07/57] added initial support for current sensing for stepper motors --- src/BLDCMotor.h | 1 + src/StepperMotor.cpp | 146 ++++++++++--- src/StepperMotor.h | 2 + src/common/base_classes/BLDCDriver.h | 32 +-- src/common/base_classes/CurrentSense.cpp | 253 +++++++++++++++++++++- src/common/base_classes/CurrentSense.h | 56 ++++- src/common/base_classes/FOCDriver.h | 47 ++++ src/common/base_classes/StepperDriver.h | 32 ++- src/common/foc_utils.h | 2 + src/communication/SimpleFOCDebug.h | 5 +- src/current_sense/GenericCurrentSense.cpp | 100 --------- src/current_sense/InlineCurrentSense.cpp | 177 +-------------- src/current_sense/InlineCurrentSense.h | 25 +-- src/current_sense/LowsideCurrentSense.cpp | 172 +-------------- src/current_sense/LowsideCurrentSense.h | 23 +- src/drivers/BLDCDriver3PWM.h | 6 +- src/drivers/BLDCDriver6PWM.h | 1 - src/drivers/StepperDriver2PWM.cpp | 16 +- src/drivers/StepperDriver2PWM.h | 9 + src/drivers/StepperDriver4PWM.cpp | 17 +- src/drivers/StepperDriver4PWM.h | 10 + 21 files changed, 550 insertions(+), 582 deletions(-) create mode 100644 src/common/base_classes/FOCDriver.h diff --git a/src/BLDCMotor.h b/src/BLDCMotor.h index a7155c1f..41a5a1c0 100644 --- a/src/BLDCMotor.h +++ b/src/BLDCMotor.h @@ -4,6 +4,7 @@ #include "Arduino.h" #include "common/base_classes/FOCMotor.h" #include "common/base_classes/Sensor.h" +#include "common/base_classes/FOCDriver.h" #include "common/base_classes/BLDCDriver.h" #include "common/foc_utils.h" #include "common/time_utils.h" diff --git a/src/StepperMotor.cpp b/src/StepperMotor.cpp index 5d519f29..1e4e343c 100644 --- a/src/StepperMotor.cpp +++ b/src/StepperMotor.cpp @@ -108,12 +108,28 @@ int StepperMotor::initFOC() { // alignment necessary for encoders! // sensor and motor alignment - can be skipped // by setting motor.sensor_direction and motor.zero_electric_angle - _delay(500); if(sensor){ exit_flag *= alignSensor(); // added the shaft_angle update sensor->update(); - shaft_angle = sensor->getAngle(); + shaft_angle = shaftAngle(); + + // aligning the current sensor - can be skipped + // checks if driver phases are the same as current sense phases + // and checks the direction of measuremnt. + if(exit_flag){ + if(current_sense){ + if (!current_sense->initialized) { + motor_status = FOCMotorStatus::motor_calib_failed; + SIMPLEFOC_DEBUG("MOT: Init FOC error, current sense not initialized"); + exit_flag = 0; + }else{ + exit_flag *= alignCurrentSense(); + } + } + else { SIMPLEFOC_DEBUG("MOT: No current sense."); } + } + } else { SIMPLEFOC_DEBUG("MOT: No sensor."); if ((controller == MotionControlType::angle_openloop || controller == MotionControlType::velocity_openloop)){ @@ -136,6 +152,26 @@ int StepperMotor::initFOC() { return exit_flag; } +// Calibrate the motor and current sense phases +int StepperMotor::alignCurrentSense() { + int exit_flag = 1; // success + + SIMPLEFOC_DEBUG("MOT: Align current sense."); + + // align current sense and the driver + exit_flag = current_sense->driverAlign(voltage_sensor_align); + if(!exit_flag){ + // error in current sense - phase either not measured or bad connection + SIMPLEFOC_DEBUG("MOT: Align error!"); + exit_flag = 0; + }else{ + // output the alignment status flag + SIMPLEFOC_DEBUG("MOT: Success: ", exit_flag); + } + + return exit_flag > 0; +} + // Encoder alignment to electrical 0 angle int StepperMotor::alignSensor() { int exit_flag = 1; //success @@ -261,8 +297,6 @@ void StepperMotor::loopFOC() { // if open-loop do nothing if( controller==MotionControlType::angle_openloop || controller==MotionControlType::velocity_openloop ) return; - // shaft angle - shaft_angle = shaftAngle(); // if disabled do nothing if(!enabled) return; @@ -272,6 +306,44 @@ void StepperMotor::loopFOC() { // which is in range 0-2PI electrical_angle = electricalAngle(); + // Needs the update() to be called first + // This function will not have numerical issues because it uses Sensor::getMechanicalAngle() + // which is in range 0-2PI + electrical_angle = electricalAngle(); + switch (torque_controller) { + case TorqueControlType::voltage: + // no need to do anything really + break; + case TorqueControlType::dc_current: + if(!current_sense) return; + // read overall current magnitude + current.q = current_sense->getDCCurrent(electrical_angle); + // filter the value values + current.q = LPF_current_q(current.q); + // calculate the phase voltage + voltage.q = PID_current_q(current_sp - current.q); + // d voltage - lag compensation + if(_isset(phase_inductance)) voltage.d = _constrain( -current_sp*shaft_velocity*pole_pairs*phase_inductance, -voltage_limit, voltage_limit); + else voltage.d = 0; + break; + case TorqueControlType::foc_current: + if(!current_sense) return; + // read dq currents + current = current_sense->getFOCCurrents(electrical_angle); + // filter values + current.q = LPF_current_q(current.q); + current.d = LPF_current_d(current.d); + // calculate the phase voltages + voltage.q = PID_current_q(current_sp - current.q); + voltage.d = PID_current_d(-current.d); + // d voltage - lag compensation - TODO verify + // if(_isset(phase_inductance)) voltage.d = _constrain( voltage.d - current_sp*shaft_velocity*pole_pairs*phase_inductance, -voltage_limit, voltage_limit); + break; + default: + // no torque control selected + SIMPLEFOC_DEBUG("MOT: no torque control selected!"); + break; + } // set the phase voltage - FOC heart function :) setPhaseVoltage(voltage.q, voltage.d, electrical_angle); } @@ -309,56 +381,70 @@ void StepperMotor::move(float new_target) { // estimate the motor current if phase reistance available and current_sense not available if(!current_sense && _isset(phase_resistance)) current.q = (voltage.q - voltage_bemf)/phase_resistance; - // choose control loop + // upgrade the current based voltage limit switch (controller) { case MotionControlType::torque: - if(!_isset(phase_resistance)) voltage.q = target; // if voltage torque control - else voltage.q = target*phase_resistance + voltage_bemf; - voltage.q = _constrain(voltage.q, -voltage_limit, voltage_limit); - // set d-component (lag compensation if known inductance) - if(!_isset(phase_inductance)) voltage.d = 0; - else voltage.d = _constrain( -target*shaft_velocity*pole_pairs*phase_inductance, -voltage_limit, voltage_limit); + if(torque_controller == TorqueControlType::voltage){ // if voltage torque control + if(!_isset(phase_resistance)) voltage.q = target; + else voltage.q = target*phase_resistance + voltage_bemf; + voltage.q = _constrain(voltage.q, -voltage_limit, voltage_limit); + // set d-component (lag compensation if known inductance) + if(!_isset(phase_inductance)) voltage.d = 0; + else voltage.d = _constrain( -target*shaft_velocity*pole_pairs*phase_inductance, -voltage_limit, voltage_limit); + }else{ + current_sp = target; // if current/foc_current torque control + } break; case MotionControlType::angle: + // TODO sensor precision: this calculation is not numerically precise. The target value cannot express precise positions when + // the angles are large. This results in not being able to command small changes at high position values. + // to solve this, the delta-angle has to be calculated in a numerically precise way. // angle set point shaft_angle_sp = target; // calculate velocity set point shaft_velocity_sp = feed_forward_velocity + P_angle( shaft_angle_sp - shaft_angle ); - shaft_velocity_sp = _constrain(shaft_velocity_sp, -velocity_limit, velocity_limit); - // calculate the torque command + shaft_velocity_sp = _constrain(shaft_velocity_sp,-velocity_limit, velocity_limit); + // calculate the torque command - sensor precision: this calculation is ok, but based on bad value from previous calculation current_sp = PID_velocity(shaft_velocity_sp - shaft_velocity); // if voltage torque control // if torque controlled through voltage - // use voltage if phase-resistance not provided - if(!_isset(phase_resistance)) voltage.q = current_sp; - else voltage.q = _constrain( current_sp*phase_resistance + voltage_bemf , -voltage_limit, voltage_limit); - // set d-component (lag compensation if known inductance) - if(!_isset(phase_inductance)) voltage.d = 0; - else voltage.d = _constrain( -current_sp*shaft_velocity*pole_pairs*phase_inductance, -voltage_limit, voltage_limit); + if(torque_controller == TorqueControlType::voltage){ + // use voltage if phase-resistance not provided + if(!_isset(phase_resistance)) voltage.q = current_sp; + else voltage.q = _constrain( current_sp*phase_resistance + voltage_bemf , -voltage_limit, voltage_limit); + // set d-component (lag compensation if known inductance) + if(!_isset(phase_inductance)) voltage.d = 0; + else voltage.d = _constrain( -current_sp*shaft_velocity*pole_pairs*phase_inductance, -voltage_limit, voltage_limit); + } break; case MotionControlType::velocity: - // velocity set point + // velocity set point - sensor precision: this calculation is numerically precise. shaft_velocity_sp = target; // calculate the torque command current_sp = PID_velocity(shaft_velocity_sp - shaft_velocity); // if current/foc_current torque control // if torque controlled through voltage control - // use voltage if phase-resistance not provided - if(!_isset(phase_resistance)) voltage.q = current_sp; - else voltage.q = _constrain( current_sp*phase_resistance + voltage_bemf , -voltage_limit, voltage_limit); - // set d-component (lag compensation if known inductance) - if(!_isset(phase_inductance)) voltage.d = 0; - else voltage.d = _constrain( -current_sp*shaft_velocity*pole_pairs*phase_inductance, -voltage_limit, voltage_limit); + if(torque_controller == TorqueControlType::voltage){ + // use voltage if phase-resistance not provided + if(!_isset(phase_resistance)) voltage.q = current_sp; + else voltage.q = _constrain( current_sp*phase_resistance + voltage_bemf , -voltage_limit, voltage_limit); + // set d-component (lag compensation if known inductance) + if(!_isset(phase_inductance)) voltage.d = 0; + else voltage.d = _constrain( -current_sp*shaft_velocity*pole_pairs*phase_inductance, -voltage_limit, voltage_limit); + } break; case MotionControlType::velocity_openloop: - // velocity control in open loop + // velocity control in open loop - sensor precision: this calculation is numerically precise. shaft_velocity_sp = target; voltage.q = velocityOpenloop(shaft_velocity_sp); // returns the voltage that is set to the motor - voltage.d = 0; // TODO d-component lag-compensation + voltage.d = 0; break; case MotionControlType::angle_openloop: - // angle control in open loop + // angle control in open loop - + // TODO sensor precision: this calculation NOT numerically precise, and subject + // to the same problems in small set-point changes at high angles + // as the closed loop version. shaft_angle_sp = target; voltage.q = angleOpenloop(shaft_angle_sp); // returns the voltage that is set to the motor - voltage.d = 0; // TODO d-component lag-compensation + voltage.d = 0; break; } } diff --git a/src/StepperMotor.h b/src/StepperMotor.h index 45d20c63..f76e7bcf 100644 --- a/src/StepperMotor.h +++ b/src/StepperMotor.h @@ -89,6 +89,8 @@ class StepperMotor: public FOCMotor int alignSensor(); /** Motor and sensor alignment to the sensors absolute 0 angle */ int absoluteZeroSearch(); + /** Current sense and motor phase alignment */ + int alignCurrentSense(); // Open loop motion control /** diff --git a/src/common/base_classes/BLDCDriver.h b/src/common/base_classes/BLDCDriver.h index f405d785..6ae8186f 100644 --- a/src/common/base_classes/BLDCDriver.h +++ b/src/common/base_classes/BLDCDriver.h @@ -2,40 +2,17 @@ #define BLDCDRIVER_H #include "Arduino.h" +#include "FOCDriver.h" - -enum PhaseState : uint8_t { - PHASE_OFF = 0, // both sides of the phase are off - PHASE_ON = 1, // both sides of the phase are driven with PWM, dead time is applied in 6-PWM mode - PHASE_HI = 2, // only the high side of the phase is driven with PWM (6-PWM mode only) - PHASE_LO = 3, // only the low side of the phase is driven with PWM (6-PWM mode only) -}; - - -class BLDCDriver{ +class BLDCDriver: public FOCDriver{ public: - /** Initialise hardware */ - virtual int init() = 0; - /** Enable hardware */ - virtual void enable() = 0; - /** Disable hardware */ - virtual void disable() = 0; - - long pwm_frequency; //!< pwm frequency value in hertz - float voltage_power_supply; //!< power supply voltage - float voltage_limit; //!< limiting voltage set to the motor - - float dc_a; //!< currently set duty cycle on phaseA float dc_b; //!< currently set duty cycle on phaseB float dc_c; //!< currently set duty cycle on phaseC - bool initialized = false; // true if driver was successfully initialized - void* params = 0; // pointer to hardware specific parameters of driver - /** - * Set phase voltages to the harware + * Set phase voltages to the hardware * * @param Ua - phase A voltage * @param Ub - phase B voltage @@ -51,6 +28,9 @@ class BLDCDriver{ * @param sa - phase C state : active / disabled ( high impedance ) */ virtual void setPhaseState(PhaseState sa, PhaseState sb, PhaseState sc) = 0; + + /** driver type getter function */ + virtual DriverType type() override { return DriverType::BLDC; }; }; #endif diff --git a/src/common/base_classes/CurrentSense.cpp b/src/common/base_classes/CurrentSense.cpp index 03ea19ea..4e6c3386 100644 --- a/src/common/base_classes/CurrentSense.cpp +++ b/src/common/base_classes/CurrentSense.cpp @@ -1,4 +1,5 @@ #include "CurrentSense.h" +#include "../../communication/SimpleFOCDebug.h" // get current magnitude @@ -27,7 +28,7 @@ float CurrentSense::getDCCurrent(float motor_electrical_angle){ return sign*_sqrt(ABcurrent.alpha*ABcurrent.alpha + ABcurrent.beta*ABcurrent.beta); } -// function used with the foc algorihtm +// function used with the foc algorithm // calculating DQ currents from phase currents // - function calculating park and clarke transform of the phase currents // - using getPhaseCurrents and getABCurrents internally @@ -44,11 +45,21 @@ DQCurrent_s CurrentSense::getFOCCurrents(float angle_el){ return return_current; } -// function used with the foc algorihtm +// function used with the foc algorithm // calculating Alpha Beta currents from phase currents // - function calculating Clarke transform of the phase currents ABCurrent_s CurrentSense::getABCurrents(PhaseCurrent_s current){ + // check if driver is an instance of StepperDriver + // if so there is no need to Clarke transform + if (driver_type == DriverType::Stepper){ + ABCurrent_s return_ABcurrent; + return_ABcurrent.alpha = current.a; + return_ABcurrent.beta = current.b; + return return_ABcurrent; + } + + // otherwise it's a BLDC motor and // calculate clarke transform float i_alpha, i_beta; if(!current.c){ @@ -80,7 +91,7 @@ ABCurrent_s CurrentSense::getABCurrents(PhaseCurrent_s current){ return return_ABcurrent; } -// function used with the foc algorihtm +// function used with the foc algorithm // calculating D and Q currents from Alpha Beta currents and electrical angle // - function calculating Clarke transform of the phase currents DQCurrent_s CurrentSense::getDQCurrents(ABCurrent_s current, float angle_el){ @@ -97,8 +108,10 @@ DQCurrent_s CurrentSense::getDQCurrents(ABCurrent_s current, float angle_el){ /** Driver linking to the current sense */ -void CurrentSense::linkDriver(BLDCDriver* _driver) { - driver = _driver; +void CurrentSense::linkDriver(FOCDriver* _driver) { + driver = _driver; + // save the driver type for easier access + driver_type = driver->type(); } @@ -108,4 +121,232 @@ void CurrentSense::enable(){ void CurrentSense::disable(){ // nothing is done here, but you can override this function -}; \ No newline at end of file +}; + + +// Function aligning the current sense with motor driver +// if all pins are connected well none of this is really necessary! - can be avoided +// returns flag +// 0 - fail +// 1 - success and nothing changed +// 2 - success but pins reconfigured +// 3 - success but gains inverted +// 4 - success but pins reconfigured and gains inverted +// IMPORTANT, this function can be overriden in the child class +int CurrentSense::driverAlign(float voltage){ + + int exit_flag = 1; + if(skip_align) return exit_flag; + + if (!initialized) return 0; + + // check if stepper or BLDC + if(driver_type == DriverType::Stepper) + return alignStepperDriver(voltage, (StepperDriver*)driver); + else + return alignBLDCDriver(voltage, (BLDCDriver*)driver); +} + + + +// Helper function to read and average phase currents +PhaseCurrent_s CurrentSense::readAverageCurrents(int N) { + PhaseCurrent_s c = getPhaseCurrents(); + for (int i = 0; i < N; i++) { + PhaseCurrent_s c1 = getPhaseCurrents(); + c.a = c.a * 0.6f + 0.4f * c1.a; + c.b = c.b * 0.6f + 0.4f * c1.b; + c.c = c.c * 0.6f + 0.4f * c1.c; + _delay(3); + } + return c; +}; + + + +// Function aligning the current sense with motor driver +// if all pins are connected well none of this is really necessary! - can be avoided +// returns flag +// 0 - fail +// 1 - success and nothing changed +// 2 - success but pins reconfigured +// 3 - success but gains inverted +// 4 - success but pins reconfigured and gains inverted +int CurrentSense::alignBLDCDriver(float voltage, BLDCDriver* bldc_driver){ + + int exit_flag = 1; + if(_isset(pinA)){ + // set phase A active and phases B and C down + bldc_driver->setPwm(voltage, 0, 0); + _delay(500); + PhaseCurrent_s c = readAverageCurrents(); + bldc_driver->setPwm(0, 0, 0); + // align phase A + float ab_ratio = c.b ? fabs(c.a / c.b) : 0; + float ac_ratio = c.c ? fabs(c.a / c.c) : 0; + if(_isset(pinB) && ab_ratio > 1.5f ){ // should be ~2 + gain_a *= _sign(c.a); + }else if(_isset(pinC) && ac_ratio > 1.5f ){ // should be ~2 + gain_a *= _sign(c.a); + }else if(_isset(pinB) && ab_ratio < 0.7f ){ // should be ~0.5 + SIMPLEFOC_DEBUG("CS: Switch A-B"); + // switch phase A and B + _swap(pinA, pinB); + _swap(offset_ia, offset_ib); + _swap(gain_a, gain_b); + gain_a *= _sign(c.b); + exit_flag = 2; // signal that pins have been switched + }else if(_isset(pinC) && ac_ratio < 0.7f ){ // should be ~0.5 + SIMPLEFOC_DEBUG("CS: Switch A-C"); + // switch phase A and C + _swap(pinA, pinC); + _swap(offset_ia, offset_ic); + _swap(gain_a, gain_c); + gain_a *= _sign(c.c); + exit_flag = 2;// signal that pins have been switched + }else{ + SIMPLEFOC_DEBUG("CS: Err read A"); + // error in current sense - phase either not measured or bad connection + return 0; + } + } + + if(_isset(pinB)){ + // set phase B active and phases A and C down + bldc_driver->setPwm(0, voltage, 0); + _delay(500); + PhaseCurrent_s c = readAverageCurrents(); + bldc_driver->setPwm(0, 0, 0); + float ba_ratio = c.a ? fabs(c.b / c.a) : 0; + float bc_ratio = c.c ? fabs(c.b / c.c) : 0; + if(_isset(pinA) && ba_ratio > 1.5f ){ // should be ~2); + gain_b *= _sign(c.b); + }else if(_isset(pinC) && bc_ratio > 1.5f ){ // should be ~2 + gain_b *= _sign(c.b); + }else if(_isset(pinA) && ba_ratio < 0.7f ){ // it should be ~0.5 + SIMPLEFOC_DEBUG("CS: Switch B-A"); + // switch phase A and B + _swap(pinB, pinA); + _swap(offset_ib, offset_ia); + _swap(gain_b, gain_a); + gain_b *= _sign(c.a); + exit_flag = 2; // signal that pins have been switched + }else if(_isset(pinC) && bc_ratio < 0.7f ){ // should be ~0.5 + SIMPLEFOC_DEBUG("CS: Switch B-C"); + _swap(pinB, pinC); + _swap(offset_ib, offset_ic); + _swap(gain_b, gain_c); + gain_b *= _sign(c.c); + exit_flag = 2; // signal that pins have been switched + }else{ + SIMPLEFOC_DEBUG("CS: Error read B"); + // error in current sense - phase either not measured or bad connection + return 0; + } + } + + // if phase C measured + if(_isset(pinC)){ + // set phase C active and phases A and B down + bldc_driver->setPwm(0, 0, voltage); + _delay(500); + PhaseCurrent_s c = readAverageCurrents(); + bldc_driver->setPwm(0, 0, 0); + float ca_ratio = c.a ? fabs(c.c / c.a) : 0; + float cb_ratio = c.b ? fabs(c.c / c.b) : 0; + if(_isset(pinA) && ca_ratio > 1.5f ){ // should be ~2 + gain_c *= _sign(c.c); + }else if(_isset(pinB) && cb_ratio > 1.5f ){ // should be ~2 + gain_c *= _sign(c.c); + }else if(_isset(pinA) && ca_ratio < 0.7f ){ // it should be ~0.5 + SIMPLEFOC_DEBUG("CS: Switch C-A"); + // switch phase A and C + _swap(pinC, pinA); + _swap(offset_ic, offset_ia); + _swap(gain_c, gain_a); + gain_c *= _sign(c.a); + exit_flag = 2; // signal that pins have been switched + }else if(_isset(pinB) && cb_ratio < 0.7f ){ // should be ~0.5 + SIMPLEFOC_DEBUG("CS: Switch C-B"); + _swap(pinC, pinB); + _swap(offset_ic, offset_ib); + _swap(gain_c, gain_b); + gain_b *= _sign(c.b); + exit_flag = 2; // signal that pins have been switched + }else{ + SIMPLEFOC_DEBUG("CS: Err read C"); + // error in current sense - phase either not measured or bad connection + return 0; + } + } + // add 2 if pin gains negative + if(gain_a < 0 || gain_b < 0 || gain_c < 0) exit_flag +=2; + return exit_flag; +} + + +// Function aligning the current sense with motor driver +// if all pins are connected well none of this is really necessary! - can be avoided +// returns flag +// 0 - fail +// 1 - success and nothing changed +// 2 - success but pins reconfigured +// 3 - success but gains inverted +// 4 - success but pins reconfigured and gains inverted +int CurrentSense::alignStepperDriver(float voltage, StepperDriver* stepper_driver){ + + int exit_flag = 1; + + if(_isset(pinA)){ + // set phase A active to high and B to low + stepper_driver->setPwm(voltage, 0); + _delay(500); + PhaseCurrent_s c = readAverageCurrents(); + // disable the phases + stepper_driver->setPwm(0, 0); + if (fabs(c.a) < 0.1f && fabs(c.b) < 0.1f ){ + SIMPLEFOC_DEBUG("CS: Err too low current!"); + return 0; // measurement current too low + } + // align phase A + // check if measured current a is positive and invert if not + // check if current b is around zero and if its not + // check if current a is near zero and if it is invert them + if (fabs(c.a) < fabs(c.b)){ + SIMPLEFOC_DEBUG("CS: Switch A-B"); + // switch phase A and B + _swap(pinA, pinB); + _swap(offset_ia, offset_ib); + _swap(gain_a, gain_b); + gain_a *= _sign(c.b); + exit_flag = 2; // signal that pins have been switched + }else if (c.a < 0){ + SIMPLEFOC_DEBUG("CS: Neg A"); + gain_a *= -1; + } + } + + if(_isset(pinB)){ + // set phase B active and phases A and C down + stepper_driver->setPwm(voltage, 0); + _delay(500); + PhaseCurrent_s c = readAverageCurrents(); + stepper_driver->setPwm(0, 0); + if (fabs(c.a) < 0.1f && fabs(c.b) < 0.1f ){ + SIMPLEFOC_DEBUG("CS: Err too low current!"); + return 0; // measurement current too low + } + // align phase A + // check if measured current a is positive and invert if not + if (c.b < 0){ + SIMPLEFOC_DEBUG("CS: Neg B"); + gain_b *= -1; + } + } + + // add 2 if pin gains negative + if(gain_a < 0 || gain_b < 0 ) exit_flag +=2; + return exit_flag; +} + + diff --git a/src/common/base_classes/CurrentSense.h b/src/common/base_classes/CurrentSense.h index 1c839053..c9c79650 100644 --- a/src/common/base_classes/CurrentSense.h +++ b/src/common/base_classes/CurrentSense.h @@ -1,12 +1,15 @@ #ifndef CURRENTSENSE_H #define CURRENTSENSE_H -#include "BLDCDriver.h" +#include "FOCDriver.h" #include "../foc_utils.h" +#include "../time_utils.h" +#include "StepperDriver.h" +#include "BLDCDriver.h" /** * Current sensing abstract class defintion - * Each current sensoring implementation needs to extend this interface + * Each current sensing implementation needs to extend this interface */ class CurrentSense{ public: @@ -23,24 +26,49 @@ class CurrentSense{ * Linking the current sense with the motor driver * Only necessary if synchronisation in between the two is required */ - void linkDriver(BLDCDriver *driver); + void linkDriver(FOCDriver *driver); // variables bool skip_align = false; //!< variable signaling that the phase current direction should be verified during initFOC() - BLDCDriver* driver = nullptr; //!< driver link + FOCDriver* driver = nullptr; //!< driver link bool initialized = false; // true if current sense was successfully initialized void* params = 0; //!< pointer to hardware specific parameters of current sensing + DriverType driver_type = DriverType::Unknown; //!< driver type (BLDC or Stepper) + + // ADC measurement gain for each phase + // support for different gains for different phases of more commonly - inverted phase currents + // this should be automated later + float gain_a; //!< phase A gain + float gain_b; //!< phase B gain + float gain_c; //!< phase C gain + + float offset_ia; //!< zero current A voltage value (center of the adc reading) + float offset_ib; //!< zero current B voltage value (center of the adc reading) + float offset_ic; //!< zero current C voltage value (center of the adc reading) + + // hardware variables + int pinA; //!< pin A analog pin for current measurement + int pinB; //!< pin B analog pin for current measurement + int pinC; //!< pin C analog pin for current measurement + /** * Function intended to verify if: * - phase current are oriented properly * - if their order is the same as driver phases * * This function corrects the alignment errors if possible ans if no such thing is needed it can be left empty (return 1) - * @returns - 0 - for failure & positive number (with status) - for success + * @returns - + 0 - failure + 1 - success and nothing changed + 2 - success but pins reconfigured + 3 - success but gains inverted + 4 - success but pins reconfigured and gains inverted + * + * IMPORTANT: Default implementation provided in the CurrentSense class, but can be overriden in the child classes */ - virtual int driverAlign(float align_voltage) = 0; + virtual int driverAlign(float align_voltage); /** * Function rading the phase currents a, b and c @@ -80,7 +108,7 @@ class CurrentSense{ /** * Function used for Park transform in FOC control - * It reads the Alpha Beta currents and electircal angle of the motor + * It reads the Alpha Beta currents and electrical angle of the motor * It returns the D and Q currents * * @param current - phase current @@ -98,6 +126,20 @@ class CurrentSense{ * override it to do something useful. */ virtual void disable(); + + /** + * Function used to align the current sense with the BLDC motor driver + */ + int alignBLDCDriver(float align_voltage, BLDCDriver* driver); + /** + * Function used to align the current sense with the Stepper motor driver + */ + int alignStepperDriver(float align_voltage, StepperDriver* driver); + /** + * Function used to read the average current values over N samples + */ + PhaseCurrent_s readAverageCurrents(int N=100); + }; #endif \ No newline at end of file diff --git a/src/common/base_classes/FOCDriver.h b/src/common/base_classes/FOCDriver.h new file mode 100644 index 00000000..944251a4 --- /dev/null +++ b/src/common/base_classes/FOCDriver.h @@ -0,0 +1,47 @@ +#ifndef FOCDRIVER_H +#define FOCDRIVER_H + +#include "Arduino.h" + + +enum PhaseState : uint8_t { + PHASE_OFF = 0, // both sides of the phase are off + PHASE_ON = 1, // both sides of the phase are driven with PWM, dead time is applied in 6-PWM mode + PHASE_HI = 2, // only the high side of the phase is driven with PWM (6-PWM mode only) + PHASE_LO = 3, // only the low side of the phase is driven with PWM (6-PWM mode only) +}; + + +enum DriverType{ + Unknown=0, + BLDC=1, + Stepper=2 +}; + +/** + * FOC driver class + */ +class FOCDriver{ + public: + + /** Initialise hardware */ + virtual int init() = 0; + /** Enable hardware */ + virtual void enable() = 0; + /** Disable hardware */ + virtual void disable() = 0; + + long pwm_frequency; //!< pwm frequency value in hertz + float voltage_power_supply; //!< power supply voltage + float voltage_limit; //!< limiting voltage set to the motor + + bool initialized = false; //!< true if driver was successfully initialized + void* params = 0; //!< pointer to hardware specific parameters of driver + + bool enable_active_high = true; //!< enable pin should be set to high to enable the driver (default is HIGH) + + /** get the driver type*/ + virtual DriverType type() = 0; +}; + +#endif diff --git a/src/common/base_classes/StepperDriver.h b/src/common/base_classes/StepperDriver.h index 2006fc8c..9864b235 100644 --- a/src/common/base_classes/StepperDriver.h +++ b/src/common/base_classes/StepperDriver.h @@ -1,32 +1,30 @@ #ifndef STEPPERDRIVER_H #define STEPPERDRIVER_H -#include "drivers/hardware_api.h" +#include "Arduino.h" +#include "FOCDriver.h" -class StepperDriver{ +class StepperDriver: public FOCDriver{ public: - /** Initialise hardware */ - virtual int init() = 0; - /** Enable hardware */ - virtual void enable() = 0; - /** Disable hardware */ - virtual void disable() = 0; - - long pwm_frequency; //!< pwm frequency value in hertz - float voltage_power_supply; //!< power supply voltage - float voltage_limit; //!< limiting voltage set to the motor - - bool initialized = false; // true if driver was successfully initialized - void* params = 0; // pointer to hardware specific parameters of driver - /** - * Set phase voltages to the harware + * Set phase voltages to the hardware * * @param Ua phase A voltage * @param Ub phase B voltage */ virtual void setPwm(float Ua, float Ub) = 0; + + /** + * Set phase state, enable/disable + * + * @param sc - phase A state : active / disabled ( high impedance ) + * @param sb - phase B state : active / disabled ( high impedance ) + */ + virtual void setPhaseState(PhaseState sa, PhaseState sb) = 0; + + /** driver type getter function */ + virtual DriverType type() override { return DriverType::Stepper; } ; }; #endif \ No newline at end of file diff --git a/src/common/foc_utils.h b/src/common/foc_utils.h index f2bc9ef6..9a5b53ea 100644 --- a/src/common/foc_utils.h +++ b/src/common/foc_utils.h @@ -14,6 +14,8 @@ #define _UNUSED(v) (void) (v) #define _powtwo(x) (1 << (x)) +#define _swap(a, b) { auto temp = a; a = b; b = temp; } + // utility defines #define _2_SQRT3 1.15470053838f #define _SQRT3 1.73205080757f diff --git a/src/communication/SimpleFOCDebug.h b/src/communication/SimpleFOCDebug.h index 4fcfd538..93686650 100644 --- a/src/communication/SimpleFOCDebug.h +++ b/src/communication/SimpleFOCDebug.h @@ -32,6 +32,7 @@ * **/ +// #define SIMPLEFOC_DISABLE_DEBUG #ifndef SIMPLEFOC_DISABLE_DEBUG @@ -63,10 +64,6 @@ class SimpleFOCDebug { #define SIMPLEFOC_DEBUG(msg, ...) \ SimpleFOCDebug::println(F(msg), ##__VA_ARGS__) - - - - #else //ifndef SIMPLEFOC_DISABLE_DEBUG diff --git a/src/current_sense/GenericCurrentSense.cpp b/src/current_sense/GenericCurrentSense.cpp index 635535fa..9d90f0ca 100644 --- a/src/current_sense/GenericCurrentSense.cpp +++ b/src/current_sense/GenericCurrentSense.cpp @@ -54,110 +54,10 @@ PhaseCurrent_s GenericCurrentSense::getPhaseCurrents(){ // returns flag // 0 - fail // 1 - success and nothing changed -// 2 - success but pins reconfigured -// 3 - success but gains inverted -// 4 - success but pins reconfigured and gains inverted int GenericCurrentSense::driverAlign(float voltage){ _UNUSED(voltage) ; // remove unused parameter warning int exit_flag = 1; if(skip_align) return exit_flag; - if (!initialized) return 0; - - // // set phase A active and phases B and C down - // driver->setPwm(voltage, 0, 0); - // _delay(200); - // PhaseCurrent_s c = getPhaseCurrents(); - // // read the current 100 times ( arbitrary number ) - // for (int i = 0; i < 100; i++) { - // PhaseCurrent_s c1 = getPhaseCurrents(); - // c.a = c.a*0.6f + 0.4f*c1.a; - // c.b = c.b*0.6f + 0.4f*c1.b; - // c.c = c.c*0.6f + 0.4f*c1.c; - // _delay(3); - // } - // driver->setPwm(0, 0, 0); - // // align phase A - // float ab_ratio = fabs(c.a / c.b); - // float ac_ratio = c.c ? fabs(c.a / c.c) : 0; - // if( ab_ratio > 1.5f ){ // should be ~2 - // gain_a *= _sign(c.a); - // }else if( ab_ratio < 0.7f ){ // should be ~0.5 - // // switch phase A and B - // int tmp_pinA = pinA; - // pinA = pinB; - // pinB = tmp_pinA; - // gain_a *= _sign(c.b); - // exit_flag = 2; // signal that pins have been switched - // }else if(_isset(pinC) && ac_ratio < 0.7f ){ // should be ~0.5 - // // switch phase A and C - // int tmp_pinA = pinA; - // pinA = pinC; - // pinC= tmp_pinA; - // gain_a *= _sign(c.c); - // exit_flag = 2;// signal that pins have been switched - // }else{ - // // error in current sense - phase either not measured or bad connection - // return 0; - // } - - // // set phase B active and phases A and C down - // driver->setPwm(0, voltage, 0); - // _delay(200); - // c = getPhaseCurrents(); - // // read the current 50 times - // for (int i = 0; i < 100; i++) { - // PhaseCurrent_s c1 = getPhaseCurrents(); - // c.a = c.a*0.6f + 0.4f*c1.a; - // c.b = c.b*0.6f + 0.4f*c1.b; - // c.c = c.c*0.6f + 0.4f*c1.c; - // _delay(3); - // } - // driver->setPwm(0, 0, 0); - // float ba_ratio = fabs(c.b/c.a); - // float bc_ratio = c.c ? fabs(c.b / c.c) : 0; - // if( ba_ratio > 1.5f ){ // should be ~2 - // gain_b *= _sign(c.b); - // }else if( ba_ratio < 0.7f ){ // it should be ~0.5 - // // switch phase A and B - // int tmp_pinB = pinB; - // pinB = pinA; - // pinA = tmp_pinB; - // gain_b *= _sign(c.a); - // exit_flag = 2; // signal that pins have been switched - // }else if(_isset(pinC) && bc_ratio < 0.7f ){ // should be ~0.5 - // // switch phase A and C - // int tmp_pinB = pinB; - // pinB = pinC; - // pinC = tmp_pinB; - // gain_b *= _sign(c.c); - // exit_flag = 2; // signal that pins have been switched - // }else{ - // // error in current sense - phase either not measured or bad connection - // return 0; - // } - - // // if phase C measured - // if(_isset(pinC)){ - // // set phase B active and phases A and C down - // driver->setPwm(0, 0, voltage); - // _delay(200); - // c = getPhaseCurrents(); - // // read the adc voltage 500 times ( arbitrary number ) - // for (int i = 0; i < 50; i++) { - // PhaseCurrent_s c1 = getPhaseCurrents(); - // c.c = (c.c+c1.c)/50.0f; - // } - // driver->setPwm(0, 0, 0); - // gain_c *= _sign(c.c); - // } - - // if(gain_a < 0 || gain_b < 0 || gain_c < 0) exit_flag +=2; - // exit flag is either - // 0 - fail - // 1 - success and nothing changed - // 2 - success but pins reconfigured - // 3 - success but gains inverted - // 4 - success but pins reconfigured and gains inverted return exit_flag; } diff --git a/src/current_sense/InlineCurrentSense.cpp b/src/current_sense/InlineCurrentSense.cpp index c3db74ef..35c97765 100644 --- a/src/current_sense/InlineCurrentSense.cpp +++ b/src/current_sense/InlineCurrentSense.cpp @@ -44,8 +44,14 @@ int InlineCurrentSense::init(){ params = _configureADCInline(drv_params,pinA,pinB,pinC); // if init failed return fail if (params == SIMPLEFOC_CURRENT_SENSE_INIT_FAILED) return 0; + // set the center pwm (0 voltage vector) + if(driver_type==DriverType::BLDC) + static_cast(driver)->setPwm(driver->voltage_limit/2, driver->voltage_limit/2, driver->voltage_limit/2); // calibrate zero offsets calibrateOffsets(); + // set zero voltage to all phases + if(driver_type==DriverType::BLDC) + static_cast(driver)->setPwm(0,0,0); // set the initialized flag initialized = (params!=SIMPLEFOC_CURRENT_SENSE_INIT_FAILED); // return success @@ -80,174 +86,3 @@ PhaseCurrent_s InlineCurrentSense::getPhaseCurrents(){ current.c = (!_isset(pinC)) ? 0 : (_readADCVoltageInline(pinC, params) - offset_ic)*gain_c; // amps return current; } - -// Function aligning the current sense with motor driver -// if all pins are connected well none of this is really necessary! - can be avoided -// returns flag -// 0 - fail -// 1 - success and nothing changed -// 2 - success but pins reconfigured -// 3 - success but gains inverted -// 4 - success but pins reconfigured and gains inverted -int InlineCurrentSense::driverAlign(float voltage){ - - int exit_flag = 1; - if(skip_align) return exit_flag; - - if (driver==nullptr) { - SIMPLEFOC_DEBUG("CUR: No driver linked!"); - return 0; - } - - if (!initialized) return 0; - - if(_isset(pinA)){ - // set phase A active and phases B and C down - driver->setPwm(voltage, 0, 0); - _delay(2000); - PhaseCurrent_s c = getPhaseCurrents(); - // read the current 100 times ( arbitrary number ) - for (int i = 0; i < 100; i++) { - PhaseCurrent_s c1 = getPhaseCurrents(); - c.a = c.a*0.6f + 0.4f*c1.a; - c.b = c.b*0.6f + 0.4f*c1.b; - c.c = c.c*0.6f + 0.4f*c1.c; - _delay(3); - } - driver->setPwm(0, 0, 0); - // align phase A - float ab_ratio = c.b ? fabs(c.a / c.b) : 0; - float ac_ratio = c.c ? fabs(c.a / c.c) : 0; - if(_isset(pinB) && ab_ratio > 1.5f ){ // should be ~2 - gain_a *= _sign(c.a); - }else if(_isset(pinC) && ac_ratio > 1.5f ){ // should be ~2 - gain_a *= _sign(c.a); - }else if(_isset(pinB) && ab_ratio < 0.7f ){ // should be ~0.5 - // switch phase A and B - int tmp_pinA = pinA; - pinA = pinB; - pinB = tmp_pinA; - float tmp_offsetA = offset_ia; - offset_ia = offset_ib; - offset_ib = tmp_offsetA; - gain_a *= _sign(c.b); - exit_flag = 2; // signal that pins have been switched - }else if(_isset(pinC) && ac_ratio < 0.7f ){ // should be ~0.5 - // switch phase A and C - int tmp_pinA = pinA; - pinA = pinC; - pinC= tmp_pinA; - float tmp_offsetA = offset_ia; - offset_ia = offset_ic; - offset_ic = tmp_offsetA; - gain_a *= _sign(c.c); - exit_flag = 2;// signal that pins have been switched - }else{ - // error in current sense - phase either not measured or bad connection - return 0; - } - } - - if(_isset(pinB)){ - // set phase B active and phases A and C down - driver->setPwm(0, voltage, 0); - _delay(200); - PhaseCurrent_s c = getPhaseCurrents(); - // read the current 50 times - for (int i = 0; i < 100; i++) { - PhaseCurrent_s c1 = getPhaseCurrents(); - c.a = c.a*0.6f + 0.4f*c1.a; - c.b = c.b*0.6f + 0.4f*c1.b; - c.c = c.c*0.6f + 0.4f*c1.c; - _delay(3); - } - driver->setPwm(0, 0, 0); - float ba_ratio = c.a ? fabs(c.b / c.a) : 0; - float bc_ratio = c.c ? fabs(c.b / c.c) : 0; - if(_isset(pinA) && ba_ratio > 1.5f ){ // should be ~2 - gain_b *= _sign(c.b); - }else if(_isset(pinC) && bc_ratio > 1.5f ){ // should be ~2 - gain_b *= _sign(c.b); - }else if(_isset(pinA) && ba_ratio < 0.7f ){ // it should be ~0.5 - // switch phase A and B - int tmp_pinB = pinB; - pinB = pinA; - pinA = tmp_pinB; - float tmp_offsetB = offset_ib; - offset_ib = offset_ia; - offset_ia = tmp_offsetB; - gain_b *= _sign(c.a); - exit_flag = 2; // signal that pins have been switched - }else if(_isset(pinC) && bc_ratio < 0.7f ){ // should be ~0.5 - // switch phase A and C - int tmp_pinB = pinB; - pinB = pinC; - pinC = tmp_pinB; - float tmp_offsetB = offset_ib; - offset_ib = offset_ic; - offset_ic = tmp_offsetB; - gain_b *= _sign(c.c); - exit_flag = 2; // signal that pins have been switched - }else{ - // error in current sense - phase either not measured or bad connection - return 0; - } - } - - // if phase C measured - if(_isset(pinC)){ - // set phase C active and phases A and B down - driver->setPwm(0, 0, voltage); - _delay(200); - PhaseCurrent_s c = getPhaseCurrents(); - // read the adc voltage 500 times ( arbitrary number ) - for (int i = 0; i < 100; i++) { - PhaseCurrent_s c1 = getPhaseCurrents(); - c.a = c.a*0.6f + 0.4f*c1.a; - c.b = c.b*0.6f + 0.4f*c1.b; - c.c = c.c*0.6f + 0.4f*c1.c; - _delay(3); - } - driver->setPwm(0, 0, 0); - float ca_ratio = c.a ? fabs(c.c / c.a) : 0; - float cb_ratio = c.b ? fabs(c.c / c.b) : 0; - if(_isset(pinA) && ca_ratio > 1.5f ){ // should be ~2 - gain_c *= _sign(c.c); - }else if(_isset(pinB) && cb_ratio > 1.5f ){ // should be ~2 - gain_c *= _sign(c.c); - }else if(_isset(pinA) && ca_ratio < 0.7f ){ // it should be ~0.5 - // switch phase A and C - int tmp_pinC = pinC; - pinC = pinA; - pinA = tmp_pinC; - float tmp_offsetC = offset_ic; - offset_ic = offset_ia; - offset_ia = tmp_offsetC; - gain_c *= _sign(c.a); - exit_flag = 2; // signal that pins have been switched - }else if(_isset(pinB) && cb_ratio < 0.7f ){ // should be ~0.5 - // switch phase B and C - int tmp_pinC = pinC; - pinC = pinB; - pinB = tmp_pinC; - float tmp_offsetC = offset_ic; - offset_ic = offset_ib; - offset_ib = tmp_offsetC; - gain_c *= _sign(c.b); - exit_flag = 2; // signal that pins have been switched - }else{ - // error in current sense - phase either not measured or bad connection - return 0; - } - } - - if(gain_a < 0 || gain_b < 0 || gain_c < 0) exit_flag +=2; - // exit flag is either - // 0 - fail - // 1 - success and nothing changed - // 2 - success but pins reconfigured - // 3 - success but gains inverted - // 4 - success but pins reconfigured and gains inverted - - return exit_flag; -} diff --git a/src/current_sense/InlineCurrentSense.h b/src/current_sense/InlineCurrentSense.h index 5fdca7d7..94be0f75 100644 --- a/src/current_sense/InlineCurrentSense.h +++ b/src/current_sense/InlineCurrentSense.h @@ -6,10 +6,13 @@ #include "../common/time_utils.h" #include "../common/defaults.h" #include "../common/base_classes/CurrentSense.h" +#include "../common/base_classes/StepperDriver.h" +#include "../common/base_classes/BLDCDriver.h" #include "../common/lowpass_filter.h" #include "hardware_api.h" + class InlineCurrentSense: public CurrentSense{ public: /** @@ -33,31 +36,9 @@ class InlineCurrentSense: public CurrentSense{ // CurrentSense interface implementing functions int init() override; PhaseCurrent_s getPhaseCurrents() override; - int driverAlign(float align_voltage) override; - - // ADC measuremnet gain for each phase - // support for different gains for different phases of more commonly - inverted phase currents - // this should be automated later - float gain_a; //!< phase A gain - float gain_b; //!< phase B gain - float gain_c; //!< phase C gain - - // // per phase low pass fileters - // LowPassFilter lpf_a{DEF_LPF_PER_PHASE_CURRENT_SENSE_Tf}; //!< current A low pass filter - // LowPassFilter lpf_b{DEF_LPF_PER_PHASE_CURRENT_SENSE_Tf}; //!< current B low pass filter - // LowPassFilter lpf_c{DEF_LPF_PER_PHASE_CURRENT_SENSE_Tf}; //!< current C low pass filter - float offset_ia; //!< zero current A voltage value (center of the adc reading) - float offset_ib; //!< zero current B voltage value (center of the adc reading) - float offset_ic; //!< zero current C voltage value (center of the adc reading) - private: - // hardware variables - int pinA; //!< pin A analog pin for current measurement - int pinB; //!< pin B analog pin for current measurement - int pinC; //!< pin C analog pin for current measurement - // gain variables float shunt_resistor; //!< Shunt resistor value float amp_gain; //!< amp gain value diff --git a/src/current_sense/LowsideCurrentSense.cpp b/src/current_sense/LowsideCurrentSense.cpp index aeb8dea0..3aeb7350 100644 --- a/src/current_sense/LowsideCurrentSense.cpp +++ b/src/current_sense/LowsideCurrentSense.cpp @@ -48,8 +48,14 @@ int LowsideCurrentSense::init(){ if (params == SIMPLEFOC_CURRENT_SENSE_INIT_FAILED) return 0; // sync the driver _driverSyncLowSide(driver->params, params); + // set the center pwm (0 voltage vector) + if(driver_type==DriverType::BLDC) + static_cast(driver)->setPwm(driver->voltage_limit/2, driver->voltage_limit/2, driver->voltage_limit/2); // calibrate zero offsets calibrateOffsets(); + // set zero voltage to all phases + if(driver_type==DriverType::BLDC) + static_cast(driver)->setPwm(0,0,0); // set the initialized flag initialized = (params!=SIMPLEFOC_CURRENT_SENSE_INIT_FAILED); // return success @@ -86,169 +92,3 @@ PhaseCurrent_s LowsideCurrentSense::getPhaseCurrents(){ current.c = (!_isset(pinC)) ? 0 : (_readADCVoltageLowSide(pinC, params) - offset_ic)*gain_c; // amps return current; } - -// Function aligning the current sense with motor driver -// if all pins are connected well none of this is really necessary! - can be avoided -// returns flag -// 0 - fail -// 1 - success and nothing changed -// 2 - success but pins reconfigured -// 3 - success but gains inverted -// 4 - success but pins reconfigured and gains inverted -int LowsideCurrentSense::driverAlign(float voltage){ - - int exit_flag = 1; - if(skip_align) return exit_flag; - - if (!initialized) return 0; - - if(_isset(pinA)){ - // set phase A active and phases B and C down - driver->setPwm(voltage, 0, 0); - _delay(2000); - PhaseCurrent_s c = getPhaseCurrents(); - // read the current 100 times ( arbitrary number ) - for (int i = 0; i < 100; i++) { - PhaseCurrent_s c1 = getPhaseCurrents(); - c.a = c.a*0.6f + 0.4f*c1.a; - c.b = c.b*0.6f + 0.4f*c1.b; - c.c = c.c*0.6f + 0.4f*c1.c; - _delay(3); - } - driver->setPwm(0, 0, 0); - // align phase A - float ab_ratio = c.b ? fabs(c.a / c.b) : 0; - float ac_ratio = c.c ? fabs(c.a / c.c) : 0; - if(_isset(pinB) && ab_ratio > 1.5f ){ // should be ~2 - gain_a *= _sign(c.a); - }else if(_isset(pinC) && ac_ratio > 1.5f ){ // should be ~2 - gain_a *= _sign(c.a); - }else if(_isset(pinB) && ab_ratio < 0.7f ){ // should be ~0.5 - // switch phase A and B - int tmp_pinA = pinA; - pinA = pinB; - pinB = tmp_pinA; - float tmp_offsetA = offset_ia; - offset_ia = offset_ib; - offset_ib = tmp_offsetA; - gain_a *= _sign(c.b); - exit_flag = 2; // signal that pins have been switched - }else if(_isset(pinC) && ac_ratio < 0.7f ){ // should be ~0.5 - // switch phase A and C - int tmp_pinA = pinA; - pinA = pinC; - pinC= tmp_pinA; - float tmp_offsetA = offset_ia; - offset_ia = offset_ic; - offset_ic = tmp_offsetA; - gain_a *= _sign(c.c); - exit_flag = 2;// signal that pins have been switched - }else{ - // error in current sense - phase either not measured or bad connection - return 0; - } - } - - if(_isset(pinB)){ - // set phase B active and phases A and C down - driver->setPwm(0, voltage, 0); - _delay(200); - PhaseCurrent_s c = getPhaseCurrents(); - // read the current 50 times - for (int i = 0; i < 100; i++) { - PhaseCurrent_s c1 = getPhaseCurrents(); - c.a = c.a*0.6f + 0.4f*c1.a; - c.b = c.b*0.6f + 0.4f*c1.b; - c.c = c.c*0.6f + 0.4f*c1.c; - _delay(3); - } - driver->setPwm(0, 0, 0); - float ba_ratio = c.a ? fabs(c.b / c.a) : 0; - float bc_ratio = c.c ? fabs(c.b / c.c) : 0; - if(_isset(pinA) && ba_ratio > 1.5f ){ // should be ~2 - gain_b *= _sign(c.b); - }else if(_isset(pinC) && bc_ratio > 1.5f ){ // should be ~2 - gain_b *= _sign(c.b); - }else if(_isset(pinA) && ba_ratio < 0.7f ){ // it should be ~0.5 - // switch phase A and B - int tmp_pinB = pinB; - pinB = pinA; - pinA = tmp_pinB; - float tmp_offsetB = offset_ib; - offset_ib = offset_ia; - offset_ia = tmp_offsetB; - gain_b *= _sign(c.a); - exit_flag = 2; // signal that pins have been switched - }else if(_isset(pinC) && bc_ratio < 0.7f ){ // should be ~0.5 - // switch phase A and C - int tmp_pinB = pinB; - pinB = pinC; - pinC = tmp_pinB; - float tmp_offsetB = offset_ib; - offset_ib = offset_ic; - offset_ic = tmp_offsetB; - gain_b *= _sign(c.c); - exit_flag = 2; // signal that pins have been switched - }else{ - // error in current sense - phase either not measured or bad connection - return 0; - } - } - - // if phase C measured - if(_isset(pinC)){ - // set phase C active and phases A and B down - driver->setPwm(0, 0, voltage); - _delay(200); - PhaseCurrent_s c = getPhaseCurrents(); - // read the adc voltage 500 times ( arbitrary number ) - for (int i = 0; i < 100; i++) { - PhaseCurrent_s c1 = getPhaseCurrents(); - c.a = c.a*0.6f + 0.4f*c1.a; - c.b = c.b*0.6f + 0.4f*c1.b; - c.c = c.c*0.6f + 0.4f*c1.c; - _delay(3); - } - driver->setPwm(0, 0, 0); - float ca_ratio = c.a ? fabs(c.c / c.a) : 0; - float cb_ratio = c.b ? fabs(c.c / c.b) : 0; - if(_isset(pinA) && ca_ratio > 1.5f ){ // should be ~2 - gain_c *= _sign(c.c); - }else if(_isset(pinB) && cb_ratio > 1.5f ){ // should be ~2 - gain_c *= _sign(c.c); - }else if(_isset(pinA) && ca_ratio < 0.7f ){ // it should be ~0.5 - // switch phase A and C - int tmp_pinC = pinC; - pinC = pinA; - pinA = tmp_pinC; - float tmp_offsetC = offset_ic; - offset_ic = offset_ia; - offset_ia = tmp_offsetC; - gain_c *= _sign(c.a); - exit_flag = 2; // signal that pins have been switched - }else if(_isset(pinB) && cb_ratio < 0.7f ){ // should be ~0.5 - // switch phase B and C - int tmp_pinC = pinC; - pinC = pinB; - pinB = tmp_pinC; - float tmp_offsetC = offset_ic; - offset_ic = offset_ib; - offset_ib = tmp_offsetC; - gain_c *= _sign(c.b); - exit_flag = 2; // signal that pins have been switched - }else{ - // error in current sense - phase either not measured or bad connection - return 0; - } - } - - if(gain_a < 0 || gain_b < 0 || gain_c < 0) exit_flag +=2; - // exit flag is either - // 0 - fail - // 1 - success and nothing changed - // 2 - success but pins reconfigured - // 3 - success but gains inverted - // 4 - success but pins reconfigured and gains inverted - - return exit_flag; -} diff --git a/src/current_sense/LowsideCurrentSense.h b/src/current_sense/LowsideCurrentSense.h index 1652b330..6651bcd2 100644 --- a/src/current_sense/LowsideCurrentSense.h +++ b/src/current_sense/LowsideCurrentSense.h @@ -7,6 +7,8 @@ #include "../common/defaults.h" #include "../common/base_classes/CurrentSense.h" #include "../common/base_classes/FOCMotor.h" +#include "../common/base_classes/StepperDriver.h" +#include "../common/base_classes/BLDCDriver.h" #include "../common/lowpass_filter.h" #include "hardware_api.h" @@ -34,30 +36,9 @@ class LowsideCurrentSense: public CurrentSense{ // CurrentSense interface implementing functions int init() override; PhaseCurrent_s getPhaseCurrents() override; - int driverAlign(float align_voltage) override; - // ADC measuremnet gain for each phase - // support for different gains for different phases of more commonly - inverted phase currents - // this should be automated later - float gain_a; //!< phase A gain - float gain_b; //!< phase B gain - float gain_c; //!< phase C gain - - // // per phase low pass fileters - // LowPassFilter lpf_a{DEF_LPF_PER_PHASE_CURRENT_SENSE_Tf}; //!< current A low pass filter - // LowPassFilter lpf_b{DEF_LPF_PER_PHASE_CURRENT_SENSE_Tf}; //!< current B low pass filter - // LowPassFilter lpf_c{DEF_LPF_PER_PHASE_CURRENT_SENSE_Tf}; //!< current C low pass filter - - float offset_ia; //!< zero current A voltage value (center of the adc reading) - float offset_ib; //!< zero current B voltage value (center of the adc reading) - float offset_ic; //!< zero current C voltage value (center of the adc reading) private: - // hardware variables - int pinA; //!< pin A analog pin for current measurement - int pinB; //!< pin B analog pin for current measurement - int pinC; //!< pin C analog pin for current measurement - // gain variables float shunt_resistor; //!< Shunt resistor value float amp_gain; //!< amp gain value diff --git a/src/drivers/BLDCDriver3PWM.h b/src/drivers/BLDCDriver3PWM.h index 1942f60b..75ee478c 100644 --- a/src/drivers/BLDCDriver3PWM.h +++ b/src/drivers/BLDCDriver3PWM.h @@ -38,10 +38,9 @@ class BLDCDriver3PWM: public BLDCDriver int enableA_pin; //!< enable pin number int enableB_pin; //!< enable pin number int enableC_pin; //!< enable pin number - bool enable_active_high = true; /** - * Set phase voltages to the harware + * Set phase voltages to the hardware * * @param Ua - phase A voltage * @param Ub - phase B voltage @@ -50,7 +49,8 @@ class BLDCDriver3PWM: public BLDCDriver void setPwm(float Ua, float Ub, float Uc) override; /** - * Set phase voltages to the harware + * Set phase voltages to the hardware + * > Only possible is the driver has separate enable pins for all phases! * * @param sc - phase A state : active / disabled ( high impedance ) * @param sb - phase B state : active / disabled ( high impedance ) diff --git a/src/drivers/BLDCDriver6PWM.h b/src/drivers/BLDCDriver6PWM.h index e8643cc5..7ba7631c 100644 --- a/src/drivers/BLDCDriver6PWM.h +++ b/src/drivers/BLDCDriver6PWM.h @@ -37,7 +37,6 @@ class BLDCDriver6PWM: public BLDCDriver int pwmB_h,pwmB_l; //!< phase B pwm pin number int pwmC_h,pwmC_l; //!< phase C pwm pin number int enable_pin; //!< enable pin number - bool enable_active_high = true; float dead_zone; //!< a percentage of dead-time(zone) (both high and low side in low) for each pwm cycle [0,1] diff --git a/src/drivers/StepperDriver2PWM.cpp b/src/drivers/StepperDriver2PWM.cpp index dbbf5b8f..e8ccc6c6 100644 --- a/src/drivers/StepperDriver2PWM.cpp +++ b/src/drivers/StepperDriver2PWM.cpp @@ -44,8 +44,8 @@ StepperDriver2PWM::StepperDriver2PWM(int _pwm1, int _dir1, int _pwm2, int _dir2, // enable motor driver void StepperDriver2PWM::enable(){ // enable_pin the driver - if enable_pin pin available - if ( _isset(enable_pin1) ) digitalWrite(enable_pin1, HIGH); - if ( _isset(enable_pin2) ) digitalWrite(enable_pin2, HIGH); + if ( _isset(enable_pin1) ) digitalWrite(enable_pin1, enable_active_high); + if ( _isset(enable_pin2) ) digitalWrite(enable_pin2, enable_active_high); // set zero to PWM setPwm(0,0); } @@ -56,8 +56,8 @@ void StepperDriver2PWM::disable() // set zero to PWM setPwm(0, 0); // disable the driver - if enable_pin pin available - if ( _isset(enable_pin1) ) digitalWrite(enable_pin1, LOW); - if ( _isset(enable_pin2) ) digitalWrite(enable_pin2, LOW); + if ( _isset(enable_pin1) ) digitalWrite(enable_pin1, !enable_active_high); + if ( _isset(enable_pin2) ) digitalWrite(enable_pin2, !enable_active_high); } @@ -84,6 +84,14 @@ int StepperDriver2PWM::init() { return params!=SIMPLEFOC_DRIVER_INIT_FAILED; } +// Set voltage to the pwm pin +void StepperDriver2PWM::setPhaseState(PhaseState sa, PhaseState sb) { + // disable if needed + if( _isset(enable_pin1) && _isset(enable_pin2)){ + digitalWrite(enable_pin1, sa == PhaseState::PHASE_ON ? enable_active_high:!enable_active_high); + digitalWrite(enable_pin2, sb == PhaseState::PHASE_ON ? enable_active_high:!enable_active_high); + } +} // Set voltage to the pwm pin void StepperDriver2PWM::setPwm(float Ua, float Ub) { diff --git a/src/drivers/StepperDriver2PWM.h b/src/drivers/StepperDriver2PWM.h index b349af06..1bd00db9 100644 --- a/src/drivers/StepperDriver2PWM.h +++ b/src/drivers/StepperDriver2PWM.h @@ -60,6 +60,15 @@ class StepperDriver2PWM: public StepperDriver */ void setPwm(float Ua, float Ub) override; + /** + * Set phase voltages to the hardware + * > Only possible is the driver has separate enable pins for both phases! + * + * @param sa phase A state : active / disabled ( high impedance ) + * @param sb phase B state : active / disabled ( high impedance ) + */ + virtual void setPhaseState(PhaseState sa, PhaseState sb) override; + private: }; diff --git a/src/drivers/StepperDriver4PWM.cpp b/src/drivers/StepperDriver4PWM.cpp index 836f5472..52f1c1d1 100644 --- a/src/drivers/StepperDriver4PWM.cpp +++ b/src/drivers/StepperDriver4PWM.cpp @@ -21,8 +21,8 @@ StepperDriver4PWM::StepperDriver4PWM(int ph1A,int ph1B,int ph2A,int ph2B,int en1 // enable motor driver void StepperDriver4PWM::enable(){ // enable_pin the driver - if enable_pin pin available - if ( _isset(enable_pin1) ) digitalWrite(enable_pin1, HIGH); - if ( _isset(enable_pin2) ) digitalWrite(enable_pin2, HIGH); + if ( _isset(enable_pin1) ) digitalWrite(enable_pin1, enable_active_high); + if ( _isset(enable_pin2) ) digitalWrite(enable_pin2, enable_active_high); // set zero to PWM setPwm(0,0); } @@ -33,8 +33,8 @@ void StepperDriver4PWM::disable() // set zero to PWM setPwm(0, 0); // disable the driver - if enable_pin pin available - if ( _isset(enable_pin1) ) digitalWrite(enable_pin1, LOW); - if ( _isset(enable_pin2) ) digitalWrite(enable_pin2, LOW); + if ( _isset(enable_pin1) ) digitalWrite(enable_pin1, !enable_active_high); + if ( _isset(enable_pin2) ) digitalWrite(enable_pin2, !enable_active_high); } @@ -59,6 +59,15 @@ int StepperDriver4PWM::init() { return params!=SIMPLEFOC_DRIVER_INIT_FAILED; } +// Set voltage to the pwm pin +void StepperDriver4PWM::setPhaseState(PhaseState sa, PhaseState sb) { + // disable if needed + if( _isset(enable_pin1) && _isset(enable_pin2)){ + digitalWrite(enable_pin1, sa == PhaseState::PHASE_ON ? enable_active_high:!enable_active_high); + digitalWrite(enable_pin2, sb == PhaseState::PHASE_ON ? enable_active_high:!enable_active_high); + } +} + // Set voltage to the pwm pin void StepperDriver4PWM::setPwm(float Ualpha, float Ubeta) { diff --git a/src/drivers/StepperDriver4PWM.h b/src/drivers/StepperDriver4PWM.h index e4b2ee42..33e7d0cf 100644 --- a/src/drivers/StepperDriver4PWM.h +++ b/src/drivers/StepperDriver4PWM.h @@ -47,6 +47,16 @@ class StepperDriver4PWM: public StepperDriver */ void setPwm(float Ua, float Ub) override; + + /** + * Set phase voltages to the hardware. + * > Only possible is the driver has separate enable pins for both phases! + * + * @param sa phase A state : active / disabled ( high impedance ) + * @param sb phase B state : active / disabled ( high impedance ) + */ + virtual void setPhaseState(PhaseState sa, PhaseState sb) override; + private: }; From c05289aa01d9682031224196844c9243bb4a4bda Mon Sep 17 00:00:00 2001 From: askuric Date: Mon, 20 May 2024 18:02:23 +0200 Subject: [PATCH 08/57] updated readme --- README.md | 14 ++------------ 1 file changed, 2 insertions(+), 12 deletions(-) diff --git a/README.md b/README.md index 86551f79..9fb24dd1 100644 --- a/README.md +++ b/README.md @@ -28,18 +28,8 @@ Therefore this is an attempt to: - For official driver boards see [SimpleFOCBoards](https://docs.simplefoc.com/boards) - Many many more boards developed by the community members, see [SimpleFOCCommunity](https://community.simplefoc.com/) -> NEW RELEASE 📢 : SimpleFOClibrary v2.3.3 -> - Teensy4 -> - support for low-side current sensing [#392](https://github.com/simplefoc/Arduino-FOC/pull/392) -> - support for center aligned 6pwm and 3pwm (optional) [#392](https://github.com/simplefoc/Arduino-FOC/pull/392) -> - stm32 -> - support for center aligned pwm (even across multiple timers and motors/drivers) [#374](https://github.com/simplefoc/Arduino-FOC/pull/374), [#388](https://github.com/simplefoc/Arduino-FOC/pull/388) -> - support for DMA based low-side current sensing: [#383](https://github.com/simplefoc/Arduino-FOC/pull/383),[#378](https://github.com/simplefoc/Arduino-FOC/pull/378) -> - support for f7 architecture [#388](https://github.com/simplefoc/Arduino-FOC/pull/388),[#394](https://github.com/simplefoc/Arduino-FOC/pull/394) -> - KV rating calculation fix [#347](https://github.com/simplefoc/Arduino-FOC/pull/347) -> - Much more performant Space Vector PWM calculation [#340](https://github.com/simplefoc/Arduino-FOC/pull/340) -> - And much more: -> - See the complete list of bugfixes and new features of v2.3.3 [fixes and PRs](https://github.com/simplefoc/Arduino-FOC/milestone/10?closed=1) +> NEXT RELEASE 📢 : SimpleFOClibrary v2.3.4 +> - Current sensing support for Stepper motors (lowside and inline) ## Arduino *SimpleFOClibrary* v2.3.3 From 602c668fa0ddf0c011b4828bddd28b9b7503beb9 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Wed, 5 Jun 2024 13:19:07 +0200 Subject: [PATCH 09/57] move setting target to start of move() #404 --- src/BLDCMotor.cpp | 5 +++-- src/StepperMotor.cpp | 5 +++-- 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/src/BLDCMotor.cpp b/src/BLDCMotor.cpp index fe14feff..acc5b0fe 100644 --- a/src/BLDCMotor.cpp +++ b/src/BLDCMotor.cpp @@ -393,6 +393,9 @@ void BLDCMotor::loopFOC() { // - if target is not set it uses motor.target value void BLDCMotor::move(float new_target) { + // set internal target variable + if(_isset(new_target)) target = new_target; + // downsampling (optional) if(motion_cnt++ < motion_downsample) return; motion_cnt = 0; @@ -410,8 +413,6 @@ void BLDCMotor::move(float new_target) { // if disabled do nothing if(!enabled) return; - // set internal target variable - if(_isset(new_target)) target = new_target; // calculate the back-emf voltage if KV_rating available U_bemf = vel*(1/KV) if (_isset(KV_rating)) voltage_bemf = shaft_velocity/(KV_rating*_SQRT3)/_RPM_TO_RADS; diff --git a/src/StepperMotor.cpp b/src/StepperMotor.cpp index 5d519f29..19c96cd7 100644 --- a/src/StepperMotor.cpp +++ b/src/StepperMotor.cpp @@ -283,6 +283,9 @@ void StepperMotor::loopFOC() { // - if target is not set it uses motor.target value void StepperMotor::move(float new_target) { + // set internal target variable + if(_isset(new_target) ) target = new_target; + // downsampling (optional) if(motion_cnt++ < motion_downsample) return; motion_cnt = 0; @@ -301,8 +304,6 @@ void StepperMotor::move(float new_target) { // if disabled do nothing if(!enabled) return; - // set internal target variable - if(_isset(new_target) ) target = new_target; // calculate the back-emf voltage if KV_rating available U_bemf = vel*(1/KV) if (_isset(KV_rating)) voltage_bemf = shaft_velocity/(KV_rating*_SQRT3)/_RPM_TO_RADS; From f1b7452ad557b13ae2f44d343fff89a2534ff98e Mon Sep 17 00:00:00 2001 From: ABA Date: Sat, 11 May 2024 23:56:58 +0200 Subject: [PATCH 10/57] add MT6701 I2C sensor configuration (cherry picked from commit b25ef8e59be1caa80c40da203d7967c8f4e91ee6) --- src/sensors/MagneticSensorI2C.cpp | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/src/sensors/MagneticSensorI2C.cpp b/src/sensors/MagneticSensorI2C.cpp index 7b4ea77e..2b3db0c2 100644 --- a/src/sensors/MagneticSensorI2C.cpp +++ b/src/sensors/MagneticSensorI2C.cpp @@ -16,6 +16,14 @@ MagneticSensorI2CConfig_s AS5048_I2C = { .data_start_bit = 15 }; +/** Typical configuration for the 12bit MT6701 magnetic sensor over I2C interface */ +MagneticSensorI2CConfig_s MT6701_I2C = { + .chip_address = 0x06, + .bit_resolution = 14, + .angle_register = 0x03, + .data_start_bit = 15 +}; + // MagneticSensorI2C(uint8_t _chip_address, float _cpr, uint8_t _angle_register_msb) // @param _chip_address I2C chip address @@ -34,6 +42,7 @@ MagneticSensorI2C::MagneticSensorI2C(uint8_t _chip_address, int _bit_resolution, // LSB and MSB register used bits // AS5600 uses 0..7 LSB and 8..11 MSB // AS5048 uses 0..5 LSB and 6..13 MSB + // MT6701 uses 0..5 LSB and 9..15 MSB // used bits in LSB lsb_used = _bit_resolution - _bits_used_msb; // extraction masks @@ -111,6 +120,7 @@ int MagneticSensorI2C::read(uint8_t angle_reg_msb) { // LSB and MSB register used bits // AS5600 uses 0..7 LSB and 8..11 MSB // AS5048 uses 0..5 LSB and 6..13 MSB + // MT6701 uses 0..5 LSB and 6..13 MSB readValue = ( readArray[1] & lsb_mask ); readValue += ( ( readArray[0] & msb_mask ) << lsb_used ); return readValue; From 1c732ebf424ec95d92771ead04922477173e6649 Mon Sep 17 00:00:00 2001 From: Rob Deutsch Date: Fri, 7 Jun 2024 12:28:45 +1000 Subject: [PATCH 11/57] Added gain documentation to B_G431B_ESC1.ino --- .../hardware_specific_examples/B_G431B_ESC1/B_G431B_ESC1.ino | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/examples/hardware_specific_examples/B_G431B_ESC1/B_G431B_ESC1.ino b/examples/hardware_specific_examples/B_G431B_ESC1/B_G431B_ESC1.ino index ab8e3bba..8d751742 100644 --- a/examples/hardware_specific_examples/B_G431B_ESC1/B_G431B_ESC1.ino +++ b/examples/hardware_specific_examples/B_G431B_ESC1/B_G431B_ESC1.ino @@ -7,6 +7,7 @@ // Motor instance BLDCMotor motor = BLDCMotor(11); BLDCDriver6PWM driver = BLDCDriver6PWM(A_PHASE_UH, A_PHASE_UL, A_PHASE_VH, A_PHASE_VL, A_PHASE_WH, A_PHASE_WL); +// Gain calculation shown at https://community.simplefoc.com/t/b-g431b-esc1-current-control/521/21 LowsideCurrentSense currentSense = LowsideCurrentSense(0.003f, -64.0f/7.0f, A_OP1_OUT, A_OP2_OUT, A_OP3_OUT); @@ -107,4 +108,4 @@ void loop() { // user communication command.run(); -} \ No newline at end of file +} From 93bcfe00fb0a822a934405aa35ca771f4ccb2d3d Mon Sep 17 00:00:00 2001 From: askuric Date: Fri, 7 Jun 2024 15:35:42 +0200 Subject: [PATCH 12/57] use current sensing only if mpcpwm used and force LEDC for now --- .../esp32/esp32_adc_driver.cpp | 2 +- .../esp32/esp32_adc_driver.h | 2 +- .../esp32/esp32_ledc_mcu.cpp | 2 +- .../hardware_specific/esp32/esp32_mcu.cpp | 2 +- .../esp32/esp32s_adc_driver.cpp | 2 +- src/drivers/hardware_api.h | 1 + .../esp32/esp32_ledc_mcu.cpp | 109 +++++++++--------- 7 files changed, 63 insertions(+), 57 deletions(-) diff --git a/src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp b/src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp index 57ffdfb5..7f0cc310 100644 --- a/src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp +++ b/src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp @@ -1,6 +1,6 @@ #include "esp32_adc_driver.h" -#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(CONFIG_IDF_TARGET_ESP32S2) && !defined(CONFIG_IDF_TARGET_ESP32S3) +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(CONFIG_IDF_TARGET_ESP32S2) && !defined(CONFIG_IDF_TARGET_ESP32S3) && !defined(SIMPLEFOC_ESP32_USELEDC) #include "freertos/FreeRTOS.h" #include "freertos/task.h" diff --git a/src/current_sense/hardware_specific/esp32/esp32_adc_driver.h b/src/current_sense/hardware_specific/esp32/esp32_adc_driver.h index 869c4dde..f76c003e 100644 --- a/src/current_sense/hardware_specific/esp32/esp32_adc_driver.h +++ b/src/current_sense/hardware_specific/esp32/esp32_adc_driver.h @@ -5,7 +5,7 @@ #include "Arduino.h" -#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(SIMPLEFOC_ESP32_USELEDC) /* * Get ADC value for pin * */ diff --git a/src/current_sense/hardware_specific/esp32/esp32_ledc_mcu.cpp b/src/current_sense/hardware_specific/esp32/esp32_ledc_mcu.cpp index f2d65f8e..3487dbd7 100644 --- a/src/current_sense/hardware_specific/esp32/esp32_ledc_mcu.cpp +++ b/src/current_sense/hardware_specific/esp32/esp32_ledc_mcu.cpp @@ -1,7 +1,7 @@ #include "../../hardware_api.h" #include "../../../drivers/hardware_api.h" -#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && !defined(SOC_MCPWM_SUPPORTED) +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && (!defined(SOC_MCPWM_SUPPORTED) || defined(SIMPLEFOC_ESP32_USELEDC)) #include "esp32_adc_driver.h" diff --git a/src/current_sense/hardware_specific/esp32/esp32_mcu.cpp b/src/current_sense/hardware_specific/esp32/esp32_mcu.cpp index 2057463c..8b09ca4e 100644 --- a/src/current_sense/hardware_specific/esp32/esp32_mcu.cpp +++ b/src/current_sense/hardware_specific/esp32/esp32_mcu.cpp @@ -2,7 +2,7 @@ #include "../../../drivers/hardware_api.h" #include "../../../drivers/hardware_specific/esp32/esp32_driver_mcpwm.h" -#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(SIMPLEFOC_ESP32_USELEDC) +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(SIMPLEFOC_ESP32_USELEDC) #include "esp32_adc_driver.h" diff --git a/src/current_sense/hardware_specific/esp32/esp32s_adc_driver.cpp b/src/current_sense/hardware_specific/esp32/esp32s_adc_driver.cpp index a2f58ac3..a212d57b 100644 --- a/src/current_sense/hardware_specific/esp32/esp32s_adc_driver.cpp +++ b/src/current_sense/hardware_specific/esp32/esp32s_adc_driver.cpp @@ -1,6 +1,6 @@ #include "esp32_adc_driver.h" -#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && (defined(CONFIG_IDF_TARGET_ESP32S2) || defined(CONFIG_IDF_TARGET_ESP32S3)) +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && (defined(CONFIG_IDF_TARGET_ESP32S2) || defined(CONFIG_IDF_TARGET_ESP32S3)) && !defined(SIMPLEFOC_ESP32_USELEDC) #include "freertos/FreeRTOS.h" #include "freertos/task.h" diff --git a/src/drivers/hardware_api.h b/src/drivers/hardware_api.h index 8b477458..7df00bf1 100644 --- a/src/drivers/hardware_api.h +++ b/src/drivers/hardware_api.h @@ -27,6 +27,7 @@ +#define SIMPLEFOC_ESP32_USELEDC // flag returned if driver init fails #define SIMPLEFOC_DRIVER_INIT_FAILED ((void*)-1) diff --git a/src/drivers/hardware_specific/esp32/esp32_ledc_mcu.cpp b/src/drivers/hardware_specific/esp32/esp32_ledc_mcu.cpp index a454c052..f54e0e93 100644 --- a/src/drivers/hardware_specific/esp32/esp32_ledc_mcu.cpp +++ b/src/drivers/hardware_specific/esp32/esp32_ledc_mcu.cpp @@ -1,5 +1,18 @@ #include "../../hardware_api.h" +/* +For the moment the LEDC driver implements the simplest possible way to set the PWM on esp32 while enabling to set the frequency and resolution. + +The pwm is not center aligned and moreover there are no guarantees on the proper alignement between the PWM signals. +Therefore this driver is not recommended for boards that have MCPWM. + +There are however ways to improve the LEDC driver, by not using directly espressif sdk: +https://docs.espressif.com/projects/esp-idf/en/stable/esp32/api-reference/peripherals/ledc.html#ledc-api-change-pwm-signal +- We could potentially use the ledc_set_duty_with_hpoint function to set the duty cycle and the high time point to make the signals center-aligned +- We could force the use of the same ledc timer within one driver to ensure the alignement between the signals + +*/ + #if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && ( !defined(SOC_MCPWM_SUPPORTED) || defined(SIMPLEFOC_ESP32_USELEDC) ) #pragma message("") @@ -11,14 +24,10 @@ #define _PWM_FREQUENCY 25000 // 25khz #define _PWM_FREQUENCY_MAX 38000 // 38khz max to be able to have 10 bit pwm resolution #define _PWM_RES_BIT 10 // 10 bir resolution -#define _PWM_RES 1023 // 2^10-1 = 1023-1 +#define _PWM_RES 1023 // 2^10-1 = 1024-1 -// empty motor slot -#define _EMPTY_SLOT -20 -#define _TAKEN_SLOT -21 - -// figure out how many ledc channels are avaible +// figure out how many ledc channels are available // esp32 - 2x8=16 // esp32s2 - 8 // esp32c3 - 6 @@ -30,18 +39,16 @@ #endif -// current channel stack index +// currently used ledc channels // support for multiple motors // esp32 has 16 channels // esp32s2 has 8 channels // esp32c3 has 6 channels -int channel_index = 0; - - +int channels_used = 0; typedef struct ESP32LEDCDriverParams { - int channels[6]; + int pins[6]; long pwm_frequency; } ESP32LEDCDriverParams; @@ -50,9 +57,11 @@ typedef struct ESP32LEDCDriverParams { // configure High PWM frequency -void _setHighFrequency(const long freq, const int pin, const int channel){ - ledcSetup(channel, freq, _PWM_RES_BIT ); - ledcAttachPin(pin, channel); +bool _setHighFrequency(const long freq, const int pin){ + // sets up the pwm resolution and frequency on this pin + // https://docs.espressif.com/projects/arduino-esp32/en/latest/api/ledc.html + // from v5.x no more need to deal with channels + return ledcAttach(pin, freq, _PWM_RES_BIT); } @@ -65,13 +74,14 @@ void* _configure1PWM(long pwm_frequency, const int pinA) { else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max // check if enough channels available - if ( channel_index + 1 >= LEDC_CHANNELS ) return SIMPLEFOC_DRIVER_INIT_FAILED; + if ( channels_used + 1 >= LEDC_CHANNELS ) return SIMPLEFOC_DRIVER_INIT_FAILED; + channels_used++; - int ch1 = channel_index++; - _setHighFrequency(pwm_frequency, pinA, ch1); + // setup the channel + if (!_setHighFrequency(pwm_frequency, pinA)) return SIMPLEFOC_DRIVER_INIT_FAILED; ESP32LEDCDriverParams* params = new ESP32LEDCDriverParams { - .channels = { ch1 }, + .pins = { pinA }, .pwm_frequency = pwm_frequency }; return params; @@ -89,15 +99,15 @@ void* _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max // check if enough channels available - if ( channel_index + 2 >= LEDC_CHANNELS ) return SIMPLEFOC_DRIVER_INIT_FAILED; + if ( channels_used + 2 >= LEDC_CHANNELS ) return SIMPLEFOC_DRIVER_INIT_FAILED; + channels_used += 2; - int ch1 = channel_index++; - int ch2 = channel_index++; - _setHighFrequency(pwm_frequency, pinA, ch1); - _setHighFrequency(pwm_frequency, pinB, ch2); + // setup the channels + if(!_setHighFrequency(pwm_frequency, pinA) || !_setHighFrequency(pwm_frequency, pinB)) + return SIMPLEFOC_DRIVER_INIT_FAILED; ESP32LEDCDriverParams* params = new ESP32LEDCDriverParams { - .channels = { ch1, ch2 }, + .pins = { pinA, pinB }, .pwm_frequency = pwm_frequency }; return params; @@ -110,17 +120,15 @@ void* _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const in else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max // check if enough channels available - if ( channel_index + 3 >= LEDC_CHANNELS ) return SIMPLEFOC_DRIVER_INIT_FAILED; + if ( channels_used + 3 >= LEDC_CHANNELS ) return SIMPLEFOC_DRIVER_INIT_FAILED; + channels_used += 3; - int ch1 = channel_index++; - int ch2 = channel_index++; - int ch3 = channel_index++; - _setHighFrequency(pwm_frequency, pinA, ch1); - _setHighFrequency(pwm_frequency, pinB, ch2); - _setHighFrequency(pwm_frequency, pinC, ch3); + // setup the channels + if(!_setHighFrequency(pwm_frequency, pinA) || !_setHighFrequency(pwm_frequency, pinB) || !_setHighFrequency(pwm_frequency, pinC)) + return SIMPLEFOC_DRIVER_INIT_FAILED; ESP32LEDCDriverParams* params = new ESP32LEDCDriverParams { - .channels = { ch1, ch2, ch3 }, + .pins = { pinA, pinB, pinC }, .pwm_frequency = pwm_frequency }; return params; @@ -133,19 +141,16 @@ void* _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const in else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max // check if enough channels available - if ( channel_index + 4 >= LEDC_CHANNELS ) return SIMPLEFOC_DRIVER_INIT_FAILED; + if ( channels_used + 4 >= LEDC_CHANNELS ) return SIMPLEFOC_DRIVER_INIT_FAILED; + channels_used += 4; - int ch1 = channel_index++; - int ch2 = channel_index++; - int ch3 = channel_index++; - int ch4 = channel_index++; - _setHighFrequency(pwm_frequency, pinA, ch1); - _setHighFrequency(pwm_frequency, pinB, ch2); - _setHighFrequency(pwm_frequency, pinC, ch3); - _setHighFrequency(pwm_frequency, pinD, ch4); + // setup the channels + if(!_setHighFrequency(pwm_frequency, pinA) || !_setHighFrequency(pwm_frequency, pinB) || + !_setHighFrequency(pwm_frequency, pinC)|| !_setHighFrequency(pwm_frequency, pinD)) + return SIMPLEFOC_DRIVER_INIT_FAILED; ESP32LEDCDriverParams* params = new ESP32LEDCDriverParams { - .channels = { ch1, ch2, ch3, ch4 }, + .pins = { pinA, pinB, pinC, pinD }, .pwm_frequency = pwm_frequency }; return params; @@ -155,31 +160,31 @@ void* _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const in void _writeDutyCycle1PWM(float dc_a, void* params){ - ledcWrite(((ESP32LEDCDriverParams*)params)->channels[0], _constrain(_PWM_RES*dc_a, 0, _PWM_RES)); + ledcWrite(((ESP32LEDCDriverParams*)params)->pins[0], _constrain(_PWM_RES*dc_a, 0, _PWM_RES)); } void _writeDutyCycle2PWM(float dc_a, float dc_b, void* params){ - ledcWrite(((ESP32LEDCDriverParams*)params)->channels[0], _constrain(_PWM_RES*dc_a, 0, _PWM_RES)); - ledcWrite(((ESP32LEDCDriverParams*)params)->channels[1], _constrain(_PWM_RES*dc_b, 0, _PWM_RES)); + ledcWrite(((ESP32LEDCDriverParams*)params)->pins[0], _constrain(_PWM_RES*dc_a, 0, _PWM_RES)); + ledcWrite(((ESP32LEDCDriverParams*)params)->pins[1], _constrain(_PWM_RES*dc_b, 0, _PWM_RES)); } void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, void* params){ - ledcWrite(((ESP32LEDCDriverParams*)params)->channels[0], _constrain(_PWM_RES*dc_a, 0, _PWM_RES)); - ledcWrite(((ESP32LEDCDriverParams*)params)->channels[1], _constrain(_PWM_RES*dc_b, 0, _PWM_RES)); - ledcWrite(((ESP32LEDCDriverParams*)params)->channels[2], _constrain(_PWM_RES*dc_c, 0, _PWM_RES)); + ledcWrite(((ESP32LEDCDriverParams*)params)->pins[0], _constrain(_PWM_RES*dc_a, 0, _PWM_RES)); + ledcWrite(((ESP32LEDCDriverParams*)params)->pins[1], _constrain(_PWM_RES*dc_b, 0, _PWM_RES)); + ledcWrite(((ESP32LEDCDriverParams*)params)->pins[2], _constrain(_PWM_RES*dc_c, 0, _PWM_RES)); } void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, void* params){ - ledcWrite(((ESP32LEDCDriverParams*)params)->channels[0], _constrain(_PWM_RES*dc_1a, 0, _PWM_RES)); - ledcWrite(((ESP32LEDCDriverParams*)params)->channels[1], _constrain(_PWM_RES*dc_1b, 0, _PWM_RES)); - ledcWrite(((ESP32LEDCDriverParams*)params)->channels[2], _constrain(_PWM_RES*dc_2a, 0, _PWM_RES)); - ledcWrite(((ESP32LEDCDriverParams*)params)->channels[3], _constrain(_PWM_RES*dc_2b, 0, _PWM_RES)); + ledcWrite(((ESP32LEDCDriverParams*)params)->pins[0], _constrain(_PWM_RES*dc_1a, 0, _PWM_RES)); + ledcWrite(((ESP32LEDCDriverParams*)params)->pins[1], _constrain(_PWM_RES*dc_1b, 0, _PWM_RES)); + ledcWrite(((ESP32LEDCDriverParams*)params)->pins[2], _constrain(_PWM_RES*dc_2a, 0, _PWM_RES)); + ledcWrite(((ESP32LEDCDriverParams*)params)->pins[3], _constrain(_PWM_RES*dc_2b, 0, _PWM_RES)); } #endif \ No newline at end of file From 8e8ffb25102d409771a93b13706db227cc7d0098 Mon Sep 17 00:00:00 2001 From: askuric Date: Fri, 7 Jun 2024 18:35:40 +0200 Subject: [PATCH 13/57] adde the center-aligend ledc driver - enables 6pwm --- .../esp32/esp32_driver_mcpwm.h | 16 +- .../esp32/esp32_ledc_mcu.cpp | 242 +++++++++++++----- 2 files changed, 186 insertions(+), 72 deletions(-) diff --git a/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.h b/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.h index 10497876..26db540d 100644 --- a/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.h +++ b/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.h @@ -40,7 +40,7 @@ typedef struct { int pinA; mcpwm_dev_t* mcpwm_num; mcpwm_unit_t mcpwm_unit; - mcpwm_operator_t mcpwm_operator; + mcpwm_operator_t mcpwm_operator; mcpwm_io_signals_t mcpwm_a; mcpwm_io_signals_t mcpwm_b; mcpwm_io_signals_t mcpwm_c; @@ -50,8 +50,8 @@ typedef struct { int pin1A; mcpwm_dev_t* mcpwm_num; mcpwm_unit_t mcpwm_unit; - mcpwm_operator_t mcpwm_operator1; - mcpwm_operator_t mcpwm_operator2; + mcpwm_operator_t mcpwm_operator1; + mcpwm_operator_t mcpwm_operator2; mcpwm_io_signals_t mcpwm_1a; mcpwm_io_signals_t mcpwm_1b; mcpwm_io_signals_t mcpwm_2a; @@ -62,7 +62,7 @@ typedef struct { int pin1pwm; mcpwm_dev_t* mcpwm_num; mcpwm_unit_t mcpwm_unit; - mcpwm_operator_t mcpwm_operator; + mcpwm_operator_t mcpwm_operator; mcpwm_io_signals_t mcpwm_a; mcpwm_io_signals_t mcpwm_b; } stepper_2pwm_motor_slots_t; @@ -71,8 +71,8 @@ typedef struct { int pinAH; mcpwm_dev_t* mcpwm_num; mcpwm_unit_t mcpwm_unit; - mcpwm_operator_t mcpwm_operator1; - mcpwm_operator_t mcpwm_operator2; + mcpwm_operator_t mcpwm_operator1; + mcpwm_operator_t mcpwm_operator2; mcpwm_io_signals_t mcpwm_ah; mcpwm_io_signals_t mcpwm_bh; mcpwm_io_signals_t mcpwm_ch; @@ -86,8 +86,8 @@ typedef struct ESP32MCPWMDriverParams { long pwm_frequency; mcpwm_dev_t* mcpwm_dev; mcpwm_unit_t mcpwm_unit; - mcpwm_operator_t mcpwm_operator1; - mcpwm_operator_t mcpwm_operator2; + mcpwm_operator_t mcpwm_operator1; + mcpwm_operator_t mcpwm_operator2; float deadtime; } ESP32MCPWMDriverParams; diff --git a/src/drivers/hardware_specific/esp32/esp32_ledc_mcu.cpp b/src/drivers/hardware_specific/esp32/esp32_ledc_mcu.cpp index f54e0e93..974db6ca 100644 --- a/src/drivers/hardware_specific/esp32/esp32_ledc_mcu.cpp +++ b/src/drivers/hardware_specific/esp32/esp32_ledc_mcu.cpp @@ -1,18 +1,5 @@ #include "../../hardware_api.h" -/* -For the moment the LEDC driver implements the simplest possible way to set the PWM on esp32 while enabling to set the frequency and resolution. - -The pwm is not center aligned and moreover there are no guarantees on the proper alignement between the PWM signals. -Therefore this driver is not recommended for boards that have MCPWM. - -There are however ways to improve the LEDC driver, by not using directly espressif sdk: -https://docs.espressif.com/projects/esp-idf/en/stable/esp32/api-reference/peripherals/ledc.html#ledc-api-change-pwm-signal -- We could potentially use the ledc_set_duty_with_hpoint function to set the duty cycle and the high time point to make the signals center-aligned -- We could force the use of the same ledc timer within one driver to ensure the alignement between the signals - -*/ - #if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && ( !defined(SOC_MCPWM_SUPPORTED) || defined(SIMPLEFOC_ESP32_USELEDC) ) #pragma message("") @@ -38,52 +25,124 @@ There are however ways to improve the LEDC driver, by not using directly espress #define LEDC_CHANNELS (SOC_LEDC_CHANNEL_NUM) #endif +#define LEDC_CHANNELS_GROUP0 (LEDC_CHANNELS < 8 ? LEDC_CHANNELS : 8) +#define LEDC_CHANNELS_GROUP1 (LEDC_CHANNELS < 8 ? 0 : LEDC_CHANNELS - 8) + // currently used ledc channels // support for multiple motors // esp32 has 16 channels // esp32s2 has 8 channels // esp32c3 has 6 channels -int channels_used = 0; +// channels from 0-7 are in group 0 and 8-15 in group 1 +// - only esp32 as of mid 2024 has the second group, all the s versions don't +int group_channels_used[2] = {0}; typedef struct ESP32LEDCDriverParams { - int pins[6]; + ledc_channel_t channels[6]; + ledc_mode_t groups[6]; long pwm_frequency; + float dead_zone; } ESP32LEDCDriverParams; - - - -// configure High PWM frequency -bool _setHighFrequency(const long freq, const int pin){ - // sets up the pwm resolution and frequency on this pin - // https://docs.espressif.com/projects/arduino-esp32/en/latest/api/ledc.html - // from v5.x no more need to deal with channels - return ledcAttach(pin, freq, _PWM_RES_BIT); +/* + Function to attach a channel to a pin with advanced settings + - freq - pwm frequency + - resolution - pwm resolution + - channel - ledc channel + - inverted - output inverted + - group - ledc group + + This function is a workaround for the ledcAttachPin function that is not available in the ESP32 Arduino core, in which the + PWM signals are synchronized in pairs, while the simplefoc requires a bit more flexible configuration. + This function sets also allows configuring a channel as inverted, which is not possible with the ledcAttachPin function. + + Function returns true if the channel was successfully attached, false otherwise. +*/ +bool _ledcAttachChannelAdvanced(uint8_t pin, int _channel, int _group, uint32_t freq, uint8_t resolution, bool inverted) { + + + ledc_channel_t channel = static_cast(_channel); + ledc_mode_t group = static_cast(_group); + + ledc_timer_bit_t res = static_cast(resolution); + ledc_timer_config_t ledc_timer; + ledc_timer.speed_mode = group; + ledc_timer.timer_num = LEDC_TIMER_0; + ledc_timer.duty_resolution = res; + ledc_timer.freq_hz = freq; + ledc_timer.clk_cfg = LEDC_AUTO_CLK; + if (ledc_timer_config(&ledc_timer) != ESP_OK) { + return false; + } + + // if active high is false invert + int pin_high_level = SIMPLEFOC_PWM_ACTIVE_HIGH ? 1 : 0; + if (inverted) pin_high_level = !pin_high_level; + + uint32_t duty = ledc_get_duty(group, channel); + ledc_channel_config_t ledc_channel; + ledc_channel.speed_mode = group; + ledc_channel.channel = channel; + ledc_channel.timer_sel = LEDC_TIMER_0; + ledc_channel.intr_type = LEDC_INTR_DISABLE; + ledc_channel.gpio_num = pin; + ledc_channel.duty = duty; + ledc_channel.hpoint = 0; + ledc_channel.flags.output_invert = pin_high_level; + if (ledc_channel_config(&ledc_channel)!= ESP_OK) { + return false; + } + + return true; } - - - +// returns the number of the group that has enough channels available +// returns -1 if no group has enough channels +// +// NOT IMPLEMENTED BUT COULD BE USEFUL +// returns 2 if no group has enough channels but combined they do +int _findGroupWithChannelsAvailable(int no_channels){ + if (group_channels_used[0] + no_channels < LEDC_CHANNELS_GROUP0){ + return 0; + }else if (group_channels_used[1] + no_channels < LEDC_CHANNELS_GROUP1){ + return 1; + } + // else if (group_channels_used[0] + group_channels_used[1] + no_channels < LEDC_CHANNELS){ + // return 2; + // } + return -1; +} void* _configure1PWM(long pwm_frequency, const int pinA) { if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max + SIMPLEFOC_DEBUG("EP32-DRV: Configuring 1PWM"); // check if enough channels available - if ( channels_used + 1 >= LEDC_CHANNELS ) return SIMPLEFOC_DRIVER_INIT_FAILED; - channels_used++; - - // setup the channel - if (!_setHighFrequency(pwm_frequency, pinA)) return SIMPLEFOC_DRIVER_INIT_FAILED; + int group = _findGroupWithChannelsAvailable(1); + if (group < 0){ + SIMPLEFOC_DEBUG("EP32-DRV: ERROR - Not enough channels available!"); + return SIMPLEFOC_DRIVER_INIT_FAILED; + } + SIMPLEFOC_DEBUG("EP32-DRV: 1PWM setup in group: ", (group)); + // configure the channel + group_channels_used[group] += 1; + if(!_ledcAttachChannelAdvanced(pinA, group_channels_used[group], group, pwm_frequency, _PWM_RES_BIT, false)) { + SIMPLEFOC_DEBUG("EP32-DRV: ERROR - Failed to attach channel:", group_channels_used[group]); + return SIMPLEFOC_DRIVER_INIT_FAILED; + } + ESP32LEDCDriverParams* params = new ESP32LEDCDriverParams { - .pins = { pinA }, + .channels = { static_cast(group_channels_used[group]) }, + .groups = { (ledc_mode_t)group }, .pwm_frequency = pwm_frequency }; + SIMPLEFOC_DEBUG("EP32-DRV: 1PWM setup successful in group: ", (group)); return params; } @@ -98,18 +157,33 @@ void* _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max - // check if enough channels available - if ( channels_used + 2 >= LEDC_CHANNELS ) return SIMPLEFOC_DRIVER_INIT_FAILED; - channels_used += 2; + SIMPLEFOC_DEBUG("EP32-DRV: Configuring 2PWM"); - // setup the channels - if(!_setHighFrequency(pwm_frequency, pinA) || !_setHighFrequency(pwm_frequency, pinB)) + // check if enough channels available + int group = _findGroupWithChannelsAvailable(2); + if (group < 0) { + SIMPLEFOC_DEBUG("EP32-DRV: ERROR - Not enough channels available!"); return SIMPLEFOC_DRIVER_INIT_FAILED; + } + SIMPLEFOC_DEBUG("EP32-DRV: 2PWM setup in group: ", (group)); ESP32LEDCDriverParams* params = new ESP32LEDCDriverParams { - .pins = { pinA, pinB }, + .channels = { static_cast(0)}, + .groups = { (ledc_mode_t)0 }, .pwm_frequency = pwm_frequency }; + + int pins[2] = {pinA, pinB}; + for(int i = 0; i < 2; i++){ + group_channels_used[group]++; + if(!_ledcAttachChannelAdvanced(pins[i], group_channels_used[group], group, pwm_frequency, _PWM_RES_BIT, false)){ + SIMPLEFOC_DEBUG("EP32-DRV: ERROR - Failed to attach channel:", group_channels_used[group]); + return SIMPLEFOC_DRIVER_INIT_FAILED; + } + params->channels[i] = static_cast(group_channels_used[group]); + params->groups[i] = (ledc_mode_t)group; + } + SIMPLEFOC_DEBUG("EP32-DRV: 2PWM setup successful in group: ", (group)); return params; } @@ -119,18 +193,33 @@ void* _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const in if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max - // check if enough channels available - if ( channels_used + 3 >= LEDC_CHANNELS ) return SIMPLEFOC_DRIVER_INIT_FAILED; - channels_used += 3; + SIMPLEFOC_DEBUG("EP32-DRV: Configuring 3PWM"); - // setup the channels - if(!_setHighFrequency(pwm_frequency, pinA) || !_setHighFrequency(pwm_frequency, pinB) || !_setHighFrequency(pwm_frequency, pinC)) + // check if enough channels available + int group = _findGroupWithChannelsAvailable(3); + if (group < 0) { + SIMPLEFOC_DEBUG("EP32-DRV: ERROR - Not enough channels available!"); return SIMPLEFOC_DRIVER_INIT_FAILED; + } + SIMPLEFOC_DEBUG("EP32-DRV: 3PWM setup in group: ", (group)); ESP32LEDCDriverParams* params = new ESP32LEDCDriverParams { - .pins = { pinA, pinB, pinC }, + .channels = { static_cast(0)}, + .groups = { (ledc_mode_t)0 }, .pwm_frequency = pwm_frequency }; + + int pins[3] = {pinA, pinB, pinC}; + for(int i = 0; i < 3; i++){ + group_channels_used[group]++; + if(!_ledcAttachChannelAdvanced(pins[i], group_channels_used[group], group, pwm_frequency, _PWM_RES_BIT, false)){ + SIMPLEFOC_DEBUG("EP32-DRV: ERROR - Failed to attach channel:", group_channels_used[group]); + return SIMPLEFOC_DRIVER_INIT_FAILED; + } + params->channels[i] = static_cast(group_channels_used[group]); + params->groups[i] = (ledc_mode_t)group; + } + SIMPLEFOC_DEBUG("EP32-DRV: 3PWM setup successful in group: ", (group)); return params; } @@ -140,51 +229,76 @@ void* _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const in if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max + SIMPLEFOC_DEBUG("EP32-DRV: Configuring 4PWM"); // check if enough channels available - if ( channels_used + 4 >= LEDC_CHANNELS ) return SIMPLEFOC_DRIVER_INIT_FAILED; - channels_used += 4; - - // setup the channels - if(!_setHighFrequency(pwm_frequency, pinA) || !_setHighFrequency(pwm_frequency, pinB) || - !_setHighFrequency(pwm_frequency, pinC)|| !_setHighFrequency(pwm_frequency, pinD)) + int group = _findGroupWithChannelsAvailable(4); + if (group < 0){ + SIMPLEFOC_DEBUG("EP32-DRV: ERROR - Not enough channels available!"); return SIMPLEFOC_DRIVER_INIT_FAILED; - + } + SIMPLEFOC_DEBUG("EP32-DRV: 4PWM setup in group: ", (group)); ESP32LEDCDriverParams* params = new ESP32LEDCDriverParams { - .pins = { pinA, pinB, pinC, pinD }, + .channels = { static_cast(0)}, + .groups = { (ledc_mode_t)0 }, .pwm_frequency = pwm_frequency }; + + int pins[4] = {pinA, pinB, pinC, pinD}; + for(int i = 0; i < 4; i++){ + group_channels_used[group]++; + if(!_ledcAttachChannelAdvanced(pins[i], group_channels_used[group], group, pwm_frequency, _PWM_RES_BIT, false)){ + SIMPLEFOC_DEBUG("EP32-DRV: ERROR - Failed to attach channel:", group_channels_used[group]); + return SIMPLEFOC_DRIVER_INIT_FAILED; + } + params->channels[i] = static_cast(group_channels_used[group]); + params->groups[i] = (ledc_mode_t)group; + } + SIMPLEFOC_DEBUG("EP32-DRV: 4PWM setup successful in group: ", (group)); return params; } - +void _writeDutyCycle(float dc, void* params, int index){ + ledc_set_duty_with_hpoint(((ESP32LEDCDriverParams*)params)->groups[index],((ESP32LEDCDriverParams*)params)->channels[index], _PWM_RES*dc, _PWM_RES/2.0*(1.0-dc)); + ledc_update_duty(((ESP32LEDCDriverParams*)params)->groups[index],((ESP32LEDCDriverParams*)params)->channels[index]); +} void _writeDutyCycle1PWM(float dc_a, void* params){ - ledcWrite(((ESP32LEDCDriverParams*)params)->pins[0], _constrain(_PWM_RES*dc_a, 0, _PWM_RES)); + _writeDutyCycle(dc_a, params, 0); } void _writeDutyCycle2PWM(float dc_a, float dc_b, void* params){ - ledcWrite(((ESP32LEDCDriverParams*)params)->pins[0], _constrain(_PWM_RES*dc_a, 0, _PWM_RES)); - ledcWrite(((ESP32LEDCDriverParams*)params)->pins[1], _constrain(_PWM_RES*dc_b, 0, _PWM_RES)); + _writeDutyCycle(dc_a, params, 0); + _writeDutyCycle(dc_b, params, 1); } void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, void* params){ - ledcWrite(((ESP32LEDCDriverParams*)params)->pins[0], _constrain(_PWM_RES*dc_a, 0, _PWM_RES)); - ledcWrite(((ESP32LEDCDriverParams*)params)->pins[1], _constrain(_PWM_RES*dc_b, 0, _PWM_RES)); - ledcWrite(((ESP32LEDCDriverParams*)params)->pins[2], _constrain(_PWM_RES*dc_c, 0, _PWM_RES)); + _writeDutyCycle(dc_a, params, 0); + _writeDutyCycle(dc_b, params, 1); + _writeDutyCycle(dc_c, params, 2); } void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, void* params){ - ledcWrite(((ESP32LEDCDriverParams*)params)->pins[0], _constrain(_PWM_RES*dc_1a, 0, _PWM_RES)); - ledcWrite(((ESP32LEDCDriverParams*)params)->pins[1], _constrain(_PWM_RES*dc_1b, 0, _PWM_RES)); - ledcWrite(((ESP32LEDCDriverParams*)params)->pins[2], _constrain(_PWM_RES*dc_2a, 0, _PWM_RES)); - ledcWrite(((ESP32LEDCDriverParams*)params)->pins[3], _constrain(_PWM_RES*dc_2b, 0, _PWM_RES)); + _writeDutyCycle(dc_1a, params, 0); + _writeDutyCycle(dc_1b, params, 1); + _writeDutyCycle(dc_2a, params, 2); + _writeDutyCycle(dc_2b, params, 3); +} + + +// TODO - implement 6PWM +void _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l){ + SIMPLEFOC_DEBUG("EP32-DRV: 6PWM will be implemented soon for LEDC driver!"); + return SIMPLEFOC_DRIVER_INIT_FAILED; +} +void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, void* params) { + SIMPLEFOC_DEBUG("EP32-DRV: 6PWM not supported"); } #endif \ No newline at end of file From ae4df18374eccfe1bc4d0ffeecf73df649434248 Mon Sep 17 00:00:00 2001 From: askuric Date: Fri, 7 Jun 2024 21:48:36 +0200 Subject: [PATCH 14/57] forgotten star --- src/drivers/hardware_specific/esp32/esp32_ledc_mcu.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/hardware_specific/esp32/esp32_ledc_mcu.cpp b/src/drivers/hardware_specific/esp32/esp32_ledc_mcu.cpp index 974db6ca..8a63dd43 100644 --- a/src/drivers/hardware_specific/esp32/esp32_ledc_mcu.cpp +++ b/src/drivers/hardware_specific/esp32/esp32_ledc_mcu.cpp @@ -293,8 +293,8 @@ void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, vo // TODO - implement 6PWM -void _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l){ - SIMPLEFOC_DEBUG("EP32-DRV: 6PWM will be implemented soon for LEDC driver!"); +void* _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l){ + SIMPLEFOC_DEBUG("EP32-DRV: 6PWM not supported!"); return SIMPLEFOC_DRIVER_INIT_FAILED; } void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, void* params) { From 26d111dd458a7e3a9f97258d14b1da5d5f4af0d3 Mon Sep 17 00:00:00 2001 From: askuric Date: Sat, 8 Jun 2024 08:55:34 +0200 Subject: [PATCH 15/57] added the 6wpm driver --- .../esp32/esp32_ledc_mcu.cpp | 161 ++++++++++++++---- 1 file changed, 127 insertions(+), 34 deletions(-) diff --git a/src/drivers/hardware_specific/esp32/esp32_ledc_mcu.cpp b/src/drivers/hardware_specific/esp32/esp32_ledc_mcu.cpp index 8a63dd43..b5bc8f9a 100644 --- a/src/drivers/hardware_specific/esp32/esp32_ledc_mcu.cpp +++ b/src/drivers/hardware_specific/esp32/esp32_ledc_mcu.cpp @@ -75,11 +75,12 @@ bool _ledcAttachChannelAdvanced(uint8_t pin, int _channel, int _group, uint32_t ledc_timer.freq_hz = freq; ledc_timer.clk_cfg = LEDC_AUTO_CLK; if (ledc_timer_config(&ledc_timer) != ESP_OK) { + SIMPLEFOC_DEBUG("EP32-DRV: ERROR - Failed to configure the timer:", LEDC_TIMER_0); return false; } // if active high is false invert - int pin_high_level = SIMPLEFOC_PWM_ACTIVE_HIGH ? 1 : 0; + int pin_high_level = SIMPLEFOC_PWM_ACTIVE_HIGH ? 0 : 1; if (inverted) pin_high_level = !pin_high_level; uint32_t duty = ledc_get_duty(group, channel); @@ -91,28 +92,31 @@ bool _ledcAttachChannelAdvanced(uint8_t pin, int _channel, int _group, uint32_t ledc_channel.gpio_num = pin; ledc_channel.duty = duty; ledc_channel.hpoint = 0; - ledc_channel.flags.output_invert = pin_high_level; + ledc_channel.flags.output_invert = pin_high_level; // 0 is active high, 1 is active low if (ledc_channel_config(&ledc_channel)!= ESP_OK) { + SIMPLEFOC_DEBUG("EP32-DRV: ERROR - Failed to attach channel:", _channel); return false; } return true; } + +// returns the number of available channels in the group +int _availableGroupChannels(int group){ + if(group == 0) return LEDC_CHANNELS_GROUP0 - group_channels_used[0]; + else if(group == 1) return LEDC_CHANNELS_GROUP1 - group_channels_used[1]; + return 0; +} + // returns the number of the group that has enough channels available // returns -1 if no group has enough channels // // NOT IMPLEMENTED BUT COULD BE USEFUL // returns 2 if no group has enough channels but combined they do int _findGroupWithChannelsAvailable(int no_channels){ - if (group_channels_used[0] + no_channels < LEDC_CHANNELS_GROUP0){ - return 0; - }else if (group_channels_used[1] + no_channels < LEDC_CHANNELS_GROUP1){ - return 1; - } - // else if (group_channels_used[0] + group_channels_used[1] + no_channels < LEDC_CHANNELS){ - // return 2; - // } + if(no_channels <= _availableGroupChannels(0)) return 0; + if(no_channels <= _availableGroupChannels(1)) return 1; return -1; } @@ -132,11 +136,12 @@ void* _configure1PWM(long pwm_frequency, const int pinA) { // configure the channel group_channels_used[group] += 1; - if(!_ledcAttachChannelAdvanced(pinA, group_channels_used[group], group, pwm_frequency, _PWM_RES_BIT, false)) { - SIMPLEFOC_DEBUG("EP32-DRV: ERROR - Failed to attach channel:", group_channels_used[group]); + if(!_ledcAttachChannelAdvanced(pinA, group_channels_used[group], group, pwm_frequency, _PWM_RES_BIT, false)){ + SIMPLEFOC_DEBUG("EP32-DRV: ERROR - Failed to configure pin:", pinA); return SIMPLEFOC_DRIVER_INIT_FAILED; } - + + ESP32LEDCDriverParams* params = new ESP32LEDCDriverParams { .channels = { static_cast(group_channels_used[group]) }, .groups = { (ledc_mode_t)group }, @@ -177,9 +182,10 @@ void* _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { for(int i = 0; i < 2; i++){ group_channels_used[group]++; if(!_ledcAttachChannelAdvanced(pins[i], group_channels_used[group], group, pwm_frequency, _PWM_RES_BIT, false)){ - SIMPLEFOC_DEBUG("EP32-DRV: ERROR - Failed to attach channel:", group_channels_used[group]); + SIMPLEFOC_DEBUG("EP32-DRV: ERROR - Failed to configure pin:", pins[i]); return SIMPLEFOC_DRIVER_INIT_FAILED; } + params->channels[i] = static_cast(group_channels_used[group]); params->groups[i] = (ledc_mode_t)group; } @@ -213,9 +219,10 @@ void* _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const in for(int i = 0; i < 3; i++){ group_channels_used[group]++; if(!_ledcAttachChannelAdvanced(pins[i], group_channels_used[group], group, pwm_frequency, _PWM_RES_BIT, false)){ - SIMPLEFOC_DEBUG("EP32-DRV: ERROR - Failed to attach channel:", group_channels_used[group]); + SIMPLEFOC_DEBUG("EP32-DRV: ERROR - Failed to configure pin:", pins[i]); return SIMPLEFOC_DRIVER_INIT_FAILED; } + params->channels[i] = static_cast(group_channels_used[group]); params->groups[i] = (ledc_mode_t)group; } @@ -229,31 +236,49 @@ void* _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const in if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max - SIMPLEFOC_DEBUG("EP32-DRV: Configuring 4PWM"); - // check if enough channels available - int group = _findGroupWithChannelsAvailable(4); - if (group < 0){ - SIMPLEFOC_DEBUG("EP32-DRV: ERROR - Not enough channels available!"); - return SIMPLEFOC_DRIVER_INIT_FAILED; - } - SIMPLEFOC_DEBUG("EP32-DRV: 4PWM setup in group: ", (group)); + ESP32LEDCDriverParams* params = new ESP32LEDCDriverParams { .channels = { static_cast(0)}, .groups = { (ledc_mode_t)0 }, .pwm_frequency = pwm_frequency }; + SIMPLEFOC_DEBUG("EP32-DRV: Configuring 4PWM"); + // check if enough channels available + int group = _findGroupWithChannelsAvailable(4); + if (group < 0){ + // not enough channels available on any individual group + // check if their combined number is enough (two channels per group) + if(_availableGroupChannels(0) >=2 && _availableGroupChannels(1) >=2){ + group = 2; + SIMPLEFOC_DEBUG("EP32-DRV: WARNING: Not enough available ledc channels for 4pwm in a single group! Using two groups!"); + SIMPLEFOC_DEBUG("EP32-DRV: 4PWM setup in groups: 0 and 1!"); + params->groups[2] = (ledc_mode_t)1; + params->groups[3] = (ledc_mode_t)1; + }else{ + SIMPLEFOC_DEBUG("EP32-DRV: ERROR - Not enough available ledc channels for 4pwm!"); + return SIMPLEFOC_DRIVER_INIT_FAILED; + } + }else{ + SIMPLEFOC_DEBUG("EP32-DRV: 4PWM setup in group: ", (group)); + params->groups[0] = (ledc_mode_t)group; + params->groups[1] = (ledc_mode_t)group; + params->groups[2] = (ledc_mode_t)group; + params->groups[3] = (ledc_mode_t)group; + } + + + int pins[4] = {pinA, pinB, pinC, pinD}; for(int i = 0; i < 4; i++){ - group_channels_used[group]++; - if(!_ledcAttachChannelAdvanced(pins[i], group_channels_used[group], group, pwm_frequency, _PWM_RES_BIT, false)){ - SIMPLEFOC_DEBUG("EP32-DRV: ERROR - Failed to attach channel:", group_channels_used[group]); + group_channels_used[params->groups[i]]++; + if(!_ledcAttachChannelAdvanced(pins[i], group_channels_used[params->groups[i]], params->groups[i], pwm_frequency, _PWM_RES_BIT, false)){ + SIMPLEFOC_DEBUG("EP32-DRV: ERROR - Failed to configure pin:", pins[i]); return SIMPLEFOC_DRIVER_INIT_FAILED; } - params->channels[i] = static_cast(group_channels_used[group]); - params->groups[i] = (ledc_mode_t)group; + params->channels[i] = static_cast(group_channels_used[params->groups[i]]); } - SIMPLEFOC_DEBUG("EP32-DRV: 4PWM setup successful in group: ", (group)); + SIMPLEFOC_DEBUG("EP32-DRV: 4PWM setup successful!"); return params; } @@ -292,13 +317,81 @@ void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, vo } -// TODO - implement 6PWM void* _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l){ - SIMPLEFOC_DEBUG("EP32-DRV: 6PWM not supported!"); - return SIMPLEFOC_DRIVER_INIT_FAILED; + if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz + else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max + + SIMPLEFOC_DEBUG("EP32-DRV: Configuring 6PWM"); + SIMPLEFOC_DEBUG("EP32-DRV: WARNING - 6PWM on LEDC is poorly supported and not tested, consider using MCPWM driver instead!"); + // check if enough channels available + int group = _findGroupWithChannelsAvailable(6); + if (group < 0){ + SIMPLEFOC_DEBUG("EP32-DRV: ERROR - Not enough channels available!"); + return SIMPLEFOC_DRIVER_INIT_FAILED; + } + SIMPLEFOC_DEBUG("EP32-DRV: 6PWM setup in group: ", (group)); + ESP32LEDCDriverParams* params = new ESP32LEDCDriverParams { + .channels = { static_cast(0)}, + .groups = { (ledc_mode_t)group }, + .pwm_frequency = pwm_frequency, + .dead_zone = dead_zone + }; + + int high_side_invert = SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH ? false : true; + int low_side_invert = SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH ? true : false; + + int pin_pairs[6][2] = { + {pinA_h, pinA_l}, + {pinB_h, pinB_l}, + {pinC_h, pinC_l} + }; + + for(int i = 0; i < 3; i++){ + group_channels_used[group]++; + if(!_ledcAttachChannelAdvanced(pin_pairs[i][0], group_channels_used[group], group, pwm_frequency, _PWM_RES_BIT, high_side_invert)){ + SIMPLEFOC_DEBUG("EP32-DRV: ERROR - Failed to configure pin:", pin_pairs[i][0]); + return SIMPLEFOC_DRIVER_INIT_FAILED; + } + + params->channels[2*i] = static_cast(group_channels_used[group]); + params->groups[2*i] = (ledc_mode_t)group; + + group_channels_used[group]++; + if(!_ledcAttachChannelAdvanced(pin_pairs[i][1], group_channels_used[group], group, pwm_frequency, _PWM_RES_BIT, low_side_invert)){ + SIMPLEFOC_DEBUG("EP32-DRV: ERROR - Failed to configure pin:", pin_pairs[i][0]); + return SIMPLEFOC_DRIVER_INIT_FAILED; + } + + params->channels[2*i+1] = static_cast(group_channels_used[group]); + params->groups[2*i+1] = (ledc_mode_t)group; + } + + SIMPLEFOC_DEBUG("EP32-DRV: 6PWM setup successful in group: ", (group)); + return params; } -void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, void* params) { - SIMPLEFOC_DEBUG("EP32-DRV: 6PWM not supported"); + +void _setPwmPairDutyCycle( void* params, int ind_h, int ind_l, float val, float dead_time, PhaseState ps){ + float pwm_h = _constrain(val - dead_time/2.0, 0, 1.0); + float pwm_l = _constrain(val + dead_time/2.0, 0, 1.0); + + // determine the phase state and set the pwm accordingly + // deactivate phases if needed + if((ps == PhaseState::PHASE_OFF) || (ps == PhaseState::PHASE_LO)){ + _writeDutyCycle(0, params, ind_h); + }else{ + _writeDutyCycle(pwm_h, params, ind_h); + } + if((ps == PhaseState::PHASE_OFF) || (ps == PhaseState::PHASE_HI)){ + _writeDutyCycle(0, params, ind_l); + }else{ + _writeDutyCycle(pwm_l, params, ind_l); + } +} + +void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, PhaseState *phase_state, void* params){ + _setPwmPairDutyCycle(params, 0, 1, dc_a, ((ESP32LEDCDriverParams*)params)->dead_zone, phase_state[0]); + _setPwmPairDutyCycle(params, 2, 3, dc_b, ((ESP32LEDCDriverParams*)params)->dead_zone, phase_state[1]); + _setPwmPairDutyCycle(params, 4, 5, dc_c, ((ESP32LEDCDriverParams*)params)->dead_zone, phase_state[2]); } #endif \ No newline at end of file From ee91e27eb79413ee4ffcec930a41ce01c0e2ad33 Mon Sep 17 00:00:00 2001 From: askuric Date: Sat, 8 Jun 2024 09:33:33 +0200 Subject: [PATCH 16/57] removed the forced ledc --- src/drivers/hardware_api.h | 4 ---- 1 file changed, 4 deletions(-) diff --git a/src/drivers/hardware_api.h b/src/drivers/hardware_api.h index 7df00bf1..7809233d 100644 --- a/src/drivers/hardware_api.h +++ b/src/drivers/hardware_api.h @@ -25,10 +25,6 @@ #define SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH true #endif - - -#define SIMPLEFOC_ESP32_USELEDC - // flag returned if driver init fails #define SIMPLEFOC_DRIVER_INIT_FAILED ((void*)-1) From 53624e48db35b5bae85c9d68c25ab5e41efd7b3d Mon Sep 17 00:00:00 2001 From: askuric Date: Mon, 10 Jun 2024 17:04:45 +0200 Subject: [PATCH 17/57] added the inital support for new mcpwm driver --- src/communication/SimpleFOCDebug.cpp | 15 + src/communication/SimpleFOCDebug.h | 4 +- .../esp32/esp32_driver_mcpwm.cpp | 447 ++++++++++++++ .../esp32/esp32_driver_mcpwm.h | 205 ++++--- .../esp32/esp32_ledc_mcu.cpp | 7 + .../hardware_specific/esp32/esp32_mcu.cpp | 534 ++++++----------- .../teensy/teensy4_mcu1.cpp.new | 543 ------------------ 7 files changed, 762 insertions(+), 993 deletions(-) create mode 100644 src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.cpp delete mode 100644 src/drivers/hardware_specific/teensy/teensy4_mcu1.cpp.new diff --git a/src/communication/SimpleFOCDebug.cpp b/src/communication/SimpleFOCDebug.cpp index 3bb62bce..4d50b87c 100644 --- a/src/communication/SimpleFOCDebug.cpp +++ b/src/communication/SimpleFOCDebug.cpp @@ -38,6 +38,7 @@ void SimpleFOCDebug::println(const __FlashStringHelper* str) { } } + void SimpleFOCDebug::println(const char* str, float val) { if (_debugPrint != NULL) { _debugPrint->print(str); @@ -86,6 +87,20 @@ void SimpleFOCDebug::print(const __FlashStringHelper* str) { } } +void SimpleFOCDebug::print(const StringSumHelper str) { + if (_debugPrint != NULL) { + _debugPrint->print(str.c_str()); + } +} + + +void SimpleFOCDebug::println(const StringSumHelper str) { + if (_debugPrint != NULL) { + _debugPrint->println(str.c_str()); + } +} + + void SimpleFOCDebug::print(int val) { if (_debugPrint != NULL) { diff --git a/src/communication/SimpleFOCDebug.h b/src/communication/SimpleFOCDebug.h index 4fcfd538..668e08af 100644 --- a/src/communication/SimpleFOCDebug.h +++ b/src/communication/SimpleFOCDebug.h @@ -33,13 +33,14 @@ **/ -#ifndef SIMPLEFOC_DISABLE_DEBUG +#ifndef SIMPLEFOC_DISABLE_DEBUG class SimpleFOCDebug { public: static void enable(Print* debugPrint = &Serial); static void println(const __FlashStringHelper* msg); + static void println(const StringSumHelper msg); static void println(const char* msg); static void println(const __FlashStringHelper* msg, float val); static void println(const char* msg, float val); @@ -52,6 +53,7 @@ class SimpleFOCDebug { static void print(const char* msg); static void print(const __FlashStringHelper* msg); + static void print(const StringSumHelper msg); static void print(int val); static void print(float val); diff --git a/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.cpp b/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.cpp new file mode 100644 index 00000000..87f78788 --- /dev/null +++ b/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.cpp @@ -0,0 +1,447 @@ +/* + * MCPWM in Espressif v5.x has + * - 2x groups (units) + * each one has + * - 3 timers + * - 3 operators (that can be associated with any timer) + * which control a 2xPWM signals + * - 1x comparator + 1x generator per PWM signal for independent mode + * - 1x comparator + 2x generator per pair or PWM signals for complementary mode + * + * Independent mode: + * ------------------ + * 6 PWM independent signals per unit + * unit(0/1) > timer(0-2) > operator(0-2) > comparator(0-1) > generator(0-1) > pwm(A/B) + * + * group | timer | operator | comparator | generator | pwm + * -------------------------------------------------------------------------------- + * 0-1 | 0-2 | 0 | 0 | 0 | A + * 0-1 | 0-2 | 0 | 1(0 complementary) | 1 | B + * 0-1 | 0-2 | 1 | 0 | 0 | A + * 0-1 | 0-2 | 1 | 1(0 complementary) | 1 | B + * 0-1 | 0-2 | 2 | 0 | 0 | A + * 0-1 | 0-2 | 2 | 1(0 complementary) | 1 | B + * + * Complementary mode + * ------------------ + * - : 3 pairs of complementary PWM signals per unit + * unit(0/1) > timer(0) > operator(0-2) > comparator(0) > generator(0-1) > pwm(A-B pair) + * + * group | timer | operator | comparator | generator | pwm + * ------------------------------------------------------------------------ + * 0-1 | 0 | 0 | 0 | 0 | A + * 0-1 | 0 | 0 | 0 | 1 | B + * 0-1 | 0 | 1 | 0 | 0 | A + * 0-1 | 0 | 1 | 0 | 1 | B + * 0-1 | 0 | 2 | 0 | 0 | A + * 0-1 | 0 | 2 | 0 | 1 | B + * + * More info + * ---------- + * - timers can be associated with any operator, and multiple operators can be associated with the same timer + * - comparators can be associated with any operator + * - two comparators per operator for independent mode + * - one comparator per operator for complementary mode + * - generators can be associated with any comparator + * - one generator per PWM signal for independent mode + * - two generators per pair of PWM signals for complementary mode + * - dead-time can be set for each generator pair in complementary mode + * + * Docs + * ------- + * More info here: https://www.espressif.com/sites/default/files/documentation/esp32_technical_reference_manual_en.pdf#mcpwm + * and here: // https://docs.espressif.com/projects/esp-idf/en/v5.1.4/esp32/migration-guides/release-5.x/5.0/peripherals.html +*/ +#include "../../hardware_api.h" + +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(SIMPLEFOC_ESP32_USELEDC) + +#include "esp32_driver_mcpwm.h" + +// MCPWM driver hardware timer pointers +mcpwm_timer_handle_t timers[2][3] = {NULL}; +// MCPWM timer periods configured (directly related to the pwm frequency) +uint32_t pwm_periods[2][3]; +// how many pins from the groups 6 pins is used +uint8_t group_pins_used[2] = {0}; +// last operator in the group +mcpwm_oper_handle_t last_operator[2]; + + + +// checking if group has pins available +bool _hasAvailablePins(int group, int no_pins){ + if(group_pins_used[group] + no_pins > 6){ + return false; + } + return true; +} + +// returns the index of the last timer in the group +// -1 if no timer instantiated yet +uint8_t _findLastTimer(int group){ + int i = 0; + for(; i<3; i++){ + if(timers[group][i] == NULL){ + return i-1; + } + } + // return the last index + return i; +} +// returns the index of the next timer to instantiate +// -1 if no timers available +uint8_t _findNextTimer(int group){ + int i = 0; + for(; i<3; i++){ + if(timers[group][i] == NULL){ + return i; + } + } + return -1; +} + +/* + * find the best group for the pins + * if 6pwm + * - Only option is an an empty group + * if 3pwm + * - Best is an empty group (we can set a pwm frequency) + * - Second best is a group with 4pwms (2 operators) available (we can set the pwm frequency -> new timer+new operator) + * - Third best option is any group which has 3pwms available (but uses previously defined pwm_frequency) + * if 1pwm + * - Best option is an empty group (we can set the pwm frequency) + * - Second best is a group with 2pwms (one operator) available (we can set the pwm frequency -> new timer+new operator) + * - Third best is a group with 1pwm available (but uses previously defined pwm_frequency ) + * if 2pwm + * - Best option is an empty group (we can set the pwm frequency) + * - Second best is a group with 2pwms available (we can set the pwm frequency -> new timer+new operator) + * - Third best is one pin per group (but uses previously defined pwm_frequency ) + * if 4pwm + * - best option is an empty group (we can set the pwm frequency) + * - second best is a group with 4pwms available (we can set the pwm frequency -> new timer + new operators) + * - third best is 2pwms per group (we can set the pwm frequency -> new timers + new operators) + * + * PROBLEM: Skipping/loosing channels happens in some cases when the group has already used some odd number of pwm channels (for example 3pwm or 1pwm) + * For example if the group has already used 3pwms, there is one generator that has one pwm channel left. + * If we use this channel we have to use the same timer it has been used with before, so we cannot change the pwm frequency. + * Current implementation does use the remaining channel only if there isn't other options that would allow changing the pwm frequency. + * In this example where we have 3pwms already configured, if we try to configure 2pws after, we will skip the remaining channel + * and use a new timer and operator to allow changing the pwm frequency. In such cases we loose (cannot be used) the remaining channel. + * TODO: use the pwm_frequency to avoid skipping pwm channels ! + * + * returns + * - 1 if solution found in one group + * - 2 if solution requires using both groups + * - 0 if no solution possible +*/ +int _findBestGroup(int no_pins, long pwm_freq, int* group, int* timer){ + // an empty group is always the best option + for(int i=0; i<2; i++){ + if(!group_pins_used[i]){ + *group = i; + *timer=0; // use the first timer in an empty group + return 1; + } + } + + // if 3 or 1pwm + // check if there is available space in one of the groups + // otherwise fail + if(no_pins == 3 || no_pins==1){ + // second best option is if there is a group with + // pair number of pwms available as we can then + // set the pwm frequency + for(int i=0; i<2; i++){ + if(_hasAvailablePins(i, no_pins+1)) { + *group=i; + *timer = _findNextTimer(i); + return 1; + } + } + // third best option is any group that has enough pins + for(int i=0; i<2; i++){ + if(_hasAvailablePins(i, no_pins)) { + *group=i; + *timer = _findLastTimer(i); + return 1; + } + } + } + + // if 2 or 4 pwm + // check if there is available space in one of the groups + // if not check if they can be separated in two groups + if(no_pins == 2 || no_pins==4){ + // second best option is any group that has enough pins + for(int i=0; i<2; i++){ + if(_hasAvailablePins(i, no_pins)) { + *group=i; + *timer = _findNextTimer(i); + return 1; + } + } + // third best option is half pwms per group + int half_no_pins = (int)no_pins/2; + if(_hasAvailablePins(0,half_no_pins) && _hasAvailablePins(1 ,half_no_pins)){ + return 2; + } + } + + // otherwise fail + return 0; +} + + +// configuring center aligned pwm +// More info here: https://docs.espressif.com/projects/esp-idf/en/v5.1.4/esp32/api-reference/peripherals/mcpwm.html#symmetric-dual-edge-active-low +void _configureCenterAlign(mcpwm_gen_handle_t gena, mcpwm_cmpr_handle_t cmpa, bool inverted = false){ + if(inverted) + mcpwm_generator_set_actions_on_compare_event(gena, + MCPWM_GEN_COMPARE_EVENT_ACTION(MCPWM_TIMER_DIRECTION_UP, cmpa, MCPWM_GEN_ACTION_HIGH), + MCPWM_GEN_COMPARE_EVENT_ACTION(MCPWM_TIMER_DIRECTION_DOWN, cmpa, MCPWM_GEN_ACTION_LOW), + MCPWM_GEN_COMPARE_EVENT_ACTION_END()); + else + mcpwm_generator_set_actions_on_compare_event(gena, + MCPWM_GEN_COMPARE_EVENT_ACTION(MCPWM_TIMER_DIRECTION_UP, cmpa, MCPWM_GEN_ACTION_LOW), + MCPWM_GEN_COMPARE_EVENT_ACTION(MCPWM_TIMER_DIRECTION_DOWN, cmpa, MCPWM_GEN_ACTION_HIGH), + MCPWM_GEN_COMPARE_EVENT_ACTION_END()); +} + + + +// Helper function calculating the pwm period from the pwm frequency +// - pwm_frequency - pwm frequency in hertz +// returns pwm period in ticks (uint32_t) +uint32_t _calcPWMPeriod(long pwm_frequency) { + return (uint32_t)(1 * _PWM_TIMEBASE_RESOLUTION_HZ / pwm_frequency); +} +/* + Helper function calculating the pwm frequency from the pwm period + - pwm_period - pwm period in ticks + returns pwm frequency in hertz (long) +*/ +long _calcPWMFreq(long pwm_period) { + return (uint32_t)(1 * _PWM_TIMEBASE_RESOLUTION_HZ / pwm_period / 2); +} + +void* _configure6PWMPinsMCPWM(long pwm_frequency, int mcpwm_group, int timer_no, float dead_zone, int* pins){ + ESP32MCPWMDriverParams* params = new ESP32MCPWMDriverParams{ + .pwm_frequency = pwm_frequency, + .group_id = mcpwm_group + }; + + mcpwm_timer_config_t pwm_config; + pwm_config.group_id = mcpwm_group; + pwm_config.clk_src = MCPWM_TIMER_CLK_SRC_DEFAULT; + pwm_config.resolution_hz = _PWM_TIMEBASE_RESOLUTION_HZ; + pwm_config.count_mode = MCPWM_TIMER_COUNT_MODE_UP_DOWN; + pwm_config.intr_priority = 0; + pwm_config.period_ticks = _calcPWMPeriod(pwm_frequency); + + CHECK_ERR(mcpwm_new_timer(&pwm_config, &timers[mcpwm_group][timer_no]), "Could not initialize the timer in group: " + String(mcpwm_group)); + pwm_periods[mcpwm_group][timer_no] = pwm_config.period_ticks / 2; + params->timers[0] = timers[mcpwm_group][timer_no]; + params->mcpwm_period = pwm_periods[mcpwm_group][timer_no]; + + uint8_t no_operators = 3; // use 3 comparators one per pair of pwms + SIMPLEFOC_ESP32_DEBUG("Configuring " + String(no_operators) + " operators."); + mcpwm_operator_config_t operator_config = { .group_id = mcpwm_group }; + for (int i = 0; i < no_operators; i++) { + CHECK_ERR(mcpwm_new_operator(&operator_config, ¶ms->oper[i]),"Could not create operator "+String(i)); + CHECK_ERR(mcpwm_operator_connect_timer(params->oper[i], params->timers[0]),"Could not connect timer to operator: " + String(i)); + } + +#if SIMPLEFOC_ESP32_HW_DEADTIME == true // hardware dead-time (hardware 6pwm) + + SIMPLEFOC_ESP32_DEBUG("Configuring 6PWM with hardware dead-time"); + + SIMPLEFOC_ESP32_DEBUG("Configuring " + String(no_operators) + " comparators."); + // Create and configure comparators + mcpwm_comparator_config_t comparator_config = {0}; + for (int i = 0; i < no_operators; i++) { + CHECK_ERR(mcpwm_new_comparator(params->oper[i], &comparator_config, ¶ms->comparator[i]),"Could not create comparator: " + String(i)); + CHECK_ERR(mcpwm_comparator_set_compare_value(params->comparator[i], (0)), "Could not set duty on comparator: " + String(i)); + } + + int no_generators = 6; // one per pwm + SIMPLEFOC_ESP32_DEBUG("Configuring " + String(no_generators) + " generators."); + // Create and configure generators + mcpwm_generator_config_t generator_config = {}; + for (int i = 0; i < no_generators; i++) { + generator_config.gen_gpio_num = pins[i]; + int oper_index = (int)floor(i / 2); + CHECK_ERR(mcpwm_new_generator(params->oper[oper_index], &generator_config, ¶ms->generator[i]), "Could not create generator " + String(i)); + } + + SIMPLEFOC_ESP32_DEBUG("Configuring Center-Aligned 6 pwm."); + for (int i = 0; i < 3; i++) { + _configureCenterAlign(params->generator[2*i],params->comparator[i]); + } + // only available for 6pwm + SIMPLEFOC_ESP32_DEBUG("Configuring dead-time."); + uint32_t dead_time = (int)pwm_periods[mcpwm_group][timer_no] * dead_zone; + mcpwm_dead_time_config_t dt_config_high; + dt_config_high.posedge_delay_ticks = dead_time; + dt_config_high.negedge_delay_ticks = 0; + dt_config_high.flags.invert_output = !SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH; + mcpwm_dead_time_config_t dt_config_low; + dt_config_low.posedge_delay_ticks = 0; + dt_config_low.negedge_delay_ticks = dead_time; + dt_config_low.flags.invert_output = SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH; + for (int i = 0; i < 3; i++) { + CHECK_ERR(mcpwm_generator_set_dead_time(params->generator[2*i], params->generator[2*i], &dt_config_high),"Could not set dead time for generator: " + String(i)); + CHECK_ERR(mcpwm_generator_set_dead_time(params->generator[2*i], params->generator[2*i+1], &dt_config_low),"Could not set dead time for generator: " + String(i+1)); + } +#else // software dead-time (software 6pwm) + SIMPLEFOC_ESP32_DEBUG("Configuring 6PWM with software dead-time"); + int no_pins = 6; + SIMPLEFOC_ESP32_DEBUG("Configuring " + String(no_pins) + " comparators."); + // Create and configure comparators + mcpwm_comparator_config_t comparator_config = {0}; + for (int i = 0; i < no_pins; i++) { + int oper_index = (int)floor(i / 2); + CHECK_ERR(mcpwm_new_comparator(params->oper[oper_index], &comparator_config, ¶ms->comparator[i]),"Could not create comparator: " + String(i)); + CHECK_ERR(mcpwm_comparator_set_compare_value(params->comparator[i], (0)), "Could not set duty on comparator: " + String(i)); + } + + SIMPLEFOC_ESP32_DEBUG("Configuring " + String(no_pins) + " generators."); + // Create and configure generators; + mcpwm_generator_config_t generator_config = {}; + for (int i = 0; i < no_pins; i++) { + generator_config.gen_gpio_num = pins[i]; + int oper_index = (int)floor(i / 2); + CHECK_ERR(mcpwm_new_generator(params->oper[oper_index], &generator_config, ¶ms->generator[i]), "Could not create generator " + String(i)); + } + + SIMPLEFOC_ESP32_DEBUG("Configuring center-aligned pwm."); + for (int i = 0; i < 3; i++) { + _configureCenterAlign(params->generator[2*i],params->comparator[2*i], !SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH); + _configureCenterAlign(params->generator[2*i+1],params->comparator[2*i+1], SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH); + } +#endif + SIMPLEFOC_ESP32_DEBUG("Enabling the timer: "+String(timer_no)); + // Enable and start timer + CHECK_ERR(mcpwm_timer_enable(params->timers[0]), "Failed to enable timer!"); + CHECK_ERR(mcpwm_timer_start_stop(params->timers[0], MCPWM_TIMER_START_NO_STOP), "Failed to start the timer!"); + + _delay(1); + SIMPLEFOC_ESP32_DEBUG("MCPWM configured!"); + params->dead_zone = dead_zone; + // save the configuration variables for later + group_pins_used[mcpwm_group] = 6; + return params; +} + + +/* + function configuring the pins for the mcpwm + - pwm_frequency - pwm frequency + - mcpwm_group - mcpwm group + - timer_no - timer number + - no_pins - number of pins + - pins - array of pins + - dead_zone - dead zone + + returns the driver parameters +*/ +void* _configurePinsMCPWM(long pwm_frequency, int mcpwm_group, int timer_no, int no_pins, int* pins){ + + ESP32MCPWMDriverParams* params = new ESP32MCPWMDriverParams{ + .pwm_frequency = pwm_frequency, + .group_id = mcpwm_group + }; + + bool shared_timer = false; + // check if timer is configured + if (timers[mcpwm_group][timer_no] == NULL){ + mcpwm_timer_config_t pwm_config; + pwm_config.group_id = mcpwm_group; + pwm_config.clk_src = MCPWM_TIMER_CLK_SRC_DEFAULT; + pwm_config.resolution_hz = _PWM_TIMEBASE_RESOLUTION_HZ; + pwm_config.count_mode = MCPWM_TIMER_COUNT_MODE_UP_DOWN; + pwm_config.intr_priority = 0; + pwm_config.period_ticks = _calcPWMPeriod(pwm_frequency); + // initialise the timer + CHECK_ERR(mcpwm_new_timer(&pwm_config, &timers[mcpwm_group][timer_no]), "Could not initialize the timer in group: " + String(mcpwm_group)); + // save variables for later + pwm_periods[mcpwm_group][timer_no] = pwm_config.period_ticks / 2; + params->timers[0] = timers[mcpwm_group][timer_no]; + // if the numer of used channels it not pair skip one channel + // the skipped channel cannot be used with the new timer + // TODO avoid loosing channels like this + if(group_pins_used[mcpwm_group] %2) group_pins_used[mcpwm_group]++; + }else{ + // we will use an already instantiated timer + params->timers[0] = timers[mcpwm_group][timer_no]; + SIMPLEFOC_ESP32_DEBUG("Using previously configured timer: " + String(timer_no)); + // but we cannot change its configuration without affecting the other drivers + // so let's first verify that the configuration is the same + if(_calcPWMPeriod(pwm_frequency)/2 != pwm_periods[mcpwm_group][timer_no]){ + SIMPLEFOC_ESP32_DEBUG("ERR: Timer: "+String(timer_no)+" is confgured for freq: "+String(_calcPWMFreq(pwm_periods[mcpwm_group][timer_no]))+", not for freq:" +String(pwm_frequency)); + return SIMPLEFOC_DRIVER_INIT_FAILED; + } + CHECK_ERR(mcpwm_timer_start_stop( params->timers[0], MCPWM_TIMER_STOP_EMPTY), "Failed to stop the timer!"); + + shared_timer = true; + } + + uint8_t no_operators = ceil(no_pins / 2.0); + SIMPLEFOC_ESP32_DEBUG("Configuring " + String(no_operators) + " operators."); + mcpwm_operator_config_t operator_config = { .group_id = mcpwm_group }; + for (int i = 0; i < no_operators; i++) { + if (shared_timer && i == 0) { // first operator already configured + params->oper[0] = last_operator[mcpwm_group]; + continue; + } + CHECK_ERR(mcpwm_new_operator(&operator_config, ¶ms->oper[i]),"Could not create operator "+String(i)); + CHECK_ERR(mcpwm_operator_connect_timer(params->oper[i], params->timers[0]),"Could not connect timer to operator: " + String(i)); + } + // save the last operator in this group + last_operator[mcpwm_group] = params->oper[no_operators - 1]; + + SIMPLEFOC_ESP32_DEBUG("Configuring " + String(no_pins) + " comparators."); + // Create and configure comparators + mcpwm_comparator_config_t comparator_config = {0}; + for (int i = 0; i < no_pins; i++) { + int oper_index = shared_timer ? (int)floor((i + 1) / 2) : (int)floor(i / 2); + CHECK_ERR(mcpwm_new_comparator(params->oper[oper_index], &comparator_config, ¶ms->comparator[i]),"Could not create comparator: " + String(i)); + CHECK_ERR(mcpwm_comparator_set_compare_value(params->comparator[i], (0)), "Could not set duty on comparator: " + String(i)); + } + + SIMPLEFOC_ESP32_DEBUG("Configuring " + String(no_pins) + " generators."); + // Create and configure generators; + mcpwm_generator_config_t generator_config = {}; + for (int i = 0; i < no_pins; i++) { + generator_config.gen_gpio_num = pins[i]; + int oper_index = shared_timer ? (int)floor((i + 1) / 2) : (int)floor(i / 2); + CHECK_ERR(mcpwm_new_generator(params->oper[oper_index], &generator_config, ¶ms->generator[i]), "Could not create generator " + String(i)); + } + + + SIMPLEFOC_ESP32_DEBUG("Configuring center-aligned pwm."); + for (int i = 0; i < no_pins; i++) { + _configureCenterAlign(params->generator[i],params->comparator[i], !SIMPLEFOC_PWM_ACTIVE_HIGH); + } + + SIMPLEFOC_ESP32_DEBUG("Enabling the timer: "+String(timer_no)); + // Enable and start timer if not shared + if (!shared_timer) CHECK_ERR(mcpwm_timer_enable(params->timers[0]), "Failed to enable timer!"); + // start the timer + CHECK_ERR(mcpwm_timer_start_stop(params->timers[0], MCPWM_TIMER_START_NO_STOP), "Failed to start the timer!"); + + _delay(1); + SIMPLEFOC_ESP32_DEBUG("MCPWM configured!"); + // save the configuration variables for later + params->mcpwm_period = pwm_periods[mcpwm_group][timer_no]; + group_pins_used[mcpwm_group] += no_pins; + return params; +} + +// function setting the duty cycle to the MCPWM pin +void _setDutyCycle(mcpwm_cmpr_handle_t cmpr, uint32_t mcpwm_period, float duty_cycle){ + float duty = constrain(duty_cycle, 0.0, 1.0); + mcpwm_comparator_set_compare_value(cmpr, (uint32_t)(mcpwm_period*duty)); +} + +#endif \ No newline at end of file diff --git a/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.h b/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.h index 26db540d..d0b7933e 100644 --- a/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.h +++ b/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.h @@ -5,92 +5,143 @@ #if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(SIMPLEFOC_ESP32_USELEDC) +#include "driver/mcpwm_prelude.h" +#include "soc/mcpwm_reg.h" +#include "soc/mcpwm_struct.h" +#include "esp_idf_version.h" +// version check - this mcpwm driver is specific for ESP-IDF 5.x and arduino-esp32 3.x +#if ESP_IDF_VERSION_MAJOR < 5 +#error SimpleFOC: ESP-IDF version 4 or lower detected. Please update to ESP-IDF 5.x and Arduino-esp32 3.0 (or higher) +#endif -#pragma message("") -#pragma message("SimpleFOC: compiling for ESP32 MCPWM driver") -#pragma message("") +#ifndef SIMPLEFOC_ESP32_HW_DEADTIME + #define SIMPLEFOC_ESP32_HW_DEADTIME true // TODO: Change to false when sw-deadtime & phase_state is approved ready for general use. +#endif +//!< ESP32 MCPWM driver parameters +typedef struct ESP32MCPWMDriverParams { + long pwm_frequency; //!< frequency of the pwm signal + int group_id; //!< group of the mcpwm + mcpwm_timer_handle_t timers[2]; //!< timers of the mcpwm + mcpwm_oper_handle_t oper[3]; //!< operators of the mcpwm + mcpwm_cmpr_handle_t comparator[6]; //!< comparators of the mcpwm + mcpwm_gen_handle_t generator[6]; //!< generators of the mcpwm + uint32_t mcpwm_period; //!< period of the pwm signal + float dead_zone; //!< dead zone of the pwm signal +} ESP32MCPWMDriverParams; -#include "driver/mcpwm.h" -#include "soc/mcpwm_reg.h" -#include "soc/mcpwm_struct.h" -// empty motor slot -#define _EMPTY_SLOT -20 -#define _TAKEN_SLOT -21 +#define SIMPLEFOC_ESP32_DEBUG(str)\ + SimpleFOCDebug::println( "ESP32-DRV: " + String(str));\ + +// macro for checking the error of the mcpwm functions +// if the function returns an error the function will return SIMPLEFOC_DRIVER_INIT_FAILED +#define CHECK_ERR(func_call, message) \ + if ((func_call) != ESP_OK) { \ + SIMPLEFOC_ESP32_DEBUG("ERROR - " + String(message)); \ + return SIMPLEFOC_DRIVER_INIT_FAILED; \ + } + // ABI bus frequency - would be better to take it from somewhere // but I did nto find a good exposed variable #define _MCPWM_FREQ 160e6f - -// preferred pwm resolution default -#define _PWM_RES_DEF 4096 -// min resolution -#define _PWM_RES_MIN 3000 -// max resolution -#define _PWM_RES_MAX 8000 -// pwm frequency -#define _PWM_FREQUENCY 25000 // default -#define _PWM_FREQUENCY_MAX 50000 // mqx - -// structure containing motor slot configuration -// this library supports up to 4 motors -typedef struct { - int pinA; - mcpwm_dev_t* mcpwm_num; - mcpwm_unit_t mcpwm_unit; - mcpwm_operator_t mcpwm_operator; - mcpwm_io_signals_t mcpwm_a; - mcpwm_io_signals_t mcpwm_b; - mcpwm_io_signals_t mcpwm_c; -} bldc_3pwm_motor_slots_t; - -typedef struct { - int pin1A; - mcpwm_dev_t* mcpwm_num; - mcpwm_unit_t mcpwm_unit; - mcpwm_operator_t mcpwm_operator1; - mcpwm_operator_t mcpwm_operator2; - mcpwm_io_signals_t mcpwm_1a; - mcpwm_io_signals_t mcpwm_1b; - mcpwm_io_signals_t mcpwm_2a; - mcpwm_io_signals_t mcpwm_2b; -} stepper_4pwm_motor_slots_t; - -typedef struct { - int pin1pwm; - mcpwm_dev_t* mcpwm_num; - mcpwm_unit_t mcpwm_unit; - mcpwm_operator_t mcpwm_operator; - mcpwm_io_signals_t mcpwm_a; - mcpwm_io_signals_t mcpwm_b; -} stepper_2pwm_motor_slots_t; - -typedef struct { - int pinAH; - mcpwm_dev_t* mcpwm_num; - mcpwm_unit_t mcpwm_unit; - mcpwm_operator_t mcpwm_operator1; - mcpwm_operator_t mcpwm_operator2; - mcpwm_io_signals_t mcpwm_ah; - mcpwm_io_signals_t mcpwm_bh; - mcpwm_io_signals_t mcpwm_ch; - mcpwm_io_signals_t mcpwm_al; - mcpwm_io_signals_t mcpwm_bl; - mcpwm_io_signals_t mcpwm_cl; -} bldc_6pwm_motor_slots_t; - - -typedef struct ESP32MCPWMDriverParams { - long pwm_frequency; - mcpwm_dev_t* mcpwm_dev; - mcpwm_unit_t mcpwm_unit; - mcpwm_operator_t mcpwm_operator1; - mcpwm_operator_t mcpwm_operator2; - float deadtime; -} ESP32MCPWMDriverParams; - +#define _PWM_TIMEBASE_RESOLUTION_HZ (_MCPWM_FREQ) /*!< Resolution of MCPWM */ +// pwm frequency settings +#define _PWM_FREQUENCY 25000 // 25khz +#define _PWM_FREQUENCY_MAX 50000 // 50kHz + + +// low-level configuration API + +/** + * checking if group has pins available + * @param group - group of the mcpwm + * @param no_pins - number of pins + * @returns true if pins are available, false otherwise + */ +bool _hasAvailablePins(int group, int no_pins); +/** + * function finding the last timer in the group + * @param group - group of the mcpwm + * @returns index of the last timer in the group + * -1 if no timer instantiated yet + */ +uint8_t _findLastTimer(int group); + +/** + * function finding the next timer in the group + * @param group - group of the mcpwm + * @returns index of the next timer in the group + * -1 if all timers are used + */ +uint8_t _findNextTimer(int group); + + +/** + * function finding the best group and timer for the pwm signals + * + * @param no_pins - number of pins + * @param pwm_freq - frequency of the pwm signal + * @param group - pointer to the group + * @param timer - pointer to the timer + * @returns + * 1 if solution found in one group + * 2 if solution requires using both groups + * 0 if no solution possible + */ +int _findBestGroup(int no_pins, long pwm_freq, int* group, int* timer); + + +/** + * function configuring the center alignement and inversion of a pwm signal + * @param gena - mcpwm generator handle + * @param cmpa - mcpwm comparator handle + * @param inverted - true if the signal is inverted, false otherwise + */ +void _configureCenterAlign(mcpwm_gen_handle_t gena, mcpwm_cmpr_handle_t cmpa, bool inverted); + +/** + * function calculating the pwm period + * @param pwm_frequency - frequency of the pwm signal + * @return uint32_t - period of the pwm signal + */ +uint32_t _calcPWMPeriod(long pwm_frequency); +/** + * function calculating the pwm frequency + * @param pwm_period - period of the pwm signal + * @return long - frequency of the pwm signal + */ +long _calcPWMFreq(long pwm_period); + +/** + * function configuring the MCPWM for 6pwm + * @param pwm_frequency - frequency of the pwm signal + * @param mcpwm_group - group of the mcpwm + * @param timer_no - timer number + * @param dead_zone - dead zone of the pwm signal + * @param pins - array of pins + * @return ESP32MCPWMDriverParams* - pointer to the driver parameters if successful, -1 if failed + */ +void* _configure6PWMPinsMCPWM(long pwm_frequency, int mcpwm_group, int timer_no, float dead_zone, int* pins); +/** + * function configuring the MCPWM for pwm generation + * @param pwm_frequency - frequency of the pwm signal + * @param mcpwm_group - group of the mcpwm + * @param timer_no - timer number + * @param no_pins - number of pins + * @param pins - array of pins + * @return ESP32MCPWMDriverParams* - pointer to the driver parameters if successful, -1 if failed + */ +void* _configurePinsMCPWM(long pwm_frequency, int mcpwm_group, int timer_no, int no_pins, int* pins); +/** + * function setting the duty cycle to the MCPWM channel + * @param cmpr - mcpwm channel handle + * @param mcpwm_period - period of the pwm signal + * @param duty_cycle - duty cycle of the pwm signal + */ +void _setDutyCycle(mcpwm_cmpr_handle_t cmpr, uint32_t mcpwm_period, float duty_cycle); #endif #endif \ No newline at end of file diff --git a/src/drivers/hardware_specific/esp32/esp32_ledc_mcu.cpp b/src/drivers/hardware_specific/esp32/esp32_ledc_mcu.cpp index b5bc8f9a..3c413725 100644 --- a/src/drivers/hardware_specific/esp32/esp32_ledc_mcu.cpp +++ b/src/drivers/hardware_specific/esp32/esp32_ledc_mcu.cpp @@ -7,6 +7,13 @@ #pragma message("") #include "driver/ledc.h" +#include "esp_idf_version.h" + + +// version check - this ledc driver is specific for ESP-IDF 5.x and arduino-esp32 3.x +#if ESP_IDF_VERSION_MAJOR < 5 +#error SimpleFOC: ESP-IDF version 4 or lower detected. Please update to ESP-IDF 5.x and Arduino-esp32 3.0 (or higher) +#endif #define _PWM_FREQUENCY 25000 // 25khz #define _PWM_FREQUENCY_MAX 38000 // 38khz max to be able to have 10 bit pwm resolution diff --git a/src/drivers/hardware_specific/esp32/esp32_mcu.cpp b/src/drivers/hardware_specific/esp32/esp32_mcu.cpp index 0dc23c10..33f9db20 100644 --- a/src/drivers/hardware_specific/esp32/esp32_mcu.cpp +++ b/src/drivers/hardware_specific/esp32/esp32_mcu.cpp @@ -1,431 +1,221 @@ #include "esp32_driver_mcpwm.h" #if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(SIMPLEFOC_ESP32_USELEDC) + +#pragma message("") +#pragma message("SimpleFOC: compiling for ESP32 MCPWM driver") +#pragma message("") -#ifndef SIMPLEFOC_ESP32_HW_DEADTIME - #define SIMPLEFOC_ESP32_HW_DEADTIME true // TODO: Change to false when sw-deadtime & phase_state is approved ready for general use. -#endif - -// define bldc motor slots array -bldc_3pwm_motor_slots_t esp32_bldc_3pwm_motor_slots[4] = { - {_EMPTY_SLOT, &MCPWM0, MCPWM_UNIT_0, MCPWM_OPR_A, MCPWM0A, MCPWM1A, MCPWM2A}, // 1st motor will be MCPWM0 channel A - {_EMPTY_SLOT, &MCPWM0, MCPWM_UNIT_0, MCPWM_OPR_B, MCPWM0B, MCPWM1B, MCPWM2B}, // 2nd motor will be MCPWM0 channel B - {_EMPTY_SLOT, &MCPWM1, MCPWM_UNIT_1, MCPWM_OPR_A, MCPWM0A, MCPWM1A, MCPWM2A}, // 3rd motor will be MCPWM1 channel A - {_EMPTY_SLOT, &MCPWM1, MCPWM_UNIT_1, MCPWM_OPR_B, MCPWM0B, MCPWM1B, MCPWM2B} // 4th motor will be MCPWM1 channel B - }; - -// define stepper motor slots array -bldc_6pwm_motor_slots_t esp32_bldc_6pwm_motor_slots[2] = { - {_EMPTY_SLOT, &MCPWM0, MCPWM_UNIT_0, MCPWM_OPR_A, MCPWM_OPR_B, MCPWM0A, MCPWM1A, MCPWM2A, MCPWM0B, MCPWM1B, MCPWM2B}, // 1st motor will be on MCPWM0 - {_EMPTY_SLOT, &MCPWM1, MCPWM_UNIT_1, MCPWM_OPR_A, MCPWM_OPR_B, MCPWM0A, MCPWM1A, MCPWM2A, MCPWM0B, MCPWM1B, MCPWM2B}, // 1st motor will be on MCPWM0 - }; - -// define 4pwm stepper motor slots array -stepper_4pwm_motor_slots_t esp32_stepper_4pwm_motor_slots[2] = { - {_EMPTY_SLOT, &MCPWM0, MCPWM_UNIT_0, MCPWM_OPR_A, MCPWM_OPR_B, MCPWM0A, MCPWM1A, MCPWM0B, MCPWM1B}, // 1st motor will be on MCPWM0 - {_EMPTY_SLOT, &MCPWM1, MCPWM_UNIT_1, MCPWM_OPR_A, MCPWM_OPR_B, MCPWM0A, MCPWM1A, MCPWM0B, MCPWM1B}, // 1st motor will be on MCPWM1 - }; - -// define 2pwm stepper motor slots array -stepper_2pwm_motor_slots_t esp32_stepper_2pwm_motor_slots[4] = { - {_EMPTY_SLOT, &MCPWM0, MCPWM_UNIT_0, MCPWM_OPR_A, MCPWM0A, MCPWM1A}, // 1st motor will be MCPWM0 channel A - {_EMPTY_SLOT, &MCPWM0, MCPWM_UNIT_0, MCPWM_OPR_B, MCPWM0B, MCPWM1B}, // 2nd motor will be MCPWM0 channel B - {_EMPTY_SLOT, &MCPWM1, MCPWM_UNIT_1, MCPWM_OPR_A, MCPWM0A, MCPWM1A}, // 3rd motor will be MCPWM1 channel A - {_EMPTY_SLOT, &MCPWM1, MCPWM_UNIT_1, MCPWM_OPR_B, MCPWM0B, MCPWM1B} // 4th motor will be MCPWM1 channel B - }; - - -// configuring high frequency pwm timer -// a lot of help from this post from Paul Gauld -// https://hackaday.io/project/169905-esp-32-bldc-robot-actuator-controller -// a short tutorial for v2.0.1+ -// https://kzhead.info/sun/q6uFktWgkYeMeZ8/esp32-arduino.html -// -void _configureTimerFrequency(long pwm_frequency, mcpwm_dev_t* mcpwm_num, mcpwm_unit_t mcpwm_unit, float dead_zone = NOT_SET){ - - mcpwm_config_t pwm_config; - pwm_config.counter_mode = MCPWM_UP_DOWN_COUNTER; // Up-down counter (triangle wave) - pwm_config.duty_mode = (_isset(dead_zone) || SIMPLEFOC_PWM_ACTIVE_HIGH == true) ? MCPWM_DUTY_MODE_0 : MCPWM_DUTY_MODE_1; // Normally Active HIGH (MCPWM_DUTY_MODE_0) - pwm_config.frequency = 2*pwm_frequency; // set the desired freq - just a placeholder for now https://github.com/simplefoc/Arduino-FOC/issues/76 - mcpwm_init(mcpwm_unit, MCPWM_TIMER_0, &pwm_config); //Configure PWM0A & PWM0B with above settings - mcpwm_init(mcpwm_unit, MCPWM_TIMER_1, &pwm_config); //Configure PWM1A & PWM1B with above settings - mcpwm_init(mcpwm_unit, MCPWM_TIMER_2, &pwm_config); //Configure PWM2A & PWM2B with above settings - - if (_isset(dead_zone)){ - // dead zone is configured - - // When using hardware deadtime, setting the phase_state parameter is not supported. - #if SIMPLEFOC_ESP32_HW_DEADTIME == true - float dead_time = (float)(_MCPWM_FREQ / (pwm_frequency)) * dead_zone; - mcpwm_deadtime_type_t pwm_mode; - if ((SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH == true) && (SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH == true)) {pwm_mode = MCPWM_ACTIVE_HIGH_COMPLIMENT_MODE;} // Normal, noninverting driver - else if ((SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH == true) && (SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH == false)){pwm_mode = MCPWM_ACTIVE_HIGH_MODE;} // Inverted lowside driver - else if ((SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH == false) && (SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH == true)) {pwm_mode = MCPWM_ACTIVE_LOW_MODE;} // Inverted highside driver - else if ((SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH == false) && (SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH == false)){pwm_mode = MCPWM_ACTIVE_LOW_COMPLIMENT_MODE;} // Inverted low- & highside driver. Caution: This may short the FETs on reset of the ESP32, as both pins get pulled low! - mcpwm_deadtime_enable(mcpwm_unit, MCPWM_TIMER_0, pwm_mode, dead_time/2.0, dead_time/2.0); - mcpwm_deadtime_enable(mcpwm_unit, MCPWM_TIMER_1, pwm_mode, dead_time/2.0, dead_time/2.0); - mcpwm_deadtime_enable(mcpwm_unit, MCPWM_TIMER_2, pwm_mode, dead_time/2.0, dead_time/2.0); - #else // Software deadtime - for (int i = 0; i < 3; i++){ - if (SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH == true) {mcpwm_set_duty_type(mcpwm_unit, (mcpwm_timer_t) i, MCPWM_GEN_A, MCPWM_DUTY_MODE_0);} // Normal, noninverted highside - else if (SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH == false) {mcpwm_set_duty_type(mcpwm_unit, (mcpwm_timer_t) i, MCPWM_GEN_A, MCPWM_DUTY_MODE_1);} // Inverted highside driver - - if (SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH == true) {mcpwm_set_duty_type(mcpwm_unit, (mcpwm_timer_t) i, MCPWM_GEN_B, MCPWM_DUTY_MODE_1);} // Normal, complementary lowside - else if (SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH == false) {mcpwm_set_duty_type(mcpwm_unit, (mcpwm_timer_t) i, MCPWM_GEN_B, MCPWM_DUTY_MODE_0);} // Inverted lowside driver - } - #endif +// function setting the high pwm frequency to the supplied pins +// - DC motor - 1PWM setting +// - hardware specific +void* _configure1PWM(long pwm_frequency, const int pinA) { + if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25hz + else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 40kHz max + int group, timer; + if(!_findBestGroup(1, pwm_frequency, &group, &timer)) { + SIMPLEFOC_ESP32_DEBUG("Not enough pins available for 1PWM!"); + return SIMPLEFOC_DRIVER_INIT_FAILED; } - _delay(100); - - mcpwm_stop(mcpwm_unit, MCPWM_TIMER_0); - mcpwm_stop(mcpwm_unit, MCPWM_TIMER_1); - mcpwm_stop(mcpwm_unit, MCPWM_TIMER_2); - - // manual configuration due to the lack of config flexibility in mcpwm_init() - mcpwm_num->clk_cfg.clk_prescale = 0; - // calculate prescaler and period - // step 1: calculate the prescaler using the default pwm resolution - // prescaler = bus_freq / (pwm_freq * default_pwm_res) - 1 - int prescaler = ceil((double)_MCPWM_FREQ / (double)_PWM_RES_DEF / 2.0f / (double)pwm_frequency) - 1; - // constrain prescaler - prescaler = _constrain(prescaler, 0, 128); - // now calculate the real resolution timer period necessary (pwm resolution) - // pwm_res = bus_freq / (pwm_freq * (prescaler + 1)) - int resolution_corrected = (double)_MCPWM_FREQ / 2.0f / (double)pwm_frequency / (double)(prescaler + 1); - // if pwm resolution too low lower the prescaler - if(resolution_corrected < _PWM_RES_MIN && prescaler > 0 ) - resolution_corrected = (double)_MCPWM_FREQ / 2.0f / (double)pwm_frequency / (double)(--prescaler + 1); - resolution_corrected = _constrain(resolution_corrected, _PWM_RES_MIN, _PWM_RES_MAX); - - // set prescaler - mcpwm_num->timer[0].timer_cfg0.timer_prescale = prescaler; - mcpwm_num->timer[1].timer_cfg0.timer_prescale = prescaler; - mcpwm_num->timer[2].timer_cfg0.timer_prescale = prescaler; - _delay(1); - //set period - mcpwm_num->timer[0].timer_cfg0.timer_period = resolution_corrected; - mcpwm_num->timer[1].timer_cfg0.timer_period = resolution_corrected; - mcpwm_num->timer[2].timer_cfg0.timer_period = resolution_corrected; - _delay(1); - mcpwm_num->timer[0].timer_cfg0.timer_period_upmethod = 0; - mcpwm_num->timer[1].timer_cfg0.timer_period_upmethod = 0; - mcpwm_num->timer[2].timer_cfg0.timer_period_upmethod = 0; - _delay(1); - // _delay(1); - //restart the timers - mcpwm_start(mcpwm_unit, MCPWM_TIMER_0); - mcpwm_start(mcpwm_unit, MCPWM_TIMER_1); - mcpwm_start(mcpwm_unit, MCPWM_TIMER_2); - _delay(1); - - mcpwm_sync_config_t sync_conf = { - .sync_sig = MCPWM_SELECT_TIMER0_SYNC, - .timer_val = 0, - .count_direction = MCPWM_TIMER_DIRECTION_UP - }; - mcpwm_sync_configure(mcpwm_unit, MCPWM_TIMER_0, &sync_conf); - mcpwm_sync_configure(mcpwm_unit, MCPWM_TIMER_1, &sync_conf); - mcpwm_sync_configure(mcpwm_unit, MCPWM_TIMER_2, &sync_conf); - - // Enable sync event for all timers to be the TEZ of timer0 - mcpwm_set_timer_sync_output(mcpwm_unit, MCPWM_TIMER_0, MCPWM_SWSYNC_SOURCE_TEZ); + SIMPLEFOC_ESP32_DEBUG("Configuring 1PWM in group: "+String(group)+" on timer: "+String(timer)); + // configure the timer + int pins[1] = {pinA}; + return _configurePinsMCPWM(pwm_frequency, group, timer, 1, pins); } + // function setting the high pwm frequency to the supplied pins // - Stepper motor - 2PWM setting -// - hardware speciffic -// supports Arudino/ATmega328, STM32 and ESP32 +// - hardware specific void* _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25hz else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 40kHz max - stepper_2pwm_motor_slots_t m_slot = {}; - - // determine which motor are we connecting - // and set the appropriate configuration parameters - int slot_num; - for(slot_num = 0; slot_num < 4; slot_num++){ - if(esp32_stepper_2pwm_motor_slots[slot_num].pin1pwm == _EMPTY_SLOT){ // put the new motor in the first empty slot - esp32_stepper_2pwm_motor_slots[slot_num].pin1pwm = pinA; - m_slot = esp32_stepper_2pwm_motor_slots[slot_num]; - break; - } + int group, timer; + int ret = _findBestGroup(2, pwm_frequency, &group, &timer); + if(!ret) { + SIMPLEFOC_ESP32_DEBUG("Not enough pins available for 2PWM!"); + return SIMPLEFOC_DRIVER_INIT_FAILED; } - - // disable all the slots with the same MCPWM - // disable 3pwm bldc motor which would go in the same slot - esp32_bldc_3pwm_motor_slots[slot_num].pinA = _TAKEN_SLOT; - if( slot_num < 2 ){ - // slot 0 of the 4pwm stepper - esp32_stepper_4pwm_motor_slots[0].pin1A = _TAKEN_SLOT; - // slot 0 of the 6pwm bldc - esp32_bldc_6pwm_motor_slots[0].pinAH = _TAKEN_SLOT; + if(ret == 1){ + // configure the 2pwm on only one group + SIMPLEFOC_ESP32_DEBUG("Configuring 2PWM in group: "+String(group)+" on timer: "+String(timer)); + // configure the timer + int pins[2] = {pinA, pinB}; + return _configurePinsMCPWM(pwm_frequency, group, timer, 2, pins); }else{ - // slot 1 of the stepper - esp32_stepper_4pwm_motor_slots[1].pin1A = _TAKEN_SLOT; - // slot 1 of the 6pwm bldc - esp32_bldc_6pwm_motor_slots[1].pinAH = _TAKEN_SLOT; + SIMPLEFOC_ESP32_DEBUG("Configuring 2PWM as two 1PWM drivers"); + ESP32MCPWMDriverParams* params[2]; + + // the code is a bit huge for what it does + // it just instantiates two 2PMW drivers and combines the returned params + int pins[2][1] = {{pinA}, {pinB}}; + for(int i =0; i<2; i++){ + int timer = _findLastTimer(i); //find last created timer in group i + SIMPLEFOC_ESP32_DEBUG("Configuring 1PWM in group: "+String(i)+" on timer: "+String(timer)); + void* p = _configurePinsMCPWM(pwm_frequency, i, timer, 1, pins[i]); + if(p == SIMPLEFOC_DRIVER_INIT_FAILED){ + SIMPLEFOC_ESP32_DEBUG("Error configuring 1PWM"); + return SIMPLEFOC_DRIVER_INIT_FAILED; + }else{ + params[i] = (ESP32MCPWMDriverParams*)p; + } + } + // combine the driver parameters + ESP32MCPWMDriverParams* ret_params = new ESP32MCPWMDriverParams{ + .pwm_frequency = params[0]->pwm_frequency, + .group_id = 2, // both groups + }; + for(int i =0; i<2; i++){ + ret_params->timers[i] = params[i]->timers[0]; + ret_params->oper[i] = params[i]->oper[0]; + ret_params->comparator[i] = params[i]->comparator[0]; + ret_params->generator[i] = params[i]->generator[0]; + } + ret_params->mcpwm_period = params[0]->mcpwm_period; + return ret_params; } - // configure pins - mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_a, pinA); - mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_b, pinB); - - // configure the timer - _configureTimerFrequency(pwm_frequency, m_slot.mcpwm_num, m_slot.mcpwm_unit); - - ESP32MCPWMDriverParams* params = new ESP32MCPWMDriverParams { - .pwm_frequency = pwm_frequency, - .mcpwm_dev = m_slot.mcpwm_num, - .mcpwm_unit = m_slot.mcpwm_unit, - .mcpwm_operator1 = m_slot.mcpwm_operator - }; - return params; } - // function setting the high pwm frequency to the supplied pins // - BLDC motor - 3PWM setting -// - hardware speciffic -// supports Arudino/ATmega328, STM32 and ESP32 -void* _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { +// - hardware specific +void* _configure3PWM(long pwm_frequency, const int pinA, const int pinB, const int pinC) { if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25hz else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 40kHz max - bldc_3pwm_motor_slots_t m_slot = {}; - - // determine which motor are we connecting - // and set the appropriate configuration parameters - int slot_num; - for(slot_num = 0; slot_num < 4; slot_num++){ - if(esp32_bldc_3pwm_motor_slots[slot_num].pinA == _EMPTY_SLOT){ // put the new motor in the first empty slot - esp32_bldc_3pwm_motor_slots[slot_num].pinA = pinA; - m_slot = esp32_bldc_3pwm_motor_slots[slot_num]; - break; - } - } - // disable all the slots with the same MCPWM - // disable 2pwm steppr motor which would go in the same slot - esp32_stepper_2pwm_motor_slots[slot_num].pin1pwm = _TAKEN_SLOT; - if( slot_num < 2 ){ - // slot 0 of the 4pwm stepper - esp32_stepper_4pwm_motor_slots[0].pin1A = _TAKEN_SLOT; - // slot 0 of the 6pwm bldc - esp32_bldc_6pwm_motor_slots[0].pinAH = _TAKEN_SLOT; - }else{ - // slot 1 of the stepper - esp32_stepper_4pwm_motor_slots[1].pin1A = _TAKEN_SLOT; - // slot 1 of the 6pwm bldc - esp32_bldc_6pwm_motor_slots[1].pinAH = _TAKEN_SLOT; + int group, timer; + if(!_findBestGroup(3, pwm_frequency, &group, &timer)) { + SIMPLEFOC_ESP32_DEBUG("Not enough pins available for 3PWM!"); + return SIMPLEFOC_DRIVER_INIT_FAILED; } - // configure pins - mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_a, pinA); - mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_b, pinB); - mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_c, pinC); - + SIMPLEFOC_ESP32_DEBUG("Configuring 3PWM in group: "+String(group)+" on timer: "+String(timer)); // configure the timer - _configureTimerFrequency(pwm_frequency, m_slot.mcpwm_num, m_slot.mcpwm_unit); - - ESP32MCPWMDriverParams* params = new ESP32MCPWMDriverParams { - .pwm_frequency = pwm_frequency, - .mcpwm_dev = m_slot.mcpwm_num, - .mcpwm_unit = m_slot.mcpwm_unit, - .mcpwm_operator1 = m_slot.mcpwm_operator - }; - return params; + int pins[3] = {pinA, pinB, pinC}; + return _configurePinsMCPWM(pwm_frequency, group, timer, 3, pins); } + // function setting the high pwm frequency to the supplied pins // - Stepper motor - 4PWM setting -// - hardware speciffic -void* _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC, const int pinD) { +// - hardware specific +void* _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC, const int pinD){ if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25hz else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 40kHz max - stepper_4pwm_motor_slots_t m_slot = {}; - // determine which motor are we connecting - // and set the appropriate configuration parameters - int slot_num; - for(slot_num = 0; slot_num < 2; slot_num++){ - if(esp32_stepper_4pwm_motor_slots[slot_num].pin1A == _EMPTY_SLOT){ // put the new motor in the first empty slot - esp32_stepper_4pwm_motor_slots[slot_num].pin1A = pinA; - m_slot = esp32_stepper_4pwm_motor_slots[slot_num]; - break; - } + + int group, timer; + int ret = _findBestGroup(4, pwm_frequency, &group, &timer); + if(!ret) { + SIMPLEFOC_ESP32_DEBUG("Not enough pins available for 4PWM!"); + return SIMPLEFOC_DRIVER_INIT_FAILED; } - // disable all the slots with the same MCPWM - if( slot_num == 0 ){ - // slots 0 and 1 of the 3pwm bldc - esp32_bldc_3pwm_motor_slots[0].pinA = _TAKEN_SLOT; - esp32_bldc_3pwm_motor_slots[1].pinA = _TAKEN_SLOT; - // slots 0 and 1 of the 2pwm stepper taken - esp32_stepper_2pwm_motor_slots[0].pin1pwm = _TAKEN_SLOT; - esp32_stepper_2pwm_motor_slots[1].pin1pwm = _TAKEN_SLOT; - // slot 0 of the 6pwm bldc - esp32_bldc_6pwm_motor_slots[0].pinAH = _TAKEN_SLOT; + if(ret == 1){ + SIMPLEFOC_ESP32_DEBUG("Configuring 4PWM in group: "+String(group)+" on timer: "+String(timer)); + // configure the timer + int pins[4] = {pinA, pinB, pinC, pinD}; + return _configurePinsMCPWM(pwm_frequency, group, timer, 4, pins); }else{ - // slots 2 and 3 of the 3pwm bldc - esp32_bldc_3pwm_motor_slots[2].pinA = _TAKEN_SLOT; - esp32_bldc_3pwm_motor_slots[3].pinA = _TAKEN_SLOT; - // slots 2 and 3 of the 2pwm stepper taken - esp32_stepper_2pwm_motor_slots[2].pin1pwm = _TAKEN_SLOT; - esp32_stepper_2pwm_motor_slots[3].pin1pwm = _TAKEN_SLOT; - // slot 1 of the 6pwm bldc - esp32_bldc_6pwm_motor_slots[1].pinAH = _TAKEN_SLOT; + SIMPLEFOC_ESP32_DEBUG("Configuring 4PWM as two 2PWM drivers"); + ESP32MCPWMDriverParams* params[2]; + + // the code is a bit huge for what it does + // it just instantiates two 2PMW drivers and combines the returned params + int pins[2][2] = {{pinA, pinB},{pinC, pinD}}; + for(int i =0; i<2; i++){ + int timer = _findNextTimer(i); //find next available timer in group i + SIMPLEFOC_ESP32_DEBUG("Configuring 2PWM in group: "+String(i)+" on timer: "+String(timer)); + void* p = _configurePinsMCPWM(pwm_frequency, i, timer, 2, pins[i]); + if(p == SIMPLEFOC_DRIVER_INIT_FAILED){ + SIMPLEFOC_ESP32_DEBUG("Error configuring 2PWM"); + return SIMPLEFOC_DRIVER_INIT_FAILED; + }else{ + params[i] = (ESP32MCPWMDriverParams*)p; + } + } + // combine the driver parameters + ESP32MCPWMDriverParams* ret_params = new ESP32MCPWMDriverParams{ + .pwm_frequency = params[0]->pwm_frequency, + .group_id = 2, // both groups + .timers = {params[0]->timers[0], params[1]->timers[0]}, + .oper = {params[0]->oper[0], params[1]->oper[0]} + }; + for(int i =0; i<4; i++){ + ret_params->comparator[i] = params[(int)floor(i/2)]->comparator[i%2]; + ret_params->generator[i] = params[(int)floor(i/2)]->generator[i%2]; + } + ret_params->mcpwm_period = params[0]->mcpwm_period; + return ret_params; } - - // configure pins - mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_1a, pinA); - mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_1b, pinB); - mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_2a, pinC); - mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_2b, pinD); - - // configure the timer - _configureTimerFrequency(pwm_frequency, m_slot.mcpwm_num, m_slot.mcpwm_unit); - - ESP32MCPWMDriverParams* params = new ESP32MCPWMDriverParams { - .pwm_frequency = pwm_frequency, - .mcpwm_dev = m_slot.mcpwm_num, - .mcpwm_unit = m_slot.mcpwm_unit, - .mcpwm_operator1 = m_slot.mcpwm_operator1, - .mcpwm_operator2 = m_slot.mcpwm_operator2 - }; - return params; } +void* _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l){ + if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25hz + else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 40kHz max - -// function setting the pwm duty cycle to the hardware -// - Stepper motor - 2PWM setting -// - hardware speciffic -// ESP32 uses MCPWM -void _writeDutyCycle2PWM(float dc_a, float dc_b, void* params){ - // se the PWM on the slot timers - // transform duty cycle from [0,1] to [0,100] - mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_0, ((ESP32MCPWMDriverParams*)params)->mcpwm_operator1, dc_a*100.0); - mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_1, ((ESP32MCPWMDriverParams*)params)->mcpwm_operator1, dc_b*100.0); + int group, timer; + if(!_findBestGroup(6, pwm_frequency, &group, &timer)) { + SIMPLEFOC_ESP32_DEBUG("Not enough pins available for 6PWM!"); + return SIMPLEFOC_DRIVER_INIT_FAILED; + } + SIMPLEFOC_ESP32_DEBUG("Configuring 6PWM in group: "+String(group)+" on timer: "+String(timer)); + // configure the timer + int pins[6] = {pinA_h,pinA_l, pinB_h, pinB_l, pinC_h, pinC_l}; + return _configure6PWMPinsMCPWM(pwm_frequency, group, timer, dead_zone, pins); } - - - // function setting the pwm duty cycle to the hardware // - BLDC motor - 3PWM setting -// - hardware speciffic -// ESP32 uses MCPWM void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, void* params){ - // se the PWM on the slot timers - // transform duty cycle from [0,1] to [0,100] - mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_0, ((ESP32MCPWMDriverParams*)params)->mcpwm_operator1, dc_a*100.0); - mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_1, ((ESP32MCPWMDriverParams*)params)->mcpwm_operator1, dc_b*100.0); - mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_2, ((ESP32MCPWMDriverParams*)params)->mcpwm_operator1, dc_c*100.0); + _setDutyCycle(((ESP32MCPWMDriverParams*)params)->comparator[0], ((ESP32MCPWMDriverParams*)params)->mcpwm_period, dc_a); + _setDutyCycle(((ESP32MCPWMDriverParams*)params)->comparator[1], ((ESP32MCPWMDriverParams*)params)->mcpwm_period, dc_b); + _setDutyCycle(((ESP32MCPWMDriverParams*)params)->comparator[2], ((ESP32MCPWMDriverParams*)params)->mcpwm_period, dc_c); } - - - // function setting the pwm duty cycle to the hardware -// - Stepper motor - 4PWM setting -// - hardware speciffic -// ESP32 uses MCPWM -void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, void* params){ - // se the PWM on the slot timers - // transform duty cycle from [0,1] to [0,100] - mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_0, ((ESP32MCPWMDriverParams*)params)->mcpwm_operator1, dc_1a*100.0); - mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_1, ((ESP32MCPWMDriverParams*)params)->mcpwm_operator1, dc_1b*100.0); - mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_0, ((ESP32MCPWMDriverParams*)params)->mcpwm_operator2, dc_2a*100.0); - mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_1, ((ESP32MCPWMDriverParams*)params)->mcpwm_operator2, dc_2b*100.0); +// - DCMotor -1PWM setting +// - hardware specific +void _writeDutyCycle1PWM(float dc_a, void* params){ + _setDutyCycle(((ESP32MCPWMDriverParams*)params)->comparator[0], ((ESP32MCPWMDriverParams*)params)->mcpwm_period, dc_a); } - - - -// Configuring PWM frequency, resolution and alignment -// - BLDC driver - 6PWM setting +// function setting the pwm duty cycle to the hardware +// - Stepper motor - 2PWM setting // - hardware specific -void* _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l){ - - if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = 20000; // default frequency 20khz - centered pwm has twice lower frequency - else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 40kHz max - centered pwm has twice lower frequency - bldc_6pwm_motor_slots_t m_slot = {}; - // determine which motor are we connecting - // and set the appropriate configuration parameters - int slot_num; - for(slot_num = 0; slot_num < 2; slot_num++){ - if(esp32_bldc_6pwm_motor_slots[slot_num].pinAH == _EMPTY_SLOT){ // put the new motor in the first empty slot - esp32_bldc_6pwm_motor_slots[slot_num].pinAH = pinA_h; - m_slot = esp32_bldc_6pwm_motor_slots[slot_num]; - break; - } - } - // if no slots available - if(slot_num >= 2) return SIMPLEFOC_DRIVER_INIT_FAILED; - - // disable all the slots with the same MCPWM - if( slot_num == 0 ){ - // slots 0 and 1 of the 3pwm bldc - esp32_bldc_3pwm_motor_slots[0].pinA = _TAKEN_SLOT; - esp32_bldc_3pwm_motor_slots[1].pinA = _TAKEN_SLOT; - // slot 0 of the 6pwm bldc - esp32_stepper_4pwm_motor_slots[0].pin1A = _TAKEN_SLOT; - }else{ - // slots 2 and 3 of the 3pwm bldc - esp32_bldc_3pwm_motor_slots[2].pinA = _TAKEN_SLOT; - esp32_bldc_3pwm_motor_slots[3].pinA = _TAKEN_SLOT; - // slot 1 of the 6pwm bldc - esp32_stepper_4pwm_motor_slots[1].pin1A = _TAKEN_SLOT; - } - - // configure pins - mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_ah, pinA_h); - mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_al, pinA_l); - mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_bh, pinB_h); - mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_bl, pinB_l); - mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_ch, pinC_h); - mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_cl, pinC_l); - - // configure the timer - _configureTimerFrequency(pwm_frequency, m_slot.mcpwm_num, m_slot.mcpwm_unit, dead_zone); - // return - ESP32MCPWMDriverParams* params = new ESP32MCPWMDriverParams { - .pwm_frequency = pwm_frequency, - .mcpwm_dev = m_slot.mcpwm_num, - .mcpwm_unit = m_slot.mcpwm_unit, - .mcpwm_operator1 = m_slot.mcpwm_operator1, - .mcpwm_operator2 = m_slot.mcpwm_operator2, - .deadtime = _isset(dead_zone) ? dead_zone : 0 - }; - return params; +void _writeDutyCycle2PWM(float dc_a, float dc_b, void* params){ + _setDutyCycle(((ESP32MCPWMDriverParams*)params)->comparator[0], ((ESP32MCPWMDriverParams*)params)->mcpwm_period, dc_a); + _setDutyCycle(((ESP32MCPWMDriverParams*)params)->comparator[1], ((ESP32MCPWMDriverParams*)params)->mcpwm_period, dc_b); } - -// Function setting the duty cycle to the pwm pin (ex. analogWrite()) -// - BLDC driver - 6PWM setting +// function setting the pwm duty cycle to the hardware +// - Stepper motor - 4PWM setting // - hardware specific -void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, PhaseState *phase_state, void* params){ - // set the PWM on the slot timers - // transform duty cycle from [0,1] to [0,100.0] - #if SIMPLEFOC_ESP32_HW_DEADTIME == true - // Hardware deadtime does deadtime insertion internally - mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_0, MCPWM_OPR_A, dc_a*100.0f); - mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_0, MCPWM_OPR_B, dc_a*100.0f); - mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_1, MCPWM_OPR_A, dc_b*100.0f); - mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_1, MCPWM_OPR_B, dc_b*100.0f); - mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_2, MCPWM_OPR_A, dc_c*100.0f); - mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_2, MCPWM_OPR_B, dc_c*100.0f); - _UNUSED(phase_state); - #else - float deadtime = 0.5f*((ESP32MCPWMDriverParams*)params)->deadtime; - mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_0, MCPWM_OPR_A, (phase_state[0] == PHASE_ON || phase_state[0] == PHASE_HI) ? _constrain(dc_a-deadtime, 0.0f, 1.0f) * 100.0f : 0); - mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_0, MCPWM_OPR_B, (phase_state[0] == PHASE_ON || phase_state[0] == PHASE_LO) ? _constrain(dc_a+deadtime, 0.0f, 1.0f) * 100.0f : 100.0f); - mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_1, MCPWM_OPR_A, (phase_state[1] == PHASE_ON || phase_state[1] == PHASE_HI) ? _constrain(dc_b-deadtime, 0.0f, 1.0f) * 100.0f : 0); - mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_1, MCPWM_OPR_B, (phase_state[1] == PHASE_ON || phase_state[1] == PHASE_LO) ? _constrain(dc_b+deadtime, 0.0f, 1.0f) * 100.0f : 100.0f); - mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_2, MCPWM_OPR_A, (phase_state[2] == PHASE_ON || phase_state[2] == PHASE_HI) ? _constrain(dc_c-deadtime, 0.0f, 1.0f) * 100.0f : 0); - mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_2, MCPWM_OPR_B, (phase_state[2] == PHASE_ON || phase_state[2] == PHASE_LO) ? _constrain(dc_c+deadtime, 0.0f, 1.0f) * 100.0f : 100.0f); - #endif +void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, void* params){ + // se the PWM on the slot timers + _setDutyCycle(((ESP32MCPWMDriverParams*)params)->comparator[0], ((ESP32MCPWMDriverParams*)params)->mcpwm_period, dc_1a); + _setDutyCycle(((ESP32MCPWMDriverParams*)params)->comparator[1], ((ESP32MCPWMDriverParams*)params)->mcpwm_period, dc_1b); + _setDutyCycle(((ESP32MCPWMDriverParams*)params)->comparator[2], ((ESP32MCPWMDriverParams*)params)->mcpwm_period, dc_2a); + _setDutyCycle(((ESP32MCPWMDriverParams*)params)->comparator[3], ((ESP32MCPWMDriverParams*)params)->mcpwm_period, dc_2b); } +void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, PhaseState *phase_state, void* params){ +#if SIMPLEFOC_ESP32_HW_DEADTIME == true + _setDutyCycle(((ESP32MCPWMDriverParams*)params)->comparator[0], ((ESP32MCPWMDriverParams*)params)->mcpwm_period, dc_a); + _setDutyCycle(((ESP32MCPWMDriverParams*)params)->comparator[1], ((ESP32MCPWMDriverParams*)params)->mcpwm_period, dc_b); + _setDutyCycle(((ESP32MCPWMDriverParams*)params)->comparator[2], ((ESP32MCPWMDriverParams*)params)->mcpwm_period, dc_c); +#else + uint32_t period = ((ESP32MCPWMDriverParams*)params)->mcpwm_period; + float dead_zone = (float)((ESP32MCPWMDriverParams*)params)->dead_zone /2.0f; + _setDutyCycle(((ESP32MCPWMDriverParams*)params)->comparator[0], period, (phase_state[0] == PHASE_ON || phase_state[0] == PHASE_HI) ? dc_a-dead_zone : 0.0f); + _setDutyCycle(((ESP32MCPWMDriverParams*)params)->comparator[1], period, (phase_state[0] == PHASE_ON || phase_state[0] == PHASE_LO) ? dc_a+dead_zone : 1.0f); + _setDutyCycle(((ESP32MCPWMDriverParams*)params)->comparator[2], period, (phase_state[1] == PHASE_ON || phase_state[1] == PHASE_HI) ? dc_b-dead_zone : 0.0f); + _setDutyCycle(((ESP32MCPWMDriverParams*)params)->comparator[3], period, (phase_state[1] == PHASE_ON || phase_state[1] == PHASE_LO) ? dc_b+dead_zone : 1.0f); + _setDutyCycle(((ESP32MCPWMDriverParams*)params)->comparator[4], period, (phase_state[2] == PHASE_ON || phase_state[2] == PHASE_HI) ? dc_c-dead_zone : 0.0f); + _setDutyCycle(((ESP32MCPWMDriverParams*)params)->comparator[5], period, (phase_state[2] == PHASE_ON || phase_state[2] == PHASE_LO) ? dc_c+dead_zone : 1.0f); +#endif +} #endif diff --git a/src/drivers/hardware_specific/teensy/teensy4_mcu1.cpp.new b/src/drivers/hardware_specific/teensy/teensy4_mcu1.cpp.new deleted file mode 100644 index 5dcac90d..00000000 --- a/src/drivers/hardware_specific/teensy/teensy4_mcu1.cpp.new +++ /dev/null @@ -1,543 +0,0 @@ -#include "teensy_mcu.h" -#include "teensy4_mcu.h" -#include "../../../communication/SimpleFOCDebug.h" - -// if defined -// - Teensy 4.0 -// - Teensy 4.1 -#if defined(__arm__) && defined(CORE_TEENSY) && ( defined(__IMXRT1062__) || defined(ARDUINO_TEENSY40) || defined(ARDUINO_TEENSY41) || defined(ARDUINO_TEENSY_MICROMOD) ) - - -#pragma message("") -#pragma message("SimpleFOC: compiling for Teensy 4.x") -#pragma message("") - - - -// function finding the TRIG event given the flexpwm timer and the submodule -// returning -1 if the submodule is not valid or no trigger is available -// allowing flexpwm1-4 and submodule 0-3 -// -// the flags are defined in the imxrt.h file -// https://github.com/PaulStoffregen/cores/blob/dd6aa8419ee173a0a6593eab669fbff54ed85f48/teensy4/imxrt.h#L9662-L9693 -int flexpwm_submodule_to_trig(IMXRT_FLEXPWM_t* flexpwm, int submodule){ - if(submodule <0 && submodule > 3) return -1; - if(flexpwm == &IMXRT_FLEXPWM1){ - return XBARA1_IN_FLEXPWM1_PWM1_OUT_TRIG0 + submodule; - }else if(flexpwm == &IMXRT_FLEXPWM2){ - return XBARA1_IN_FLEXPWM2_PWM1_OUT_TRIG0 + submodule; - }else if(flexpwm == &IMXRT_FLEXPWM3){ - return XBARA1_IN_FLEXPWM3_PWM1_OUT_TRIG0 + submodule; - }else if(flexpwm == &IMXRT_FLEXPWM4){ - return XBARA1_IN_FLEXPWM4_PWM1_OUT_TRIG0 + submodule; - } - return -1; -} - -// function finding the EXT_SYNC event given the flexpwm timer and the submodule -// returning -1 if the submodule is not valid or no trigger is available -// allowing flexpwm1-4 and submodule 0-3 -// -// the flags are defined in the imxrt.h file -// https://github.com/PaulStoffregen/cores/blob/dd6aa8419ee173a0a6593eab669fbff54ed85f48/teensy4/imxrt.h#L9757 -int flexpwm_submodule_to_ext_sync(IMXRT_FLEXPWM_t* flexpwm, int submodule){ - if(submodule <0 && submodule > 3) return -1; - if(flexpwm == &IMXRT_FLEXPWM1){ - return XBARA1_OUT_FLEXPWM1_PWM0_EXT_SYNC + submodule; - }else if(flexpwm == &IMXRT_FLEXPWM2){ - return XBARA1_OUT_FLEXPWM2_PWM0_EXT_SYNC + submodule; - }else if(flexpwm == &IMXRT_FLEXPWM3){ - return XBARA1_OUT_FLEXPWM3_EXT_SYNC0 + submodule; // TODO verify why they are not called PWM0_EXT_SYNC but EXT_SYNC0 - }else if(flexpwm == &IMXRT_FLEXPWM4){ - return XBARA1_OUT_FLEXPWM4_EXT_SYNC0 + submodule; // TODO verify why they are not called PWM0_EXT_SYNC but EXT_SYNC0 - } - return -1; -} - -// The i.MXRT1062 uses one config register per two XBAR outputs, so a helper -// function to make code more readable. -void xbar_connect(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 xbar_init() { - CCM_CCGR2 |= CCM_CCGR2_XBAR1(CCM_CCGR_ON); //turn clock on for xbara1 -} - -// half_cycle of the PWM variable -int half_cycle = 0; - -// function which finds the flexpwm instance for a pin -// if it does not belong to the flexpwm timer it returns a nullpointer -IMXRT_FLEXPWM_t* get_flexpwm(uint8_t pin){ - - const struct pwm_pin_info_struct *info; - if (pin >= CORE_NUM_DIGITAL) { -#ifdef SIMPLEFOC_TEENSY_DEBUG - char s[60]; - sprintf (s, "TEENSY-DRV: ERR: Pin: %d not Flextimer pin!", pin); - SIMPLEFOC_DEBUG(s); -#endif - return nullptr; - } - info = pwm_pin_info + pin; - // FlexPWM pin - IMXRT_FLEXPWM_t *flexpwm; - switch ((info->module >> 4) & 3) { - case 0: flexpwm = &IMXRT_FLEXPWM1; break; - case 1: flexpwm = &IMXRT_FLEXPWM2; break; - case 2: flexpwm = &IMXRT_FLEXPWM3; break; - default: flexpwm = &IMXRT_FLEXPWM4; - } - if(flexpwm != nullptr){ -#ifdef SIMPLEFOC_TEENSY_DEBUG - char s[60]; - sprintf (s, "TEENSY-DRV: Pin: %d on Flextimer %d.", pin, ((info->module >> 4) & 3) + 1); - SIMPLEFOC_DEBUG(s); -#endif - return flexpwm; - } - return nullptr; -} - - -// function which finds the timer submodule for a pin -// if it does not belong to the submodule it returns a -1 -int get_submodule(uint8_t pin){ - - const struct pwm_pin_info_struct *info; - if (pin >= CORE_NUM_DIGITAL){ -#ifdef SIMPLEFOC_TEENSY_DEBUG - char s[60]; - sprintf (s, "TEENSY-DRV: ERR: Pin: %d not Flextimer pin!", pin); - SIMPLEFOC_DEBUG(s); -#endif - return -1; - } - - info = pwm_pin_info + pin; - int sm1 = info->module&0x3; - - if (sm1 >= 0 && sm1 < 4) { -#ifdef SIMPLEFOC_TEENSY_DEBUG - char s[60]; - sprintf (s, "TEENSY-DRV: Pin %d on submodule %d.", pin, sm1); - SIMPLEFOC_DEBUG(s); -#endif - return sm1; - } else { -#ifdef SIMPLEFOC_TEENSY_DEBUG - char s[50]; - sprintf (s, "TEENSY-DRV: ERR: Pin: %d not in submodule!", pin); - SIMPLEFOC_DEBUG(s); -#endif - return -1; - } -} - -// function which finds the flexpwm instance for a pair of pins -// if they do not belong to the same timer it returns a nullpointer -IMXRT_FLEXPWM_t* get_flexpwm(uint8_t pin, uint8_t pin1){ - - const struct pwm_pin_info_struct *info; - if (pin >= CORE_NUM_DIGITAL || pin1 >= CORE_NUM_DIGITAL) { -#ifdef SIMPLEFOC_TEENSY_DEBUG - char s[60]; - sprintf (s, "TEENSY-DRV: ERR: Pins: %d, %d not Flextimer pins!", pin, pin1); - SIMPLEFOC_DEBUG(s); -#endif - return nullptr; - } - info = pwm_pin_info + pin; - // FlexPWM pin - IMXRT_FLEXPWM_t *flexpwm1,*flexpwm2; - switch ((info->module >> 4) & 3) { - case 0: flexpwm1 = &IMXRT_FLEXPWM1; break; - case 1: flexpwm1 = &IMXRT_FLEXPWM2; break; - case 2: flexpwm1 = &IMXRT_FLEXPWM3; break; - default: flexpwm1 = &IMXRT_FLEXPWM4; - } - - info = pwm_pin_info + pin1; - switch ((info->module >> 4) & 3) { - case 0: flexpwm2 = &IMXRT_FLEXPWM1; break; - case 1: flexpwm2 = &IMXRT_FLEXPWM2; break; - case 2: flexpwm2 = &IMXRT_FLEXPWM3; break; - default: flexpwm2 = &IMXRT_FLEXPWM4; - } - if(flexpwm1 == flexpwm2){ - char s[60]; - sprintf (s, "TEENSY-DRV: Pins: %d, %d on Flextimer %d.", pin, pin1, ((info->module >> 4) & 3) + 1); - SIMPLEFOC_DEBUG(s); - return flexpwm1; - } else { -#ifdef SIMPLEFOC_TEENSY_DEBUG - char s[60]; - sprintf (s, "TEENSY-DRV: ERR: Pins: %d, %d not in same Flextimer!", pin, pin1); - SIMPLEFOC_DEBUG(s); -#endif - return nullptr; - } -} - - -// function which finds the timer submodule for a pair of pins -// if they do not belong to the same submodule it returns a -1 -int get_submodule(uint8_t pin, uint8_t pin1){ - - const struct pwm_pin_info_struct *info; - if (pin >= CORE_NUM_DIGITAL || pin1 >= CORE_NUM_DIGITAL){ -#ifdef SIMPLEFOC_TEENSY_DEBUG - char s[60]; - sprintf (s, "TEENSY-DRV: ERR: Pins: %d, %d not Flextimer pins!", pin, pin1); - SIMPLEFOC_DEBUG(s); -#endif - return -1; - } - - info = pwm_pin_info + pin; - int sm1 = info->module&0x3; - info = pwm_pin_info + pin1; - int sm2 = info->module&0x3; - - if (sm1 == sm2) { - char s[60]; - sprintf (s, "TEENSY-DRV: Pins: %d, %d on submodule %d.", pin, pin1, sm1); - SIMPLEFOC_DEBUG(s); - return sm1; - } else { -#ifdef SIMPLEFOC_TEENSY_DEBUG - char s[50]; - sprintf (s, "TEENSY-DRV: ERR: Pins: %d, %d not in same submodule!", pin, pin1); - SIMPLEFOC_DEBUG(s); -#endif - return -1; - } -} - - -// function which finds the timer submodule for a pair of pins -// if they do not belong to the same submodule it returns a -1 -int get_inverted_channel(uint8_t pin, uint8_t pin1){ - - const struct pwm_pin_info_struct *info; - if (pin >= CORE_NUM_DIGITAL || pin1 >= CORE_NUM_DIGITAL){ -#ifdef SIMPLEFOC_TEENSY_DEBUG - char s[60]; - sprintf (s, "TEENSY-DRV: ERR: Pins: %d, %d not Flextimer pins!", pin, pin1); - SIMPLEFOC_DEBUG(s); -#endif - return -1; - } - - info = pwm_pin_info + pin; - int ch1 = info->channel; - info = pwm_pin_info + pin1; - int ch2 = info->channel; - - if (ch1 != 1) { -#ifdef SIMPLEFOC_TEENSY_DEBUG - char s[60]; - sprintf (s, "TEENSY-DRV: ERR: Pin: %d on channel %s - only A supported", pin1, ch1==2 ? "B" : "X"); - SIMPLEFOC_DEBUG(s); -#endif - return -1; - } else if (ch2 != 2) { -#ifdef SIMPLEFOC_TEENSY_DEBUG - char s[60]; - sprintf (s, "TEENSY-DRV: ERR: Inverted pin: %d on channel %s - only B supported", pin1, ch2==1 ? "A" : "X"); - SIMPLEFOC_DEBUG(s); -#endif - return -1; - } else { -#ifdef SIMPLEFOC_TEENSY_DEBUG - char s[60]; - sprintf (s, "TEENSY-DRV: Pin: %d on channel B inverted.", pin1); - SIMPLEFOC_DEBUG(s); -#endif -return ch2; - } -} - -// Helper to set up A/B pair on a FlexPWM submodule. -// can configure sync, prescale and B inversion. -// sets the desired frequency of the PWM -// sets the center-aligned pwm -void setup_pwm_pair (IMXRT_FLEXPWM_t * flexpwm, int submodule, bool ext_sync, const long frequency, float dead_zone ) -{ - int submodule_mask = 1 << submodule ; - flexpwm->MCTRL &= ~ FLEXPWM_MCTRL_RUN (submodule_mask) ; // stop it if its already running - flexpwm->MCTRL |= FLEXPWM_MCTRL_CLDOK (submodule_mask) ; // clear load OK - - - // calculate the counter and prescaler for the desired pwm frequency - uint32_t newdiv = (uint32_t)((float)F_BUS_ACTUAL / frequency + 0.5f); - uint32_t prescale = 0; - //printf(" div=%lu\n", newdiv); - while (newdiv > 65535 && prescale < 7) { - newdiv = newdiv >> 1; - prescale = prescale + 1; - } - if (newdiv > 65535) { - newdiv = 65535; - } else if (newdiv < 2) { - newdiv = 2; - } - - // the halfcycle of the PWM - half_cycle = int(newdiv/2.0f); - int dead_time = int(dead_zone*half_cycle); //default dead-time - 2% - int mid_pwm = int((half_cycle)/2.0f); - - // if the timer should be externally synced with the master timer - int sel = ext_sync ? 3 : 0; - - // setup the timer - // https://github.com/PaulStoffregen/cores/blob/master/teensy4/imxrt.h - flexpwm->SM[submodule].CTRL2 = FLEXPWM_SMCTRL2_WAITEN | FLEXPWM_SMCTRL2_DBGEN | - FLEXPWM_SMCTRL2_FRCEN | FLEXPWM_SMCTRL2_INIT_SEL(sel) | FLEXPWM_SMCTRL2_FORCE_SEL(6); - flexpwm->SM[submodule].CTRL = FLEXPWM_SMCTRL_FULL | FLEXPWM_SMCTRL_HALF | FLEXPWM_SMCTRL_PRSC(prescale) ; - // https://github.com/PaulStoffregen/cores/blob/70ba01accd728abe75ebfc8dcd8b3d3a8f3e3f25/teensy4/imxrt.h#L4948 - flexpwm->SM[submodule].OCTRL = 0; //FLEXPWM_SMOCTRL_POLA | FLEXPWM_SMOCTRL_POLB;//channel_to_invert==2 ? 0 : FLEXPWM_SMOCTRL_POLA | FLEXPWM_SMOCTRL_POLB ; - if (!ext_sync) flexpwm->SM[submodule].TCTRL = FLEXPWM_SMTCTRL_OUT_TRIG_EN (0b000010) ; // sync trig out on VAL1 match if master timer - flexpwm->SM[submodule].DTCNT0 = dead_time ; // should try this out (deadtime control) - flexpwm->SM[submodule].DTCNT1 = dead_time ; // should try this out (deadtime control) - flexpwm->SM[submodule].INIT = -half_cycle; // count from -HALFCYCLE to +HALFCYCLE - flexpwm->SM[submodule].VAL0 = 0; - flexpwm->SM[submodule].VAL1 = half_cycle ; - flexpwm->SM[submodule].VAL2 = -mid_pwm ; - flexpwm->SM[submodule].VAL3 = +mid_pwm ; - - flexpwm->MCTRL |= FLEXPWM_MCTRL_LDOK (submodule_mask) ; // loading reenabled - flexpwm->MCTRL |= FLEXPWM_MCTRL_RUN (submodule_mask) ; // start it running -} - - -// staring the PWM on A and B channels of the submodule -void startup_pwm_pair (IMXRT_FLEXPWM_t * flexpwm, int submodule) -{ - int submodule_mask = 1 << submodule ; - - flexpwm->OUTEN |= FLEXPWM_OUTEN_PWMA_EN (submodule_mask); // enable A output - flexpwm->OUTEN |= FLEXPWM_OUTEN_PWMB_EN (submodule_mask); // enable B output -} - - - -// PWM setting on the high and low pair of the PWM channels -void write_pwm_pair(IMXRT_FLEXPWM_t * flexpwm, int submodule, float duty){ - int mid_pwm = int((half_cycle)/2.0f); - int count_pwm = int(mid_pwm*(duty*2-1)) + mid_pwm; - - flexpwm->SM[submodule].VAL2 = -count_pwm; // A on - flexpwm->SM[submodule].VAL3 = count_pwm ; // A off - flexpwm->MCTRL |= FLEXPWM_MCTRL_LDOK (1<SM[submodule].VAL1; - int count_pwm = int(count*duty); - - SIMPLEFOC_DEBUG("VAL0: ",flexpwm->SM[submodule].VAL0); - SIMPLEFOC_DEBUG("VAL1: ",flexpwm->SM[submodule].VAL1); - SIMPLEFOC_DEBUG("count: ",count_pwm); - - // flexpwm->SM[submodule].VAL1 = 0; // A on - flexpwm->SM[submodule].VAL2 = count_pwm ; // A off - flexpwm->SM[submodule].VAL3 = count_pwm; // A on - flexpwm->SM[submodule].VAL4 = count_pwm ; // A off - flexpwm->SM[submodule].VAL5 = count_pwm ; // A off - flexpwm->MCTRL |= FLEXPWM_MCTRL_LDOK (1<SM[submodule].VAL1; - uint32_t cval = ((uint32_t)val * (modulo + 1)) >> analog_write_res; - if (cval > modulo) cval = modulo; // TODO: is this check correct? - - //printf("flexpwmWrite, p=%08lX, sm=%d, ch=%c, cval=%ld\n", - //(uint32_t)p, submodule, channel == 0 ? 'X' : (channel == 1 ? 'A' : 'B'), cval); - p->MCTRL |= FLEXPWM_MCTRL_CLDOK(mask); - switch (channel) { - case 0: // X - p->SM[submodule].VAL0 = modulo - cval; - p->OUTEN |= FLEXPWM_OUTEN_PWMX_EN(mask); - //printf(" write channel X\n"); - break; - case 1: // A - p->SM[submodule].VAL3 = cval; - p->OUTEN |= FLEXPWM_OUTEN_PWMA_EN(mask); - //printf(" write channel A\n"); - break; - case 2: // B - p->SM[submodule].VAL5 = cval; - p->OUTEN |= FLEXPWM_OUTEN_PWMB_EN(mask); - //printf(" write channel B\n"); - } - p->MCTRL |= FLEXPWM_MCTRL_LDOK(mask); -} - - -// function setting the high pwm frequency to the supplied pins -// - Stepper motor - 6PWM setting -// - hardware specific -void* _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l) { - if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz - else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max - - IMXRT_FLEXPWM_t *flexpwmA,*flexpwmB,*flexpwmC; - int submoduleA,submoduleB,submoduleC; - int inverted_channelA,inverted_channelB,inverted_channelC; - flexpwmA = get_flexpwm(pinA_h,pinA_l); - submoduleA = get_submodule(pinA_h,pinA_l); - inverted_channelA = get_inverted_channel(pinA_h,pinA_l); - flexpwmB = get_flexpwm(pinB_h,pinB_l); - submoduleB = get_submodule(pinB_h,pinB_l); - inverted_channelB = get_inverted_channel(pinB_h,pinB_l); - flexpwmC = get_flexpwm(pinC_h,pinC_l); - submoduleC = get_submodule(pinC_h,pinC_l); - inverted_channelC = get_inverted_channel(pinC_h,pinC_l); - - if((flexpwmA == nullptr) || (flexpwmB == nullptr) || (flexpwmC == nullptr) ){ -#ifdef SIMPLEFOC_TEENSY_DEBUG - SIMPLEFOC_DEBUG("TEENSY-DRV: ERR: Flextimer problem - failed driver config!"); -#endif - return SIMPLEFOC_DRIVER_INIT_FAILED; - } - if((submoduleA < 0) || (submoduleB < 0) || (submoduleC < 0) ){ -#ifdef SIMPLEFOC_TEENSY_DEBUG - SIMPLEFOC_DEBUG("TEENSY-DRV: ERR: Flextimer submodule problem - failed driver config!"); -#endif - return SIMPLEFOC_DRIVER_INIT_FAILED; - } - if((inverted_channelA < 0) || (inverted_channelB < 0) || (inverted_channelC < 0) ){ -#ifdef SIMPLEFOC_TEENSY_DEBUG - SIMPLEFOC_DEBUG("TEENSY-DRV: ERR: Flextimer channel problem - failed driver config!"); -#endif - return SIMPLEFOC_DRIVER_INIT_FAILED; - } - - - Teensy4DriverParams* params = new Teensy4DriverParams { - .pins = { pinA_h, pinA_l, pinB_h, pinB_l, pinC_h, pinC_l }, - .flextimers = { flexpwmA, flexpwmB, flexpwmC}, - .submodules = { submoduleA, submoduleB, submoduleC}, - .pwm_frequency = pwm_frequency, - .dead_zone = dead_zone - }; - - - // Configure FlexPWM units, each driving A/B pair, B inverted. - // full speed about 80kHz, prescale 2 (div by 4) gives 20kHz - setup_pwm_pair (flexpwmA, submoduleA, true, pwm_frequency, dead_zone) ; // this is the master, internally synced - setup_pwm_pair (flexpwmB, submoduleB, true, pwm_frequency, dead_zone) ; // others externally synced - setup_pwm_pair (flexpwmC, submoduleC, false, pwm_frequency, dead_zone) ; - delayMicroseconds (100) ; - - // turn on XBAR1 clock for all but stop mode - xbar_init() ; - - // // Connect trigger to synchronize all timers - xbar_connect (flexpwm_submodule_to_trig(flexpwmC, submoduleC), flexpwm_submodule_to_ext_sync(flexpwmA, submoduleA)) ; - xbar_connect (flexpwm_submodule_to_trig(flexpwmC, submoduleC), flexpwm_submodule_to_ext_sync(flexpwmB, submoduleB)) ; - - startup_pwm_pair (flexpwmA, submoduleA) ; - startup_pwm_pair (flexpwmB, submoduleB) ; - startup_pwm_pair (flexpwmC, submoduleC) ; - - - delayMicroseconds(50) ; - // config the pins 2/3/6/9/8/7 as their FLEXPWM alternates. - *portConfigRegister(pinA_h) = pwm_pin_info[pinA_h].muxval ; - *portConfigRegister(pinA_l) = pwm_pin_info[pinA_l].muxval ; - *portConfigRegister(pinB_h) = pwm_pin_info[pinB_h].muxval ; - *portConfigRegister(pinB_l) = pwm_pin_info[pinB_l].muxval ; - *portConfigRegister(pinC_h) = pwm_pin_info[pinC_h].muxval ; - *portConfigRegister(pinC_l) = pwm_pin_info[pinC_l].muxval ; - - return params; -} - - - -// function setting the pwm duty cycle to the hardware -// - Stepper motor - 6PWM setting -// - hardware specific -void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, PhaseState *phase_state, void* params){ - _UNUSED(phase_state); - write_pwm_pair (((Teensy4DriverParams*)params)->flextimers[0], ((Teensy4DriverParams*)params)->submodules[0], dc_a); - write_pwm_pair (((Teensy4DriverParams*)params)->flextimers[1], ((Teensy4DriverParams*)params)->submodules[1], dc_b); - write_pwm_pair (((Teensy4DriverParams*)params)->flextimers[2], ((Teensy4DriverParams*)params)->submodules[2], dc_c); -} - - -// function setting the high pwm frequency to the supplied pins -// - BLDC motor - 3PWM setting -// - hardware speciffic -// in generic case dont do anything - void* _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { - - if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz - else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max - - IMXRT_FLEXPWM_t *flexpwmA,*flexpwmB,*flexpwmC; - int submoduleA,submoduleB,submoduleC; - int inverted_channelA,inverted_channelB,inverted_channelC; - flexpwmA = get_flexpwm(pinA); - submoduleA = get_submodule(pinA); - flexpwmB = get_flexpwm(pinB); - submoduleB = get_submodule(pinB); - flexpwmC = get_flexpwm(pinC); - submoduleC = get_submodule(pinC); - - Teensy4DriverParams* params = new Teensy4DriverParams { - .pins = { pinA, pinB, pinC }, - .flextimers = { flexpwmA, flexpwmB, flexpwmC}, - .submodules = { submoduleA, submoduleB, submoduleC}, - .pwm_frequency = pwm_frequency, - }; - - startup_pwm_pair (flexpwmA, submoduleA) ; - startup_pwm_pair (flexpwmB, submoduleB) ; - startup_pwm_pair (flexpwmC, submoduleC) ; - - // analogWriteFrequency(pinA, pwm_frequency); - // analogWriteFrequency(pinB, pwm_frequency); - // analogWriteFrequency(pinC, pwm_frequency); - // analogWrite(pinA, 0); - // analogWrite(pinB, 0); - // analogWrite(pinC, 0); - - // // config the pins 2/3/6/9/8/7 as their FLEXPWM alternates. - // *portConfigRegister(pinA) = pwm_pin_info[pinA].muxval ; - // *portConfigRegister(pinB) = pwm_pin_info[pinB].muxval ; - // *portConfigRegister(pinC) = pwm_pin_info[pinC].muxval ; - - return params; -} - -// function setting the pwm duty cycle to the hardware -// - Stepper motor - 6PWM setting -// - hardware specific -void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, void* params){ - write_pwm_on_pin (((Teensy4DriverParams*)params)->flextimers[0], ((Teensy4DriverParams*)params)->submodules[0], dc_a); - write_pwm_on_pin (((Teensy4DriverParams*)params)->flextimers[1], ((Teensy4DriverParams*)params)->submodules[1], dc_b); - write_pwm_on_pin (((Teensy4DriverParams*)params)->flextimers[2], ((Teensy4DriverParams*)params)->submodules[2], dc_c); -} - - -#endif \ No newline at end of file From e221e063ba90a3fee7b05b6048361f8423f28f5f Mon Sep 17 00:00:00 2001 From: askuric Date: Mon, 10 Jun 2024 17:05:15 +0200 Subject: [PATCH 18/57] disable current sensing --- src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp | 2 +- src/current_sense/hardware_specific/esp32/esp32_adc_driver.h | 2 +- src/current_sense/hardware_specific/esp32/esp32_mcu.cpp | 2 +- src/current_sense/hardware_specific/esp32/esp32s_adc_driver.cpp | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp b/src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp index 7f0cc310..4a37ddb8 100644 --- a/src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp +++ b/src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp @@ -1,6 +1,6 @@ #include "esp32_adc_driver.h" -#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(CONFIG_IDF_TARGET_ESP32S2) && !defined(CONFIG_IDF_TARGET_ESP32S3) && !defined(SIMPLEFOC_ESP32_USELEDC) +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(CONFIG_IDF_TARGET_ESP32S2) && !defined(CONFIG_IDF_TARGET_ESP32S3) && !defined(SIMPLEFOC_ESP32_USELEDC) && 0 #include "freertos/FreeRTOS.h" #include "freertos/task.h" diff --git a/src/current_sense/hardware_specific/esp32/esp32_adc_driver.h b/src/current_sense/hardware_specific/esp32/esp32_adc_driver.h index f76c003e..357b35b0 100644 --- a/src/current_sense/hardware_specific/esp32/esp32_adc_driver.h +++ b/src/current_sense/hardware_specific/esp32/esp32_adc_driver.h @@ -5,7 +5,7 @@ #include "Arduino.h" -#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(SIMPLEFOC_ESP32_USELEDC) +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(SIMPLEFOC_ESP32_USELEDC) && 0 /* * Get ADC value for pin * */ diff --git a/src/current_sense/hardware_specific/esp32/esp32_mcu.cpp b/src/current_sense/hardware_specific/esp32/esp32_mcu.cpp index 8b09ca4e..d334008e 100644 --- a/src/current_sense/hardware_specific/esp32/esp32_mcu.cpp +++ b/src/current_sense/hardware_specific/esp32/esp32_mcu.cpp @@ -2,7 +2,7 @@ #include "../../../drivers/hardware_api.h" #include "../../../drivers/hardware_specific/esp32/esp32_driver_mcpwm.h" -#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(SIMPLEFOC_ESP32_USELEDC) +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(SIMPLEFOC_ESP32_USELEDC) && 0 #include "esp32_adc_driver.h" diff --git a/src/current_sense/hardware_specific/esp32/esp32s_adc_driver.cpp b/src/current_sense/hardware_specific/esp32/esp32s_adc_driver.cpp index a212d57b..31cbb027 100644 --- a/src/current_sense/hardware_specific/esp32/esp32s_adc_driver.cpp +++ b/src/current_sense/hardware_specific/esp32/esp32s_adc_driver.cpp @@ -1,6 +1,6 @@ #include "esp32_adc_driver.h" -#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && (defined(CONFIG_IDF_TARGET_ESP32S2) || defined(CONFIG_IDF_TARGET_ESP32S3)) && !defined(SIMPLEFOC_ESP32_USELEDC) +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && (defined(CONFIG_IDF_TARGET_ESP32S2) || defined(CONFIG_IDF_TARGET_ESP32S3)) && !defined(SIMPLEFOC_ESP32_USELEDC) && 0 #include "freertos/FreeRTOS.h" #include "freertos/task.h" From a7ed4ba247782915dd5ccbeb8d8258a2fb8e3951 Mon Sep 17 00:00:00 2001 From: askuric Date: Sat, 15 Jun 2024 17:49:38 +0200 Subject: [PATCH 19/57] an initial implementation of the lowside and inline current sense with the new esp32 api --- .../esp32/esp32_adc_driver.cpp | 2 +- .../esp32/esp32_adc_driver.h | 2 +- .../hardware_specific/esp32/esp32_mcu.cpp | 162 +++++++----------- .../esp32/esp32s_adc_driver.cpp | 2 +- src/drivers/BLDCDriver3PWM.cpp | 2 + src/drivers/BLDCDriver6PWM.cpp | 2 + src/drivers/StepperDriver2PWM.cpp | 2 + src/drivers/StepperDriver4PWM.cpp | 2 + src/drivers/hardware_api.h | 11 ++ .../esp32/esp32_driver_mcpwm.cpp | 26 +-- .../esp32/esp32_driver_mcpwm.h | 8 + .../hardware_specific/esp32/esp32_mcu.cpp | 13 ++ src/drivers/hardware_specific/generic_mcu.cpp | 6 + 13 files changed, 128 insertions(+), 112 deletions(-) diff --git a/src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp b/src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp index 4a37ddb8..7f0cc310 100644 --- a/src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp +++ b/src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp @@ -1,6 +1,6 @@ #include "esp32_adc_driver.h" -#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(CONFIG_IDF_TARGET_ESP32S2) && !defined(CONFIG_IDF_TARGET_ESP32S3) && !defined(SIMPLEFOC_ESP32_USELEDC) && 0 +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(CONFIG_IDF_TARGET_ESP32S2) && !defined(CONFIG_IDF_TARGET_ESP32S3) && !defined(SIMPLEFOC_ESP32_USELEDC) #include "freertos/FreeRTOS.h" #include "freertos/task.h" diff --git a/src/current_sense/hardware_specific/esp32/esp32_adc_driver.h b/src/current_sense/hardware_specific/esp32/esp32_adc_driver.h index 357b35b0..c79afdb4 100644 --- a/src/current_sense/hardware_specific/esp32/esp32_adc_driver.h +++ b/src/current_sense/hardware_specific/esp32/esp32_adc_driver.h @@ -5,7 +5,7 @@ #include "Arduino.h" -#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(SIMPLEFOC_ESP32_USELEDC) && 0 +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(SIMPLEFOC_ESP32_USELEDC) /* * Get ADC value for pin * */ diff --git a/src/current_sense/hardware_specific/esp32/esp32_mcu.cpp b/src/current_sense/hardware_specific/esp32/esp32_mcu.cpp index d334008e..f3131a1e 100644 --- a/src/current_sense/hardware_specific/esp32/esp32_mcu.cpp +++ b/src/current_sense/hardware_specific/esp32/esp32_mcu.cpp @@ -2,26 +2,26 @@ #include "../../../drivers/hardware_api.h" #include "../../../drivers/hardware_specific/esp32/esp32_driver_mcpwm.h" -#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(SIMPLEFOC_ESP32_USELEDC) && 0 +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(SIMPLEFOC_ESP32_USELEDC) -#include "esp32_adc_driver.h" - -#include "driver/mcpwm.h" +#include "driver/mcpwm_prelude.h" #include "soc/mcpwm_reg.h" #include "soc/mcpwm_struct.h" - #include #include #define _ADC_VOLTAGE 3.3f #define _ADC_RESOLUTION 4095.0f +// set the pin 19 in high during the adc interrupt +// #define SIMPLEFOC_ESP32_INTERRUPT_DEBUG typedef struct ESP32MCPWMCurrentSenseParams { int pins[3]; float adc_voltage_conv; - mcpwm_unit_t mcpwm_unit; - int buffer_index; + int adc_buffer[3] = {}; + int buffer_index = 0; + int no_adc_channels = 0; } ESP32MCPWMCurrentSenseParams; @@ -36,8 +36,8 @@ float _readADCVoltageInline(const int pinA, const void* cs_params){ // function reading an ADC value and returning the read voltage void* _configureADCInline(const void* driver_params, const int pinA, const int pinB, const int pinC){ - _UNUSED(driver_params); + SIMPLEFOC_DEBUG("ESP32-CS: Configuring ADC inline"); if( _isset(pinA) ) pinMode(pinA, INPUT); if( _isset(pinB) ) pinMode(pinB, INPUT); if( _isset(pinC) ) pinMode(pinC, INPUT); @@ -51,30 +51,15 @@ void* _configureADCInline(const void* driver_params, const int pinA, const int p } - -/** - * Low side adc reading implementation -*/ - -static void IRAM_ATTR mcpwm0_isr_handler(void*); -static void IRAM_ATTR mcpwm1_isr_handler(void*); -byte currentState = 1; -// two mcpwm units -// - max 2 motors per mcpwm unit (6 adc channels) -int adc_pins[2][6]={0}; -int adc_pin_count[2]={0}; -uint32_t adc_buffer[2][6]={0}; -int adc_read_index[2]={0}; - // function reading an ADC value and returning the read voltage float _readADCVoltageLowSide(const int pin, const void* cs_params){ - mcpwm_unit_t unit = ((ESP32MCPWMCurrentSenseParams*)cs_params)->mcpwm_unit; - int buffer_index = ((ESP32MCPWMCurrentSenseParams*)cs_params)->buffer_index; - float adc_voltage_conv = ((ESP32MCPWMCurrentSenseParams*)cs_params)->adc_voltage_conv; - - for(int i=0; i < adc_pin_count[unit]; i++){ - if( pin == ((ESP32MCPWMCurrentSenseParams*)cs_params)->pins[i]) // found in the buffer - return adc_buffer[unit][buffer_index + i] * adc_voltage_conv; + ESP32MCPWMCurrentSenseParams* p = (ESP32MCPWMCurrentSenseParams*)cs_params; + int no_channel = 0; + for(int i=0; i < 3; i++){ + if(!_isset(p->pins[i])) continue; + if(pin == p->pins[i]) // found in the buffer + return p->adc_buffer[no_channel] * p->adc_voltage_conv; + else no_channel++; } // not found return 0; @@ -83,83 +68,68 @@ float _readADCVoltageLowSide(const int pin, const void* cs_params){ // function configuring low-side current sensing void* _configureADCLowSide(const void* driver_params, const int pinA,const int pinB,const int pinC){ - mcpwm_unit_t unit = ((ESP32MCPWMDriverParams*)driver_params)->mcpwm_unit; - int index_start = adc_pin_count[unit]; - if( _isset(pinA) ) adc_pins[unit][adc_pin_count[unit]++] = pinA; - if( _isset(pinB) ) adc_pins[unit][adc_pin_count[unit]++] = pinB; - if( _isset(pinC) ) adc_pins[unit][adc_pin_count[unit]++] = pinC; + SIMPLEFOC_DEBUG("ESP32-CS: Configuring ADC low-side"); + // check if driver timer is already running + // fail if it is + // the easiest way that I've found to check if timer is running + // is to start it and stop it + ESP32MCPWMDriverParams *p = (ESP32MCPWMDriverParams*)driver_params; + if(mcpwm_timer_start_stop(p->timers[0], MCPWM_TIMER_START_NO_STOP) != ESP_ERR_INVALID_STATE){ + // if we get the invalid state error it means that the timer is not enabled + // that means that we can configure it for low-side current sensing + SIMPLEFOC_DEBUG("ESP32-CS: ERR - The timer is already enabled. Cannot be configured for low-side current sensing."); + return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; + } - if( _isset(pinA) ) pinMode(pinA, INPUT); - if( _isset(pinB) ) pinMode(pinB, INPUT); - if( _isset(pinC) ) pinMode(pinC, INPUT); - ESP32MCPWMCurrentSenseParams* params = new ESP32MCPWMCurrentSenseParams { - .pins = { pinA, pinB, pinC }, - .adc_voltage_conv = (_ADC_VOLTAGE)/(_ADC_RESOLUTION), - .mcpwm_unit = unit, - .buffer_index = index_start - }; + ESP32MCPWMCurrentSenseParams* params = new ESP32MCPWMCurrentSenseParams{}; + int no_adc_channels = 0; + if( _isset(pinA) ){ + pinMode(pinA, INPUT); + params->pins[no_adc_channels++] = pinA; + } + if( _isset(pinB) ){ + pinMode(pinB, INPUT); + params->pins[no_adc_channels++] = pinB; + } + if( _isset(pinC) ){ + pinMode(pinC, INPUT); + params->pins[no_adc_channels++] = pinC; + } + params->adc_voltage_conv = (_ADC_VOLTAGE)/(_ADC_RESOLUTION); + params->no_adc_channels = no_adc_channels; return params; } void _driverSyncLowSide(void* driver_params, void* cs_params){ - - mcpwm_dev_t* mcpwm_dev = ((ESP32MCPWMDriverParams*)driver_params)->mcpwm_dev; - mcpwm_unit_t mcpwm_unit = ((ESP32MCPWMDriverParams*)driver_params)->mcpwm_unit; - - // low-side register enable interrupt - mcpwm_dev->int_ena.timer0_tep_int_ena = true;//A PWM timer 0 TEP event will trigger this interrupt - // high side registers enable interrupt - //mcpwm_dev->int_ena.timer0_tep_int_ena = true;//A PWM timer 0 TEZ event will trigger this interrupt - - // register interrupts (mcpwm number, interrupt handler, handler argument = NULL, interrupt signal/flag, return handler = NULL) - if(mcpwm_unit == MCPWM_UNIT_0) - mcpwm_isr_register(mcpwm_unit, mcpwm0_isr_handler, NULL, ESP_INTR_FLAG_IRAM, NULL); //Set ISR Handler - else - mcpwm_isr_register(mcpwm_unit, mcpwm1_isr_handler, NULL, ESP_INTR_FLAG_IRAM, NULL); //Set ISR Handler -} - -static void IRAM_ATTR mcpwm0_isr_handler(void*) __attribute__ ((unused)); - -// Read currents when interrupt is triggered -static void IRAM_ATTR mcpwm0_isr_handler(void*){ - // // high side - // uint32_t mcpwm_intr_status = MCPWM0.int_st.timer0_tez_int_st; +#ifdef SIMPLEFOC_ESP32_INTERRUPT_DEBUG + pinMode(19, OUTPUT); +#endif + ESP32MCPWMDriverParams *p = (ESP32MCPWMDriverParams*)driver_params; - // low side - uint32_t mcpwm_intr_status = MCPWM0.int_st.timer0_tep_int_st; - if(mcpwm_intr_status){ - adc_buffer[0][adc_read_index[0]] = adcRead(adc_pins[0][adc_read_index[0]]); - adc_read_index[0]++; - if(adc_read_index[0] == adc_pin_count[0]) adc_read_index[0] = 0; - } - // low side - MCPWM0.int_clr.timer0_tep_int_clr = mcpwm_intr_status; - // high side - // MCPWM0.int_clr.timer0_tez_int_clr = mcpwm_intr_status_0; + mcpwm_timer_event_callbacks_t cbs_timer = { + .on_full = [](mcpwm_timer_handle_t tim, const mcpwm_timer_event_data_t* edata, void* user_data){ + ESP32MCPWMCurrentSenseParams *p = (ESP32MCPWMCurrentSenseParams*)user_data; + #ifdef SIMPLEFOC_ESP32_INTERRUPT_DEBUG + digitalWrite(19, HIGH); + #endif + // increment buffer index + p->buffer_index = (p->buffer_index + 1) % p->no_adc_channels; + // sample the phase currents one at a time + p->adc_buffer[p->buffer_index] = adcRead(p->pins[p->buffer_index]); + #ifdef SIMPLEFOC_ESP32_INTERRUPT_DEBUG + digitalWrite(19, LOW); + #endif + return true; + } + }; + if(mcpwm_timer_register_event_callbacks(p->timers[0], &cbs_timer, cs_params) != ESP_OK){ + SIMPLEFOC_DEBUG("ESP32-CS: ERR - Failed to sync ADC and driver"); + } } -static void IRAM_ATTR mcpwm1_isr_handler(void*) __attribute__ ((unused)); - -// Read currents when interrupt is triggered -static void IRAM_ATTR mcpwm1_isr_handler(void*){ - // // high side - // uint32_t mcpwm_intr_status = MCPWM1.int_st.timer0_tez_int_st; - - // low side - uint32_t mcpwm_intr_status = MCPWM1.int_st.timer0_tep_int_st; - if(mcpwm_intr_status){ - adc_buffer[1][adc_read_index[1]] = adcRead(adc_pins[1][adc_read_index[1]]); - adc_read_index[1]++; - if(adc_read_index[1] == adc_pin_count[1]) adc_read_index[1] = 0; - } - // low side - MCPWM1.int_clr.timer0_tep_int_clr = mcpwm_intr_status; - // high side - // MCPWM1.int_clr.timer0_tez_int_clr = mcpwm_intr_status_0; -} #endif diff --git a/src/current_sense/hardware_specific/esp32/esp32s_adc_driver.cpp b/src/current_sense/hardware_specific/esp32/esp32s_adc_driver.cpp index 31cbb027..11149b4a 100644 --- a/src/current_sense/hardware_specific/esp32/esp32s_adc_driver.cpp +++ b/src/current_sense/hardware_specific/esp32/esp32s_adc_driver.cpp @@ -1,6 +1,6 @@ #include "esp32_adc_driver.h" -#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && (defined(CONFIG_IDF_TARGET_ESP32S2) || defined(CONFIG_IDF_TARGET_ESP32S3)) && !defined(SIMPLEFOC_ESP32_USELEDC) && 0 +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && (defined(CONFIG_IDF_TARGET_ESP32S2) || defined(CONFIG_IDF_TARGET_ESP32S3)) && !defined(SIMPLEFOC_ESP32_USELEDC) #include "freertos/FreeRTOS.h" #include "freertos/task.h" diff --git a/src/drivers/BLDCDriver3PWM.cpp b/src/drivers/BLDCDriver3PWM.cpp index 637c8db5..f08c9f11 100644 --- a/src/drivers/BLDCDriver3PWM.cpp +++ b/src/drivers/BLDCDriver3PWM.cpp @@ -20,6 +20,8 @@ BLDCDriver3PWM::BLDCDriver3PWM(int phA, int phB, int phC, int en1, int en2, int // enable motor driver void BLDCDriver3PWM::enable(){ + // enable hardware if available + _enablePWM(params); // enable_pin the driver - if enable_pin pin available if ( _isset(enableA_pin) ) digitalWrite(enableA_pin, enable_active_high); if ( _isset(enableB_pin) ) digitalWrite(enableB_pin, enable_active_high); diff --git a/src/drivers/BLDCDriver6PWM.cpp b/src/drivers/BLDCDriver6PWM.cpp index 4981858f..ba19becf 100644 --- a/src/drivers/BLDCDriver6PWM.cpp +++ b/src/drivers/BLDCDriver6PWM.cpp @@ -24,6 +24,8 @@ BLDCDriver6PWM::BLDCDriver6PWM(int phA_h,int phA_l,int phB_h,int phB_l,int phC_h // enable motor driver void BLDCDriver6PWM::enable(){ + // enable hardware if available + _enablePWM(params); // enable_pin the driver - if enable_pin pin available if ( _isset(enable_pin) ) digitalWrite(enable_pin, enable_active_high); // set phase state enabled diff --git a/src/drivers/StepperDriver2PWM.cpp b/src/drivers/StepperDriver2PWM.cpp index dbbf5b8f..e5f1930c 100644 --- a/src/drivers/StepperDriver2PWM.cpp +++ b/src/drivers/StepperDriver2PWM.cpp @@ -43,6 +43,8 @@ StepperDriver2PWM::StepperDriver2PWM(int _pwm1, int _dir1, int _pwm2, int _dir2, // enable motor driver void StepperDriver2PWM::enable(){ + // enable hardware if available + _enablePWM(params); // enable_pin the driver - if enable_pin pin available if ( _isset(enable_pin1) ) digitalWrite(enable_pin1, HIGH); if ( _isset(enable_pin2) ) digitalWrite(enable_pin2, HIGH); diff --git a/src/drivers/StepperDriver4PWM.cpp b/src/drivers/StepperDriver4PWM.cpp index 836f5472..f584a5b9 100644 --- a/src/drivers/StepperDriver4PWM.cpp +++ b/src/drivers/StepperDriver4PWM.cpp @@ -20,6 +20,8 @@ StepperDriver4PWM::StepperDriver4PWM(int ph1A,int ph1B,int ph2A,int ph2B,int en1 // enable motor driver void StepperDriver4PWM::enable(){ + // enable hardware if available + _enablePWM(params); // enable_pin the driver - if enable_pin pin available if ( _isset(enable_pin1) ) digitalWrite(enable_pin1, HIGH); if ( _isset(enable_pin2) ) digitalWrite(enable_pin2, HIGH); diff --git a/src/drivers/hardware_api.h b/src/drivers/hardware_api.h index 7809233d..07abb21e 100644 --- a/src/drivers/hardware_api.h +++ b/src/drivers/hardware_api.h @@ -27,6 +27,7 @@ // flag returned if driver init fails #define SIMPLEFOC_DRIVER_INIT_FAILED ((void*)-1) +#define SIMPLEFOC_DRIVER_INIT_SUCCESS ((void*)1) // generic implementation of the hardware specific structure // containing all the necessary driver parameters @@ -173,5 +174,15 @@ void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, vo */ void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, PhaseState *phase_state, void* params); +/** + * Function enabling the PWM outputs + * - hardware specific + * + * @param params the driver parameters + * + * @return the pointer to the driver parameters if successful, -1 if failed + */ +void* _enablePWM(void* params); + #endif \ No newline at end of file diff --git a/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.cpp b/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.cpp index 87f78788..1686bc0c 100644 --- a/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.cpp +++ b/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.cpp @@ -320,10 +320,6 @@ void* _configure6PWMPinsMCPWM(long pwm_frequency, int mcpwm_group, int timer_no, _configureCenterAlign(params->generator[2*i+1],params->comparator[2*i+1], SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH); } #endif - SIMPLEFOC_ESP32_DEBUG("Enabling the timer: "+String(timer_no)); - // Enable and start timer - CHECK_ERR(mcpwm_timer_enable(params->timers[0]), "Failed to enable timer!"); - CHECK_ERR(mcpwm_timer_start_stop(params->timers[0], MCPWM_TIMER_START_NO_STOP), "Failed to start the timer!"); _delay(1); SIMPLEFOC_ESP32_DEBUG("MCPWM configured!"); @@ -424,12 +420,6 @@ void* _configurePinsMCPWM(long pwm_frequency, int mcpwm_group, int timer_no, int _configureCenterAlign(params->generator[i],params->comparator[i], !SIMPLEFOC_PWM_ACTIVE_HIGH); } - SIMPLEFOC_ESP32_DEBUG("Enabling the timer: "+String(timer_no)); - // Enable and start timer if not shared - if (!shared_timer) CHECK_ERR(mcpwm_timer_enable(params->timers[0]), "Failed to enable timer!"); - // start the timer - CHECK_ERR(mcpwm_timer_start_stop(params->timers[0], MCPWM_TIMER_START_NO_STOP), "Failed to start the timer!"); - _delay(1); SIMPLEFOC_ESP32_DEBUG("MCPWM configured!"); // save the configuration variables for later @@ -439,9 +429,19 @@ void* _configurePinsMCPWM(long pwm_frequency, int mcpwm_group, int timer_no, int } // function setting the duty cycle to the MCPWM pin -void _setDutyCycle(mcpwm_cmpr_handle_t cmpr, uint32_t mcpwm_period, float duty_cycle){ - float duty = constrain(duty_cycle, 0.0, 1.0); - mcpwm_comparator_set_compare_value(cmpr, (uint32_t)(mcpwm_period*duty)); +bool _enableTimer(mcpwm_timer_handle_t timer){ + int ret = mcpwm_timer_enable(timer); // enable the timer + if (ret == ESP_ERR_INVALID_STATE){ // if already enabled + SIMPLEFOC_ESP32_DEBUG("Timer already enabled: "+String(i)); + }else if(ret != ESP_OK){ + SIMPLEFOC_ESP32_DEBUG("Failed to enable timer!"); // if failed + return false; + } + if(mcpwm_timer_start_stop(timer, MCPWM_TIMER_START_NO_STOP)!=ESP_OK){ + SIMPLEFOC_ESP32_DEBUG("Failed to start the timer!"); + return false; + } } + #endif \ No newline at end of file diff --git a/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.h b/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.h index d0b7933e..4aa4f45e 100644 --- a/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.h +++ b/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.h @@ -143,5 +143,13 @@ void* _configurePinsMCPWM(long pwm_frequency, int mcpwm_group, int timer_no, int */ void _setDutyCycle(mcpwm_cmpr_handle_t cmpr, uint32_t mcpwm_period, float duty_cycle); +/** + * Function checking if timer is enabled + * @param timer - mcpwm timer handle + * + * @returns true if timer is enabled, false otherwise + */ +bool _enableTimer(mcpwm_timer_handle_t timer); + #endif #endif \ No newline at end of file diff --git a/src/drivers/hardware_specific/esp32/esp32_mcu.cpp b/src/drivers/hardware_specific/esp32/esp32_mcu.cpp index 33f9db20..bc1990f4 100644 --- a/src/drivers/hardware_specific/esp32/esp32_mcu.cpp +++ b/src/drivers/hardware_specific/esp32/esp32_mcu.cpp @@ -218,4 +218,17 @@ void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, PhaseState *phase_ _setDutyCycle(((ESP32MCPWMDriverParams*)params)->comparator[5], period, (phase_state[2] == PHASE_ON || phase_state[2] == PHASE_LO) ? dc_c+dead_zone : 1.0f); #endif } + +void* _enablePWM(void* params){ + SIMPLEFOC_ESP32_DEBUG("Enabling timers."); + ESP32MCPWMDriverParams* p = (ESP32MCPWMDriverParams*)params; + for (int i=0; i<2; i++){ + if (p->timers[i] == nullptr) continue; // if only one timer + if(!_enableTimer(p->timers[i])){ + return SIMPLEFOC_DRIVER_INIT_FAILED; + } + } + return params; +} + #endif diff --git a/src/drivers/hardware_specific/generic_mcu.cpp b/src/drivers/hardware_specific/generic_mcu.cpp index b6bc2f06..3d648d29 100644 --- a/src/drivers/hardware_specific/generic_mcu.cpp +++ b/src/drivers/hardware_specific/generic_mcu.cpp @@ -122,4 +122,10 @@ __attribute__((weak)) void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc _UNUSED(dc_c); _UNUSED(phase_state); _UNUSED(params); +} + +// function enabling the power stage +// - hardware specific +__attribute__((weak)) void* _enablePWM(void* params){ + return params; } \ No newline at end of file From ab64524dd34fbfcde295c9b3478c60ab0e1bc791 Mon Sep 17 00:00:00 2001 From: askuric Date: Sun, 16 Jun 2024 09:51:12 +0200 Subject: [PATCH 20/57] forgotten import --- src/current_sense/hardware_specific/esp32/esp32_mcu.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/src/current_sense/hardware_specific/esp32/esp32_mcu.cpp b/src/current_sense/hardware_specific/esp32/esp32_mcu.cpp index f3131a1e..f4a95473 100644 --- a/src/current_sense/hardware_specific/esp32/esp32_mcu.cpp +++ b/src/current_sense/hardware_specific/esp32/esp32_mcu.cpp @@ -4,11 +4,19 @@ #if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(SIMPLEFOC_ESP32_USELEDC) +#include "esp32_adc_driver.h" + #include "driver/mcpwm_prelude.h" #include "soc/mcpwm_reg.h" #include "soc/mcpwm_struct.h" #include #include +#include "esp_idf_version.h" + +// version check - this mcpwm driver is specific for ESP-IDF 5.x and arduino-esp32 3.x +#if ESP_IDF_VERSION_MAJOR < 5 +#error SimpleFOC: ESP-IDF version 4 or lower detected. Please update to ESP-IDF 5.x and Arduino-esp32 3.0 (or higher) +#endif #define _ADC_VOLTAGE 3.3f #define _ADC_RESOLUTION 4095.0f From aa8591e4ea0a2c5c744afe9285e93f0bca416a46 Mon Sep 17 00:00:00 2001 From: askuric Date: Sun, 16 Jun 2024 15:47:36 +0200 Subject: [PATCH 21/57] Revert "an initial implementation of the lowside and inline current sense with the new esp32 api" This reverts commit a7ed4ba247782915dd5ccbeb8d8258a2fb8e3951. --- .../esp32/esp32_adc_driver.cpp | 2 +- .../esp32/esp32_adc_driver.h | 2 +- .../hardware_specific/esp32/esp32_mcu.cpp | 166 ++++++++++-------- .../esp32/esp32s_adc_driver.cpp | 2 +- src/drivers/BLDCDriver3PWM.cpp | 2 - src/drivers/BLDCDriver6PWM.cpp | 2 - src/drivers/StepperDriver2PWM.cpp | 2 - src/drivers/StepperDriver4PWM.cpp | 2 - src/drivers/hardware_api.h | 11 -- .../esp32/esp32_driver_mcpwm.cpp | 26 +-- .../esp32/esp32_driver_mcpwm.h | 8 - .../hardware_specific/esp32/esp32_mcu.cpp | 13 -- src/drivers/hardware_specific/generic_mcu.cpp | 6 - 13 files changed, 110 insertions(+), 134 deletions(-) diff --git a/src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp b/src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp index 7f0cc310..4a37ddb8 100644 --- a/src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp +++ b/src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp @@ -1,6 +1,6 @@ #include "esp32_adc_driver.h" -#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(CONFIG_IDF_TARGET_ESP32S2) && !defined(CONFIG_IDF_TARGET_ESP32S3) && !defined(SIMPLEFOC_ESP32_USELEDC) +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(CONFIG_IDF_TARGET_ESP32S2) && !defined(CONFIG_IDF_TARGET_ESP32S3) && !defined(SIMPLEFOC_ESP32_USELEDC) && 0 #include "freertos/FreeRTOS.h" #include "freertos/task.h" diff --git a/src/current_sense/hardware_specific/esp32/esp32_adc_driver.h b/src/current_sense/hardware_specific/esp32/esp32_adc_driver.h index c79afdb4..357b35b0 100644 --- a/src/current_sense/hardware_specific/esp32/esp32_adc_driver.h +++ b/src/current_sense/hardware_specific/esp32/esp32_adc_driver.h @@ -5,7 +5,7 @@ #include "Arduino.h" -#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(SIMPLEFOC_ESP32_USELEDC) +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(SIMPLEFOC_ESP32_USELEDC) && 0 /* * Get ADC value for pin * */ diff --git a/src/current_sense/hardware_specific/esp32/esp32_mcu.cpp b/src/current_sense/hardware_specific/esp32/esp32_mcu.cpp index f4a95473..d334008e 100644 --- a/src/current_sense/hardware_specific/esp32/esp32_mcu.cpp +++ b/src/current_sense/hardware_specific/esp32/esp32_mcu.cpp @@ -2,34 +2,26 @@ #include "../../../drivers/hardware_api.h" #include "../../../drivers/hardware_specific/esp32/esp32_driver_mcpwm.h" -#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(SIMPLEFOC_ESP32_USELEDC) +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(SIMPLEFOC_ESP32_USELEDC) && 0 #include "esp32_adc_driver.h" -#include "driver/mcpwm_prelude.h" +#include "driver/mcpwm.h" #include "soc/mcpwm_reg.h" #include "soc/mcpwm_struct.h" + #include #include -#include "esp_idf_version.h" - -// version check - this mcpwm driver is specific for ESP-IDF 5.x and arduino-esp32 3.x -#if ESP_IDF_VERSION_MAJOR < 5 -#error SimpleFOC: ESP-IDF version 4 or lower detected. Please update to ESP-IDF 5.x and Arduino-esp32 3.0 (or higher) -#endif #define _ADC_VOLTAGE 3.3f #define _ADC_RESOLUTION 4095.0f -// set the pin 19 in high during the adc interrupt -// #define SIMPLEFOC_ESP32_INTERRUPT_DEBUG typedef struct ESP32MCPWMCurrentSenseParams { int pins[3]; float adc_voltage_conv; - int adc_buffer[3] = {}; - int buffer_index = 0; - int no_adc_channels = 0; + mcpwm_unit_t mcpwm_unit; + int buffer_index; } ESP32MCPWMCurrentSenseParams; @@ -44,8 +36,8 @@ float _readADCVoltageInline(const int pinA, const void* cs_params){ // function reading an ADC value and returning the read voltage void* _configureADCInline(const void* driver_params, const int pinA, const int pinB, const int pinC){ + _UNUSED(driver_params); - SIMPLEFOC_DEBUG("ESP32-CS: Configuring ADC inline"); if( _isset(pinA) ) pinMode(pinA, INPUT); if( _isset(pinB) ) pinMode(pinB, INPUT); if( _isset(pinC) ) pinMode(pinC, INPUT); @@ -59,15 +51,30 @@ void* _configureADCInline(const void* driver_params, const int pinA, const int p } + +/** + * Low side adc reading implementation +*/ + +static void IRAM_ATTR mcpwm0_isr_handler(void*); +static void IRAM_ATTR mcpwm1_isr_handler(void*); +byte currentState = 1; +// two mcpwm units +// - max 2 motors per mcpwm unit (6 adc channels) +int adc_pins[2][6]={0}; +int adc_pin_count[2]={0}; +uint32_t adc_buffer[2][6]={0}; +int adc_read_index[2]={0}; + // function reading an ADC value and returning the read voltage float _readADCVoltageLowSide(const int pin, const void* cs_params){ - ESP32MCPWMCurrentSenseParams* p = (ESP32MCPWMCurrentSenseParams*)cs_params; - int no_channel = 0; - for(int i=0; i < 3; i++){ - if(!_isset(p->pins[i])) continue; - if(pin == p->pins[i]) // found in the buffer - return p->adc_buffer[no_channel] * p->adc_voltage_conv; - else no_channel++; + mcpwm_unit_t unit = ((ESP32MCPWMCurrentSenseParams*)cs_params)->mcpwm_unit; + int buffer_index = ((ESP32MCPWMCurrentSenseParams*)cs_params)->buffer_index; + float adc_voltage_conv = ((ESP32MCPWMCurrentSenseParams*)cs_params)->adc_voltage_conv; + + for(int i=0; i < adc_pin_count[unit]; i++){ + if( pin == ((ESP32MCPWMCurrentSenseParams*)cs_params)->pins[i]) // found in the buffer + return adc_buffer[unit][buffer_index + i] * adc_voltage_conv; } // not found return 0; @@ -76,68 +83,83 @@ float _readADCVoltageLowSide(const int pin, const void* cs_params){ // function configuring low-side current sensing void* _configureADCLowSide(const void* driver_params, const int pinA,const int pinB,const int pinC){ - SIMPLEFOC_DEBUG("ESP32-CS: Configuring ADC low-side"); - // check if driver timer is already running - // fail if it is - // the easiest way that I've found to check if timer is running - // is to start it and stop it - ESP32MCPWMDriverParams *p = (ESP32MCPWMDriverParams*)driver_params; - if(mcpwm_timer_start_stop(p->timers[0], MCPWM_TIMER_START_NO_STOP) != ESP_ERR_INVALID_STATE){ - // if we get the invalid state error it means that the timer is not enabled - // that means that we can configure it for low-side current sensing - SIMPLEFOC_DEBUG("ESP32-CS: ERR - The timer is already enabled. Cannot be configured for low-side current sensing."); - return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; - } + mcpwm_unit_t unit = ((ESP32MCPWMDriverParams*)driver_params)->mcpwm_unit; + int index_start = adc_pin_count[unit]; + if( _isset(pinA) ) adc_pins[unit][adc_pin_count[unit]++] = pinA; + if( _isset(pinB) ) adc_pins[unit][adc_pin_count[unit]++] = pinB; + if( _isset(pinC) ) adc_pins[unit][adc_pin_count[unit]++] = pinC; + if( _isset(pinA) ) pinMode(pinA, INPUT); + if( _isset(pinB) ) pinMode(pinB, INPUT); + if( _isset(pinC) ) pinMode(pinC, INPUT); - ESP32MCPWMCurrentSenseParams* params = new ESP32MCPWMCurrentSenseParams{}; - int no_adc_channels = 0; - if( _isset(pinA) ){ - pinMode(pinA, INPUT); - params->pins[no_adc_channels++] = pinA; - } - if( _isset(pinB) ){ - pinMode(pinB, INPUT); - params->pins[no_adc_channels++] = pinB; - } - if( _isset(pinC) ){ - pinMode(pinC, INPUT); - params->pins[no_adc_channels++] = pinC; - } + ESP32MCPWMCurrentSenseParams* params = new ESP32MCPWMCurrentSenseParams { + .pins = { pinA, pinB, pinC }, + .adc_voltage_conv = (_ADC_VOLTAGE)/(_ADC_RESOLUTION), + .mcpwm_unit = unit, + .buffer_index = index_start + }; - params->adc_voltage_conv = (_ADC_VOLTAGE)/(_ADC_RESOLUTION); - params->no_adc_channels = no_adc_channels; return params; } void _driverSyncLowSide(void* driver_params, void* cs_params){ -#ifdef SIMPLEFOC_ESP32_INTERRUPT_DEBUG - pinMode(19, OUTPUT); -#endif - ESP32MCPWMDriverParams *p = (ESP32MCPWMDriverParams*)driver_params; + + mcpwm_dev_t* mcpwm_dev = ((ESP32MCPWMDriverParams*)driver_params)->mcpwm_dev; + mcpwm_unit_t mcpwm_unit = ((ESP32MCPWMDriverParams*)driver_params)->mcpwm_unit; + + // low-side register enable interrupt + mcpwm_dev->int_ena.timer0_tep_int_ena = true;//A PWM timer 0 TEP event will trigger this interrupt + // high side registers enable interrupt + //mcpwm_dev->int_ena.timer0_tep_int_ena = true;//A PWM timer 0 TEZ event will trigger this interrupt + + // register interrupts (mcpwm number, interrupt handler, handler argument = NULL, interrupt signal/flag, return handler = NULL) + if(mcpwm_unit == MCPWM_UNIT_0) + mcpwm_isr_register(mcpwm_unit, mcpwm0_isr_handler, NULL, ESP_INTR_FLAG_IRAM, NULL); //Set ISR Handler + else + mcpwm_isr_register(mcpwm_unit, mcpwm1_isr_handler, NULL, ESP_INTR_FLAG_IRAM, NULL); //Set ISR Handler +} + +static void IRAM_ATTR mcpwm0_isr_handler(void*) __attribute__ ((unused)); + +// Read currents when interrupt is triggered +static void IRAM_ATTR mcpwm0_isr_handler(void*){ + // // high side + // uint32_t mcpwm_intr_status = MCPWM0.int_st.timer0_tez_int_st; - mcpwm_timer_event_callbacks_t cbs_timer = { - .on_full = [](mcpwm_timer_handle_t tim, const mcpwm_timer_event_data_t* edata, void* user_data){ - ESP32MCPWMCurrentSenseParams *p = (ESP32MCPWMCurrentSenseParams*)user_data; - #ifdef SIMPLEFOC_ESP32_INTERRUPT_DEBUG - digitalWrite(19, HIGH); - #endif - // increment buffer index - p->buffer_index = (p->buffer_index + 1) % p->no_adc_channels; - // sample the phase currents one at a time - p->adc_buffer[p->buffer_index] = adcRead(p->pins[p->buffer_index]); - #ifdef SIMPLEFOC_ESP32_INTERRUPT_DEBUG - digitalWrite(19, LOW); - #endif - return true; - } - }; - if(mcpwm_timer_register_event_callbacks(p->timers[0], &cbs_timer, cs_params) != ESP_OK){ - SIMPLEFOC_DEBUG("ESP32-CS: ERR - Failed to sync ADC and driver"); - } + // low side + uint32_t mcpwm_intr_status = MCPWM0.int_st.timer0_tep_int_st; + if(mcpwm_intr_status){ + adc_buffer[0][adc_read_index[0]] = adcRead(adc_pins[0][adc_read_index[0]]); + adc_read_index[0]++; + if(adc_read_index[0] == adc_pin_count[0]) adc_read_index[0] = 0; + } + // low side + MCPWM0.int_clr.timer0_tep_int_clr = mcpwm_intr_status; + // high side + // MCPWM0.int_clr.timer0_tez_int_clr = mcpwm_intr_status_0; } +static void IRAM_ATTR mcpwm1_isr_handler(void*) __attribute__ ((unused)); + +// Read currents when interrupt is triggered +static void IRAM_ATTR mcpwm1_isr_handler(void*){ + // // high side + // uint32_t mcpwm_intr_status = MCPWM1.int_st.timer0_tez_int_st; + + // low side + uint32_t mcpwm_intr_status = MCPWM1.int_st.timer0_tep_int_st; + if(mcpwm_intr_status){ + adc_buffer[1][adc_read_index[1]] = adcRead(adc_pins[1][adc_read_index[1]]); + adc_read_index[1]++; + if(adc_read_index[1] == adc_pin_count[1]) adc_read_index[1] = 0; + } + // low side + MCPWM1.int_clr.timer0_tep_int_clr = mcpwm_intr_status; + // high side + // MCPWM1.int_clr.timer0_tez_int_clr = mcpwm_intr_status_0; +} #endif diff --git a/src/current_sense/hardware_specific/esp32/esp32s_adc_driver.cpp b/src/current_sense/hardware_specific/esp32/esp32s_adc_driver.cpp index 11149b4a..31cbb027 100644 --- a/src/current_sense/hardware_specific/esp32/esp32s_adc_driver.cpp +++ b/src/current_sense/hardware_specific/esp32/esp32s_adc_driver.cpp @@ -1,6 +1,6 @@ #include "esp32_adc_driver.h" -#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && (defined(CONFIG_IDF_TARGET_ESP32S2) || defined(CONFIG_IDF_TARGET_ESP32S3)) && !defined(SIMPLEFOC_ESP32_USELEDC) +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && (defined(CONFIG_IDF_TARGET_ESP32S2) || defined(CONFIG_IDF_TARGET_ESP32S3)) && !defined(SIMPLEFOC_ESP32_USELEDC) && 0 #include "freertos/FreeRTOS.h" #include "freertos/task.h" diff --git a/src/drivers/BLDCDriver3PWM.cpp b/src/drivers/BLDCDriver3PWM.cpp index f08c9f11..637c8db5 100644 --- a/src/drivers/BLDCDriver3PWM.cpp +++ b/src/drivers/BLDCDriver3PWM.cpp @@ -20,8 +20,6 @@ BLDCDriver3PWM::BLDCDriver3PWM(int phA, int phB, int phC, int en1, int en2, int // enable motor driver void BLDCDriver3PWM::enable(){ - // enable hardware if available - _enablePWM(params); // enable_pin the driver - if enable_pin pin available if ( _isset(enableA_pin) ) digitalWrite(enableA_pin, enable_active_high); if ( _isset(enableB_pin) ) digitalWrite(enableB_pin, enable_active_high); diff --git a/src/drivers/BLDCDriver6PWM.cpp b/src/drivers/BLDCDriver6PWM.cpp index ba19becf..4981858f 100644 --- a/src/drivers/BLDCDriver6PWM.cpp +++ b/src/drivers/BLDCDriver6PWM.cpp @@ -24,8 +24,6 @@ BLDCDriver6PWM::BLDCDriver6PWM(int phA_h,int phA_l,int phB_h,int phB_l,int phC_h // enable motor driver void BLDCDriver6PWM::enable(){ - // enable hardware if available - _enablePWM(params); // enable_pin the driver - if enable_pin pin available if ( _isset(enable_pin) ) digitalWrite(enable_pin, enable_active_high); // set phase state enabled diff --git a/src/drivers/StepperDriver2PWM.cpp b/src/drivers/StepperDriver2PWM.cpp index e5f1930c..dbbf5b8f 100644 --- a/src/drivers/StepperDriver2PWM.cpp +++ b/src/drivers/StepperDriver2PWM.cpp @@ -43,8 +43,6 @@ StepperDriver2PWM::StepperDriver2PWM(int _pwm1, int _dir1, int _pwm2, int _dir2, // enable motor driver void StepperDriver2PWM::enable(){ - // enable hardware if available - _enablePWM(params); // enable_pin the driver - if enable_pin pin available if ( _isset(enable_pin1) ) digitalWrite(enable_pin1, HIGH); if ( _isset(enable_pin2) ) digitalWrite(enable_pin2, HIGH); diff --git a/src/drivers/StepperDriver4PWM.cpp b/src/drivers/StepperDriver4PWM.cpp index f584a5b9..836f5472 100644 --- a/src/drivers/StepperDriver4PWM.cpp +++ b/src/drivers/StepperDriver4PWM.cpp @@ -20,8 +20,6 @@ StepperDriver4PWM::StepperDriver4PWM(int ph1A,int ph1B,int ph2A,int ph2B,int en1 // enable motor driver void StepperDriver4PWM::enable(){ - // enable hardware if available - _enablePWM(params); // enable_pin the driver - if enable_pin pin available if ( _isset(enable_pin1) ) digitalWrite(enable_pin1, HIGH); if ( _isset(enable_pin2) ) digitalWrite(enable_pin2, HIGH); diff --git a/src/drivers/hardware_api.h b/src/drivers/hardware_api.h index 07abb21e..7809233d 100644 --- a/src/drivers/hardware_api.h +++ b/src/drivers/hardware_api.h @@ -27,7 +27,6 @@ // flag returned if driver init fails #define SIMPLEFOC_DRIVER_INIT_FAILED ((void*)-1) -#define SIMPLEFOC_DRIVER_INIT_SUCCESS ((void*)1) // generic implementation of the hardware specific structure // containing all the necessary driver parameters @@ -174,15 +173,5 @@ void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, vo */ void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, PhaseState *phase_state, void* params); -/** - * Function enabling the PWM outputs - * - hardware specific - * - * @param params the driver parameters - * - * @return the pointer to the driver parameters if successful, -1 if failed - */ -void* _enablePWM(void* params); - #endif \ No newline at end of file diff --git a/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.cpp b/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.cpp index 1686bc0c..87f78788 100644 --- a/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.cpp +++ b/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.cpp @@ -320,6 +320,10 @@ void* _configure6PWMPinsMCPWM(long pwm_frequency, int mcpwm_group, int timer_no, _configureCenterAlign(params->generator[2*i+1],params->comparator[2*i+1], SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH); } #endif + SIMPLEFOC_ESP32_DEBUG("Enabling the timer: "+String(timer_no)); + // Enable and start timer + CHECK_ERR(mcpwm_timer_enable(params->timers[0]), "Failed to enable timer!"); + CHECK_ERR(mcpwm_timer_start_stop(params->timers[0], MCPWM_TIMER_START_NO_STOP), "Failed to start the timer!"); _delay(1); SIMPLEFOC_ESP32_DEBUG("MCPWM configured!"); @@ -420,6 +424,12 @@ void* _configurePinsMCPWM(long pwm_frequency, int mcpwm_group, int timer_no, int _configureCenterAlign(params->generator[i],params->comparator[i], !SIMPLEFOC_PWM_ACTIVE_HIGH); } + SIMPLEFOC_ESP32_DEBUG("Enabling the timer: "+String(timer_no)); + // Enable and start timer if not shared + if (!shared_timer) CHECK_ERR(mcpwm_timer_enable(params->timers[0]), "Failed to enable timer!"); + // start the timer + CHECK_ERR(mcpwm_timer_start_stop(params->timers[0], MCPWM_TIMER_START_NO_STOP), "Failed to start the timer!"); + _delay(1); SIMPLEFOC_ESP32_DEBUG("MCPWM configured!"); // save the configuration variables for later @@ -429,19 +439,9 @@ void* _configurePinsMCPWM(long pwm_frequency, int mcpwm_group, int timer_no, int } // function setting the duty cycle to the MCPWM pin -bool _enableTimer(mcpwm_timer_handle_t timer){ - int ret = mcpwm_timer_enable(timer); // enable the timer - if (ret == ESP_ERR_INVALID_STATE){ // if already enabled - SIMPLEFOC_ESP32_DEBUG("Timer already enabled: "+String(i)); - }else if(ret != ESP_OK){ - SIMPLEFOC_ESP32_DEBUG("Failed to enable timer!"); // if failed - return false; - } - if(mcpwm_timer_start_stop(timer, MCPWM_TIMER_START_NO_STOP)!=ESP_OK){ - SIMPLEFOC_ESP32_DEBUG("Failed to start the timer!"); - return false; - } +void _setDutyCycle(mcpwm_cmpr_handle_t cmpr, uint32_t mcpwm_period, float duty_cycle){ + float duty = constrain(duty_cycle, 0.0, 1.0); + mcpwm_comparator_set_compare_value(cmpr, (uint32_t)(mcpwm_period*duty)); } - #endif \ No newline at end of file diff --git a/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.h b/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.h index 4aa4f45e..d0b7933e 100644 --- a/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.h +++ b/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.h @@ -143,13 +143,5 @@ void* _configurePinsMCPWM(long pwm_frequency, int mcpwm_group, int timer_no, int */ void _setDutyCycle(mcpwm_cmpr_handle_t cmpr, uint32_t mcpwm_period, float duty_cycle); -/** - * Function checking if timer is enabled - * @param timer - mcpwm timer handle - * - * @returns true if timer is enabled, false otherwise - */ -bool _enableTimer(mcpwm_timer_handle_t timer); - #endif #endif \ No newline at end of file diff --git a/src/drivers/hardware_specific/esp32/esp32_mcu.cpp b/src/drivers/hardware_specific/esp32/esp32_mcu.cpp index bc1990f4..33f9db20 100644 --- a/src/drivers/hardware_specific/esp32/esp32_mcu.cpp +++ b/src/drivers/hardware_specific/esp32/esp32_mcu.cpp @@ -218,17 +218,4 @@ void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, PhaseState *phase_ _setDutyCycle(((ESP32MCPWMDriverParams*)params)->comparator[5], period, (phase_state[2] == PHASE_ON || phase_state[2] == PHASE_LO) ? dc_c+dead_zone : 1.0f); #endif } - -void* _enablePWM(void* params){ - SIMPLEFOC_ESP32_DEBUG("Enabling timers."); - ESP32MCPWMDriverParams* p = (ESP32MCPWMDriverParams*)params; - for (int i=0; i<2; i++){ - if (p->timers[i] == nullptr) continue; // if only one timer - if(!_enableTimer(p->timers[i])){ - return SIMPLEFOC_DRIVER_INIT_FAILED; - } - } - return params; -} - #endif diff --git a/src/drivers/hardware_specific/generic_mcu.cpp b/src/drivers/hardware_specific/generic_mcu.cpp index 3d648d29..b6bc2f06 100644 --- a/src/drivers/hardware_specific/generic_mcu.cpp +++ b/src/drivers/hardware_specific/generic_mcu.cpp @@ -122,10 +122,4 @@ __attribute__((weak)) void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc _UNUSED(dc_c); _UNUSED(phase_state); _UNUSED(params); -} - -// function enabling the power stage -// - hardware specific -__attribute__((weak)) void* _enablePWM(void* params){ - return params; } \ No newline at end of file From 5d4187a3eb18a7e8eb3586115dd6a3b1e3571136 Mon Sep 17 00:00:00 2001 From: askuric Date: Sun, 16 Jun 2024 22:54:19 +0200 Subject: [PATCH 22/57] added the current sensing + phase state for driver --- src/current_sense/LowsideCurrentSense.cpp | 3 +- src/current_sense/hardware_api.h | 5 +- .../esp32/esp32_adc_driver.cpp | 2 +- .../esp32/esp32_adc_driver.h | 2 +- .../esp32/esp32_mcpwm_mcu.cpp | 189 ++++++++++++++++++ .../hardware_specific/esp32/esp32_mcu.cpp | 165 --------------- .../esp32/esp32s_adc_driver.cpp | 2 +- .../hardware_specific/generic_mcu.cpp | 4 +- .../hardware_specific/rp2040/rp2040_mcu.cpp | 2 +- .../hardware_specific/samd/samd21_mcu.cpp | 5 +- .../stm32/b_g431/b_g431_mcu.cpp | 6 +- .../stm32/stm32f1/stm32f1_mcu.cpp | 7 +- .../stm32/stm32f4/stm32f4_mcu.cpp | 7 +- .../stm32/stm32f7/stm32f7_mcu.cpp | 7 +- .../stm32/stm32g4/stm32g4_mcu.cpp | 7 +- .../stm32/stm32l4/stm32l4_mcu.cpp | 6 +- .../hardware_specific/teensy/teensy4_mcu.cpp | 7 +- .../esp32/esp32_driver_mcpwm.cpp | 53 +++-- .../esp32/esp32_driver_mcpwm.h | 19 +- .../esp32/esp32_ledc_mcu.cpp | 2 +- .../{esp32_mcu.cpp => esp32_mcpwm_mcu.cpp} | 37 ++-- .../hardware_specific/esp32/mcpwm_private.h | 78 ++++++++ 22 files changed, 390 insertions(+), 225 deletions(-) create mode 100644 src/current_sense/hardware_specific/esp32/esp32_mcpwm_mcu.cpp delete mode 100644 src/current_sense/hardware_specific/esp32/esp32_mcu.cpp rename src/drivers/hardware_specific/esp32/{esp32_mcu.cpp => esp32_mcpwm_mcu.cpp} (84%) create mode 100644 src/drivers/hardware_specific/esp32/mcpwm_private.h diff --git a/src/current_sense/LowsideCurrentSense.cpp b/src/current_sense/LowsideCurrentSense.cpp index aeb8dea0..9b07d353 100644 --- a/src/current_sense/LowsideCurrentSense.cpp +++ b/src/current_sense/LowsideCurrentSense.cpp @@ -47,7 +47,8 @@ int LowsideCurrentSense::init(){ // if init failed return fail if (params == SIMPLEFOC_CURRENT_SENSE_INIT_FAILED) return 0; // sync the driver - _driverSyncLowSide(driver->params, params); + void* r = _driverSyncLowSide(driver->params, params); + if(r == SIMPLEFOC_CURRENT_SENSE_INIT_FAILED) return 0; // calibrate zero offsets calibrateOffsets(); // set the initialized flag diff --git a/src/current_sense/hardware_api.h b/src/current_sense/hardware_api.h index 7862b708..d1f5f160 100644 --- a/src/current_sense/hardware_api.h +++ b/src/current_sense/hardware_api.h @@ -59,7 +59,10 @@ float _readADCVoltageLowSide(const int pinA, const void* cs_params); * function syncing the Driver with the ADC for the LowSide Sensing * @param driver_params - driver parameter structure - hardware specific * @param cs_params - current sense parameter structure - hardware specific + * + * @return void* - returns the pointer to the current sense parameter structure (unchanged) + * - returns SIMPLEFOC_CURRENT_SENSE_INIT_FAILED if the init fails */ -void _driverSyncLowSide(void* driver_params, void* cs_params); +void* _driverSyncLowSide(void* driver_params, void* cs_params); #endif diff --git a/src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp b/src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp index 4a37ddb8..7f0cc310 100644 --- a/src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp +++ b/src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp @@ -1,6 +1,6 @@ #include "esp32_adc_driver.h" -#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(CONFIG_IDF_TARGET_ESP32S2) && !defined(CONFIG_IDF_TARGET_ESP32S3) && !defined(SIMPLEFOC_ESP32_USELEDC) && 0 +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(CONFIG_IDF_TARGET_ESP32S2) && !defined(CONFIG_IDF_TARGET_ESP32S3) && !defined(SIMPLEFOC_ESP32_USELEDC) #include "freertos/FreeRTOS.h" #include "freertos/task.h" diff --git a/src/current_sense/hardware_specific/esp32/esp32_adc_driver.h b/src/current_sense/hardware_specific/esp32/esp32_adc_driver.h index 357b35b0..f76c003e 100644 --- a/src/current_sense/hardware_specific/esp32/esp32_adc_driver.h +++ b/src/current_sense/hardware_specific/esp32/esp32_adc_driver.h @@ -5,7 +5,7 @@ #include "Arduino.h" -#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(SIMPLEFOC_ESP32_USELEDC) && 0 +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(SIMPLEFOC_ESP32_USELEDC) /* * Get ADC value for pin * */ diff --git a/src/current_sense/hardware_specific/esp32/esp32_mcpwm_mcu.cpp b/src/current_sense/hardware_specific/esp32/esp32_mcpwm_mcu.cpp new file mode 100644 index 00000000..6081f87e --- /dev/null +++ b/src/current_sense/hardware_specific/esp32/esp32_mcpwm_mcu.cpp @@ -0,0 +1,189 @@ +#include "../../hardware_api.h" +#include "../../../drivers/hardware_api.h" +#include "../../../drivers/hardware_specific/esp32/esp32_driver_mcpwm.h" +#include "../../../drivers/hardware_specific/esp32/mcpwm_private.h" + +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(SIMPLEFOC_ESP32_USELEDC) + +// check the version of the ESP-IDF +#include "esp_idf_version.h" + +#if ESP_IDF_VERSION < ESP_IDF_VERSION_VAL(5, 0, 0) +#error SimpleFOC: ESP-IDF version 4 or lower detected. Please update to ESP-IDF 5.x and Arduino-esp32 3.0 (or higher) +#endif + + +#include "esp32_adc_driver.h" + +#include "driver/mcpwm_prelude.h" +#include "soc/mcpwm_reg.h" +#include "soc/mcpwm_struct.h" + +#include +#include + +#ifdef SIMPLEFOC_ESP32_INTERRUPT_DEBUG +#include "driver/gpio.h" +#endif + +#define _ADC_VOLTAGE 3.3f +#define _ADC_RESOLUTION 4095.0f + + + +#define SIMPLEFOC_ESP32_CS_DEBUG(str)\ + SIMPLEFOC_ESP32_DEBUG("CS", str);\ + +#define CHECK_CS_ERR(func_call, message) \ + if ((func_call) != ESP_OK) { \ + SIMPLEFOC_ESP32_CS_DEBUG("ERROR - " + String(message)); \ + return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; \ + } + +typedef struct ESP32MCPWMCurrentSenseParams { + int pins[3]; + float adc_voltage_conv; + int adc_buffer[3] = {}; + int buffer_index = 0; + int no_adc_channels = 0; +} ESP32MCPWMCurrentSenseParams; + + +/** + * Inline adc reading implementation +*/ +// function reading an ADC value and returning the read voltage +float _readADCVoltageInline(const int pinA, const void* cs_params){ + uint32_t raw_adc = adcRead(pinA); + return raw_adc * ((ESP32MCPWMCurrentSenseParams*)cs_params)->adc_voltage_conv; +} + +// function reading an ADC value and returning the read voltage +void* _configureADCInline(const void* driver_params, const int pinA, const int pinB, const int pinC){ + + + if( _isset(pinA) ) pinMode(pinA, INPUT); + if( _isset(pinB) ) pinMode(pinB, INPUT); + if( _isset(pinC) ) pinMode(pinC, INPUT); + + ESP32MCPWMCurrentSenseParams* params = new ESP32MCPWMCurrentSenseParams { + .pins = { pinA, pinB, pinC }, + .adc_voltage_conv = (_ADC_VOLTAGE)/(_ADC_RESOLUTION) + }; + + return params; +} + + +/** + * Low side adc reading implementation +*/ + + +// function reading an ADC value and returning the read voltage +float _readADCVoltageLowSide(const int pin, const void* cs_params){ + ESP32MCPWMCurrentSenseParams* p = (ESP32MCPWMCurrentSenseParams*)cs_params; + int no_channel = 0; + for(int i=0; i < 3; i++){ + if(!_isset(p->pins[i])) continue; + if(pin == p->pins[i]) // found in the buffer + return p->adc_buffer[no_channel] * p->adc_voltage_conv; + else no_channel++; + } + // not found + return 0; +} + + +// function configuring low-side current sensing +void* _configureADCLowSide(const void* driver_params, const int pinA,const int pinB,const int pinC){ + // check if driver timer is already running + // fail if it is + // the easiest way that I've found to check if timer is running + // is to start it and stop it + ESP32MCPWMDriverParams *p = (ESP32MCPWMDriverParams*)driver_params; + mcpwm_timer_t* t = (mcpwm_timer_t*) p->timers[0]; + + // check if low side callback is already set + // if it is, return error + if(t->on_full != nullptr){ + SIMPLEFOC_ESP32_CS_DEBUG("Low side callback is already set. Cannot set it again for timer: "+String(t->timer_id)+", group: "+String(t->group->group_id)); + return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; + } + + ESP32MCPWMCurrentSenseParams* params = new ESP32MCPWMCurrentSenseParams{}; + int no_adc_channels = 0; + if( _isset(pinA) ){ + pinMode(pinA, INPUT); + params->pins[no_adc_channels++] = pinA; + } + if( _isset(pinB) ){ + pinMode(pinB, INPUT); + params->pins[no_adc_channels++] = pinB; + } + if( _isset(pinC) ){ + pinMode(pinC, INPUT); + params->pins[no_adc_channels++] = pinC; + } + + t->user_data = params; + params->adc_voltage_conv = (_ADC_VOLTAGE)/(_ADC_RESOLUTION); + params->no_adc_channels = no_adc_channels; + return params; +} + +void* _driverSyncLowSide(void* driver_params, void* cs_params){ +#ifdef SIMPLEFOC_ESP32_INTERRUPT_DEBUG + pinMode(19, OUTPUT); +#endif + ESP32MCPWMDriverParams *p = (ESP32MCPWMDriverParams*)driver_params; + mcpwm_timer_t* t = (mcpwm_timer_t*) p->timers[0]; + + // check if low side callback is already set + // if it is, return error + if(t->on_full != nullptr){ + SIMPLEFOC_ESP32_CS_DEBUG("Low side callback is already set. Cannot set it again for timer: "+String(t->timer_id)+", group: "+String(t->group->group_id)); + return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; + } + + // set the callback for the low side current sensing + // mcpwm_timer_event_callbacks_t can be used to set the callback + // for three timer events + // - on_full - low-side + // - on_empty - high-side + // - on_sync - sync event (not used with simplefoc) + auto cbs = mcpwm_timer_event_callbacks_t{ + .on_full = [](mcpwm_timer_handle_t tim, const mcpwm_timer_event_data_t* edata, void* user_data){ + ESP32MCPWMCurrentSenseParams *p = (ESP32MCPWMCurrentSenseParams*)user_data; +#ifdef SIMPLEFOC_ESP32_INTERRUPT_DEBUG + gpio_set_level(GPIO_NUM_19,1); //cca 250ns for on+off +#endif + // increment buffer index + p->buffer_index = (p->buffer_index + 1) % p->no_adc_channels; + // sample the phase currents one at a time + // adc read takes around 10us which is very long + // so we are sampling one phase per call + p->adc_buffer[p->buffer_index] = adcRead(p->pins[p->buffer_index]); +#ifdef SIMPLEFOC_ESP32_INTERRUPT_DEBUG + gpio_set_level(GPIO_NUM_19,0); //cca 250ns for on+off +#endif + return true; + }, + }; + SIMPLEFOC_ESP32_CS_DEBUG("Timer "+String(t->timer_id)+" enable interrupt callback."); + // set the timer state to init (so that we can call the `mcpwm_timer_register_event_callbacks` ) + // this is a hack, as this function is not supposed to be called when the timer is running + // the timer does not really go to the init state! + t->fsm = MCPWM_TIMER_FSM_INIT; + // set the callback + CHECK_CS_ERR(mcpwm_timer_register_event_callbacks(t, &cbs, cs_params), "Failed to set low side callback"); + // set the timer state to enabled again + t->fsm = MCPWM_TIMER_FSM_ENABLE; + SIMPLEFOC_ESP32_CS_DEBUG("Timer "+String(t->timer_id)+" enable interrupts."); + CHECK_CS_ERR(esp_intr_enable(t->intr), "Failed to enable low-side interrupts!"); + + return cs_params; +} + + +#endif diff --git a/src/current_sense/hardware_specific/esp32/esp32_mcu.cpp b/src/current_sense/hardware_specific/esp32/esp32_mcu.cpp deleted file mode 100644 index d334008e..00000000 --- a/src/current_sense/hardware_specific/esp32/esp32_mcu.cpp +++ /dev/null @@ -1,165 +0,0 @@ -#include "../../hardware_api.h" -#include "../../../drivers/hardware_api.h" -#include "../../../drivers/hardware_specific/esp32/esp32_driver_mcpwm.h" - -#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(SIMPLEFOC_ESP32_USELEDC) && 0 - -#include "esp32_adc_driver.h" - -#include "driver/mcpwm.h" -#include "soc/mcpwm_reg.h" -#include "soc/mcpwm_struct.h" - -#include -#include - -#define _ADC_VOLTAGE 3.3f -#define _ADC_RESOLUTION 4095.0f - - -typedef struct ESP32MCPWMCurrentSenseParams { - int pins[3]; - float adc_voltage_conv; - mcpwm_unit_t mcpwm_unit; - int buffer_index; -} ESP32MCPWMCurrentSenseParams; - - -/** - * Inline adc reading implementation -*/ -// function reading an ADC value and returning the read voltage -float _readADCVoltageInline(const int pinA, const void* cs_params){ - uint32_t raw_adc = adcRead(pinA); - return raw_adc * ((ESP32MCPWMCurrentSenseParams*)cs_params)->adc_voltage_conv; -} - -// function reading an ADC value and returning the read voltage -void* _configureADCInline(const void* driver_params, const int pinA, const int pinB, const int pinC){ - _UNUSED(driver_params); - - if( _isset(pinA) ) pinMode(pinA, INPUT); - if( _isset(pinB) ) pinMode(pinB, INPUT); - if( _isset(pinC) ) pinMode(pinC, INPUT); - - ESP32MCPWMCurrentSenseParams* params = new ESP32MCPWMCurrentSenseParams { - .pins = { pinA, pinB, pinC }, - .adc_voltage_conv = (_ADC_VOLTAGE)/(_ADC_RESOLUTION) - }; - - return params; -} - - - -/** - * Low side adc reading implementation -*/ - -static void IRAM_ATTR mcpwm0_isr_handler(void*); -static void IRAM_ATTR mcpwm1_isr_handler(void*); -byte currentState = 1; -// two mcpwm units -// - max 2 motors per mcpwm unit (6 adc channels) -int adc_pins[2][6]={0}; -int adc_pin_count[2]={0}; -uint32_t adc_buffer[2][6]={0}; -int adc_read_index[2]={0}; - -// function reading an ADC value and returning the read voltage -float _readADCVoltageLowSide(const int pin, const void* cs_params){ - mcpwm_unit_t unit = ((ESP32MCPWMCurrentSenseParams*)cs_params)->mcpwm_unit; - int buffer_index = ((ESP32MCPWMCurrentSenseParams*)cs_params)->buffer_index; - float adc_voltage_conv = ((ESP32MCPWMCurrentSenseParams*)cs_params)->adc_voltage_conv; - - for(int i=0; i < adc_pin_count[unit]; i++){ - if( pin == ((ESP32MCPWMCurrentSenseParams*)cs_params)->pins[i]) // found in the buffer - return adc_buffer[unit][buffer_index + i] * adc_voltage_conv; - } - // not found - return 0; -} - -// function configuring low-side current sensing -void* _configureADCLowSide(const void* driver_params, const int pinA,const int pinB,const int pinC){ - - mcpwm_unit_t unit = ((ESP32MCPWMDriverParams*)driver_params)->mcpwm_unit; - int index_start = adc_pin_count[unit]; - if( _isset(pinA) ) adc_pins[unit][adc_pin_count[unit]++] = pinA; - if( _isset(pinB) ) adc_pins[unit][adc_pin_count[unit]++] = pinB; - if( _isset(pinC) ) adc_pins[unit][adc_pin_count[unit]++] = pinC; - - if( _isset(pinA) ) pinMode(pinA, INPUT); - if( _isset(pinB) ) pinMode(pinB, INPUT); - if( _isset(pinC) ) pinMode(pinC, INPUT); - - ESP32MCPWMCurrentSenseParams* params = new ESP32MCPWMCurrentSenseParams { - .pins = { pinA, pinB, pinC }, - .adc_voltage_conv = (_ADC_VOLTAGE)/(_ADC_RESOLUTION), - .mcpwm_unit = unit, - .buffer_index = index_start - }; - - return params; -} - - -void _driverSyncLowSide(void* driver_params, void* cs_params){ - - mcpwm_dev_t* mcpwm_dev = ((ESP32MCPWMDriverParams*)driver_params)->mcpwm_dev; - mcpwm_unit_t mcpwm_unit = ((ESP32MCPWMDriverParams*)driver_params)->mcpwm_unit; - - // low-side register enable interrupt - mcpwm_dev->int_ena.timer0_tep_int_ena = true;//A PWM timer 0 TEP event will trigger this interrupt - // high side registers enable interrupt - //mcpwm_dev->int_ena.timer0_tep_int_ena = true;//A PWM timer 0 TEZ event will trigger this interrupt - - // register interrupts (mcpwm number, interrupt handler, handler argument = NULL, interrupt signal/flag, return handler = NULL) - if(mcpwm_unit == MCPWM_UNIT_0) - mcpwm_isr_register(mcpwm_unit, mcpwm0_isr_handler, NULL, ESP_INTR_FLAG_IRAM, NULL); //Set ISR Handler - else - mcpwm_isr_register(mcpwm_unit, mcpwm1_isr_handler, NULL, ESP_INTR_FLAG_IRAM, NULL); //Set ISR Handler -} - -static void IRAM_ATTR mcpwm0_isr_handler(void*) __attribute__ ((unused)); - -// Read currents when interrupt is triggered -static void IRAM_ATTR mcpwm0_isr_handler(void*){ - // // high side - // uint32_t mcpwm_intr_status = MCPWM0.int_st.timer0_tez_int_st; - - // low side - uint32_t mcpwm_intr_status = MCPWM0.int_st.timer0_tep_int_st; - if(mcpwm_intr_status){ - adc_buffer[0][adc_read_index[0]] = adcRead(adc_pins[0][adc_read_index[0]]); - adc_read_index[0]++; - if(adc_read_index[0] == adc_pin_count[0]) adc_read_index[0] = 0; - } - // low side - MCPWM0.int_clr.timer0_tep_int_clr = mcpwm_intr_status; - // high side - // MCPWM0.int_clr.timer0_tez_int_clr = mcpwm_intr_status_0; -} - -static void IRAM_ATTR mcpwm1_isr_handler(void*) __attribute__ ((unused)); - -// Read currents when interrupt is triggered -static void IRAM_ATTR mcpwm1_isr_handler(void*){ - // // high side - // uint32_t mcpwm_intr_status = MCPWM1.int_st.timer0_tez_int_st; - - // low side - uint32_t mcpwm_intr_status = MCPWM1.int_st.timer0_tep_int_st; - if(mcpwm_intr_status){ - adc_buffer[1][adc_read_index[1]] = adcRead(adc_pins[1][adc_read_index[1]]); - adc_read_index[1]++; - if(adc_read_index[1] == adc_pin_count[1]) adc_read_index[1] = 0; - } - // low side - MCPWM1.int_clr.timer0_tep_int_clr = mcpwm_intr_status; - // high side - // MCPWM1.int_clr.timer0_tez_int_clr = mcpwm_intr_status_0; -} - - -#endif diff --git a/src/current_sense/hardware_specific/esp32/esp32s_adc_driver.cpp b/src/current_sense/hardware_specific/esp32/esp32s_adc_driver.cpp index 31cbb027..a212d57b 100644 --- a/src/current_sense/hardware_specific/esp32/esp32s_adc_driver.cpp +++ b/src/current_sense/hardware_specific/esp32/esp32s_adc_driver.cpp @@ -1,6 +1,6 @@ #include "esp32_adc_driver.h" -#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && (defined(CONFIG_IDF_TARGET_ESP32S2) || defined(CONFIG_IDF_TARGET_ESP32S3)) && !defined(SIMPLEFOC_ESP32_USELEDC) && 0 +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && (defined(CONFIG_IDF_TARGET_ESP32S2) || defined(CONFIG_IDF_TARGET_ESP32S3)) && !defined(SIMPLEFOC_ESP32_USELEDC) #include "freertos/FreeRTOS.h" #include "freertos/task.h" diff --git a/src/current_sense/hardware_specific/generic_mcu.cpp b/src/current_sense/hardware_specific/generic_mcu.cpp index dec7205d..58545b17 100644 --- a/src/current_sense/hardware_specific/generic_mcu.cpp +++ b/src/current_sense/hardware_specific/generic_mcu.cpp @@ -34,8 +34,8 @@ __attribute__((weak)) void* _configureADCLowSide(const void* driver_params, con } // sync driver and the adc -__attribute__((weak)) void _driverSyncLowSide(void* driver_params, void* cs_params){ +__attribute__((weak)) void* _driverSyncLowSide(void* driver_params, void* cs_params){ _UNUSED(driver_params); - _UNUSED(cs_params); + return cs_params; } __attribute__((weak)) void _startADC3PinConversionLowSide(){ } diff --git a/src/current_sense/hardware_specific/rp2040/rp2040_mcu.cpp b/src/current_sense/hardware_specific/rp2040/rp2040_mcu.cpp index d84c2fd5..d2ed881b 100644 --- a/src/current_sense/hardware_specific/rp2040/rp2040_mcu.cpp +++ b/src/current_sense/hardware_specific/rp2040/rp2040_mcu.cpp @@ -86,7 +86,7 @@ void* _configureADCInline(const void *driver_params, const int pinA, const int p // }; -// void _driverSyncLowSide(void* driver_params, void* cs_params) { +// void* _driverSyncLowSide(void* driver_params, void* cs_params) { // // nothing to do // }; diff --git a/src/current_sense/hardware_specific/samd/samd21_mcu.cpp b/src/current_sense/hardware_specific/samd/samd21_mcu.cpp index da5ba85b..046f3db4 100644 --- a/src/current_sense/hardware_specific/samd/samd21_mcu.cpp +++ b/src/current_sense/hardware_specific/samd/samd21_mcu.cpp @@ -54,14 +54,15 @@ float _readADCVoltageLowSide(const int pinA, const void* cs_params) /** * function syncing the Driver with the ADC for the LowSide Sensing */ -void _driverSyncLowSide(void* driver_params, void* cs_params) +void* _driverSyncLowSide(void* driver_params, void* cs_params) { _UNUSED(driver_params); - _UNUSED(cs_params); SIMPLEFOC_SAMD_DEBUG_SERIAL.println(F("TODO! _driverSyncLowSide() is not implemented")); instance.startADCScan(); //TODO: hook with PWM interrupts + + return cs_params; } diff --git a/src/current_sense/hardware_specific/stm32/b_g431/b_g431_mcu.cpp b/src/current_sense/hardware_specific/stm32/b_g431/b_g431_mcu.cpp index 8456759c..8cfda6d7 100644 --- a/src/current_sense/hardware_specific/stm32/b_g431/b_g431_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/b_g431/b_g431_mcu.cpp @@ -148,7 +148,7 @@ void DMA1_Channel2_IRQHandler(void) { } } -void _driverSyncLowSide(void* _driver_params, void* _cs_params){ +void* _driverSyncLowSide(void* _driver_params, void* _cs_params){ STM32DriverParams* driver_params = (STM32DriverParams*)_driver_params; Stm32CurrentSenseParams* cs_params = (Stm32CurrentSenseParams*)_cs_params; @@ -169,6 +169,10 @@ void _driverSyncLowSide(void* _driver_params, void* _cs_params){ // restart all the timers of the driver _startTimers(driver_params->timers, 6); + // return the cs parameters + // successfully initialized + // TODO verify if success in future + return _cs_params; } #endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_mcu.cpp b/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_mcu.cpp index 5f090c20..327c5305 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_mcu.cpp @@ -48,7 +48,7 @@ void* _configureADCLowSide(const void* driver_params, const int pinA, const int } -void _driverSyncLowSide(void* _driver_params, void* _cs_params){ +void* _driverSyncLowSide(void* _driver_params, void* _cs_params){ STM32DriverParams* driver_params = (STM32DriverParams*)_driver_params; Stm32CurrentSenseParams* cs_params = (Stm32CurrentSenseParams*)_cs_params; @@ -97,6 +97,11 @@ void _driverSyncLowSide(void* _driver_params, void* _cs_params){ // restart all the timers of the driver _startTimers(driver_params->timers, 6); + + // return the cs parameters + // successfully initialized + // TODO verify if success in future + return _cs_params; } diff --git a/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_mcu.cpp b/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_mcu.cpp index 6388e8e0..21ac7555 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_mcu.cpp @@ -40,7 +40,7 @@ void* _configureADCLowSide(const void* driver_params, const int pinA, const int } -void _driverSyncLowSide(void* _driver_params, void* _cs_params){ +void* _driverSyncLowSide(void* _driver_params, void* _cs_params){ STM32DriverParams* driver_params = (STM32DriverParams*)_driver_params; Stm32CurrentSenseParams* cs_params = (Stm32CurrentSenseParams*)_cs_params; @@ -86,6 +86,11 @@ void _driverSyncLowSide(void* _driver_params, void* _cs_params){ // restart all the timers of the driver _startTimers(driver_params->timers, 6); + + // return the cs parameters + // successfully initialized + // TODO verify if success in future + return _cs_params; } diff --git a/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_mcu.cpp b/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_mcu.cpp index 3040ea46..458ec638 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_mcu.cpp @@ -34,7 +34,7 @@ void* _configureADCLowSide(const void* driver_params, const int pinA, const int } -void _driverSyncLowSide(void* _driver_params, void* _cs_params){ +void* _driverSyncLowSide(void* _driver_params, void* _cs_params){ STM32DriverParams* driver_params = (STM32DriverParams*)_driver_params; Stm32CurrentSenseParams* cs_params = (Stm32CurrentSenseParams*)_cs_params; @@ -69,6 +69,11 @@ void _driverSyncLowSide(void* _driver_params, void* _cs_params){ // restart all the timers of the driver _startTimers(driver_params->timers, 6); + + // return the cs parameters + // successfully initialized + // TODO verify if success in future + return _cs_params; } diff --git a/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_mcu.cpp b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_mcu.cpp index fb32d36b..7f9fd371 100644 --- a/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_mcu.cpp @@ -42,7 +42,7 @@ void* _configureADCLowSide(const void* driver_params, const int pinA, const int } -void _driverSyncLowSide(void* _driver_params, void* _cs_params){ +void* _driverSyncLowSide(void* _driver_params, void* _cs_params){ STM32DriverParams* driver_params = (STM32DriverParams*)_driver_params; Stm32CurrentSenseParams* cs_params = (Stm32CurrentSenseParams*)_cs_params; @@ -123,6 +123,11 @@ void _driverSyncLowSide(void* _driver_params, void* _cs_params){ // restart all the timers of the driver _startTimers(driver_params->timers, 6); + + // return the cs parameters + // successfully initialized + // TODO verify if success in future + return _cs_params; } diff --git a/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_mcu.cpp b/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_mcu.cpp index 1fb0ab6b..e5c9470f 100644 --- a/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_mcu.cpp @@ -41,7 +41,7 @@ void* _configureADCLowSide(const void* driver_params, const int pinA, const int } -void _driverSyncLowSide(void* _driver_params, void* _cs_params){ +void* _driverSyncLowSide(void* _driver_params, void* _cs_params){ STM32DriverParams* driver_params = (STM32DriverParams*)_driver_params; Stm32CurrentSenseParams* cs_params = (Stm32CurrentSenseParams*)_cs_params; @@ -120,6 +120,10 @@ void _driverSyncLowSide(void* _driver_params, void* _cs_params){ // restart all the timers of the driver _startTimers(driver_params->timers, 6); + // return the cs parameters + // successfully initialized + // TODO verify if success in future + return _cs_params; } diff --git a/src/current_sense/hardware_specific/teensy/teensy4_mcu.cpp b/src/current_sense/hardware_specific/teensy/teensy4_mcu.cpp index 70815d0f..3a542b53 100644 --- a/src/current_sense/hardware_specific/teensy/teensy4_mcu.cpp +++ b/src/current_sense/hardware_specific/teensy/teensy4_mcu.cpp @@ -197,7 +197,7 @@ void* _configureADCLowSide(const void* driver_params, const int pinA,const int p } // sync driver and the adc -void _driverSyncLowSide(void* driver_params, void* cs_params){ +void* _driverSyncLowSide(void* driver_params, void* cs_params){ Teensy4DriverParams* par = (Teensy4DriverParams*) ((TeensyDriverParams*)driver_params)->additional_params; IMXRT_FLEXPWM_t* flexpwm = par->flextimers[0]; int submodule = par->submodules[0]; @@ -238,6 +238,11 @@ void _driverSyncLowSide(void* driver_params, void* cs_params){ IOMUXC_SW_PAD_CTL_PAD_GPIO_EMC_06 = IOMUXC_PAD_DSE(7) | IOMUXC_PAD_SPEED(3) | IOMUXC_PAD_SRE ; #endif + + // return the cs parameters + // successfully initialized + // TODO verify if success in future + return cs_params; } diff --git a/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.cpp b/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.cpp index 87f78788..b76d17c5 100644 --- a/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.cpp +++ b/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.cpp @@ -245,7 +245,7 @@ void* _configure6PWMPinsMCPWM(long pwm_frequency, int mcpwm_group, int timer_no, params->mcpwm_period = pwm_periods[mcpwm_group][timer_no]; uint8_t no_operators = 3; // use 3 comparators one per pair of pwms - SIMPLEFOC_ESP32_DEBUG("Configuring " + String(no_operators) + " operators."); + SIMPLEFOC_ESP32_DRV_DEBUG("Configuring " + String(no_operators) + " operators."); mcpwm_operator_config_t operator_config = { .group_id = mcpwm_group }; for (int i = 0; i < no_operators; i++) { CHECK_ERR(mcpwm_new_operator(&operator_config, ¶ms->oper[i]),"Could not create operator "+String(i)); @@ -254,9 +254,9 @@ void* _configure6PWMPinsMCPWM(long pwm_frequency, int mcpwm_group, int timer_no, #if SIMPLEFOC_ESP32_HW_DEADTIME == true // hardware dead-time (hardware 6pwm) - SIMPLEFOC_ESP32_DEBUG("Configuring 6PWM with hardware dead-time"); + SIMPLEFOC_ESP32_DRV_DEBUG("Configuring 6PWM with hardware dead-time"); - SIMPLEFOC_ESP32_DEBUG("Configuring " + String(no_operators) + " comparators."); + SIMPLEFOC_ESP32_DRV_DEBUG("Configuring " + String(no_operators) + " comparators."); // Create and configure comparators mcpwm_comparator_config_t comparator_config = {0}; for (int i = 0; i < no_operators; i++) { @@ -265,7 +265,7 @@ void* _configure6PWMPinsMCPWM(long pwm_frequency, int mcpwm_group, int timer_no, } int no_generators = 6; // one per pwm - SIMPLEFOC_ESP32_DEBUG("Configuring " + String(no_generators) + " generators."); + SIMPLEFOC_ESP32_DRV_DEBUG("Configuring " + String(no_generators) + " generators."); // Create and configure generators mcpwm_generator_config_t generator_config = {}; for (int i = 0; i < no_generators; i++) { @@ -274,12 +274,12 @@ void* _configure6PWMPinsMCPWM(long pwm_frequency, int mcpwm_group, int timer_no, CHECK_ERR(mcpwm_new_generator(params->oper[oper_index], &generator_config, ¶ms->generator[i]), "Could not create generator " + String(i)); } - SIMPLEFOC_ESP32_DEBUG("Configuring Center-Aligned 6 pwm."); + SIMPLEFOC_ESP32_DRV_DEBUG("Configuring Center-Aligned 6 pwm."); for (int i = 0; i < 3; i++) { _configureCenterAlign(params->generator[2*i],params->comparator[i]); } // only available for 6pwm - SIMPLEFOC_ESP32_DEBUG("Configuring dead-time."); + SIMPLEFOC_ESP32_DRV_DEBUG("Configuring dead-time."); uint32_t dead_time = (int)pwm_periods[mcpwm_group][timer_no] * dead_zone; mcpwm_dead_time_config_t dt_config_high; dt_config_high.posedge_delay_ticks = dead_time; @@ -294,9 +294,9 @@ void* _configure6PWMPinsMCPWM(long pwm_frequency, int mcpwm_group, int timer_no, CHECK_ERR(mcpwm_generator_set_dead_time(params->generator[2*i], params->generator[2*i+1], &dt_config_low),"Could not set dead time for generator: " + String(i+1)); } #else // software dead-time (software 6pwm) - SIMPLEFOC_ESP32_DEBUG("Configuring 6PWM with software dead-time"); + SIMPLEFOC_ESP32_DRV_DEBUG("Configuring 6PWM with software dead-time"); int no_pins = 6; - SIMPLEFOC_ESP32_DEBUG("Configuring " + String(no_pins) + " comparators."); + SIMPLEFOC_ESP32_DRV_DEBUG("Configuring " + String(no_pins) + " comparators."); // Create and configure comparators mcpwm_comparator_config_t comparator_config = {0}; for (int i = 0; i < no_pins; i++) { @@ -305,7 +305,7 @@ void* _configure6PWMPinsMCPWM(long pwm_frequency, int mcpwm_group, int timer_no, CHECK_ERR(mcpwm_comparator_set_compare_value(params->comparator[i], (0)), "Could not set duty on comparator: " + String(i)); } - SIMPLEFOC_ESP32_DEBUG("Configuring " + String(no_pins) + " generators."); + SIMPLEFOC_ESP32_DRV_DEBUG("Configuring " + String(no_pins) + " generators."); // Create and configure generators; mcpwm_generator_config_t generator_config = {}; for (int i = 0; i < no_pins; i++) { @@ -314,19 +314,19 @@ void* _configure6PWMPinsMCPWM(long pwm_frequency, int mcpwm_group, int timer_no, CHECK_ERR(mcpwm_new_generator(params->oper[oper_index], &generator_config, ¶ms->generator[i]), "Could not create generator " + String(i)); } - SIMPLEFOC_ESP32_DEBUG("Configuring center-aligned pwm."); + SIMPLEFOC_ESP32_DRV_DEBUG("Configuring center-aligned pwm."); for (int i = 0; i < 3; i++) { _configureCenterAlign(params->generator[2*i],params->comparator[2*i], !SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH); _configureCenterAlign(params->generator[2*i+1],params->comparator[2*i+1], SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH); } #endif - SIMPLEFOC_ESP32_DEBUG("Enabling the timer: "+String(timer_no)); + SIMPLEFOC_ESP32_DRV_DEBUG("Enabling timer: "+String(timer_no)); // Enable and start timer CHECK_ERR(mcpwm_timer_enable(params->timers[0]), "Failed to enable timer!"); CHECK_ERR(mcpwm_timer_start_stop(params->timers[0], MCPWM_TIMER_START_NO_STOP), "Failed to start the timer!"); _delay(1); - SIMPLEFOC_ESP32_DEBUG("MCPWM configured!"); + SIMPLEFOC_ESP32_DRV_DEBUG("MCPWM configured!"); params->dead_zone = dead_zone; // save the configuration variables for later group_pins_used[mcpwm_group] = 6; @@ -374,11 +374,11 @@ void* _configurePinsMCPWM(long pwm_frequency, int mcpwm_group, int timer_no, int }else{ // we will use an already instantiated timer params->timers[0] = timers[mcpwm_group][timer_no]; - SIMPLEFOC_ESP32_DEBUG("Using previously configured timer: " + String(timer_no)); + SIMPLEFOC_ESP32_DRV_DEBUG("Using previously configured timer: " + String(timer_no)); // but we cannot change its configuration without affecting the other drivers // so let's first verify that the configuration is the same if(_calcPWMPeriod(pwm_frequency)/2 != pwm_periods[mcpwm_group][timer_no]){ - SIMPLEFOC_ESP32_DEBUG("ERR: Timer: "+String(timer_no)+" is confgured for freq: "+String(_calcPWMFreq(pwm_periods[mcpwm_group][timer_no]))+", not for freq:" +String(pwm_frequency)); + SIMPLEFOC_ESP32_DRV_DEBUG("ERR: Timer: "+String(timer_no)+" is confgured for freq: "+String(_calcPWMFreq(pwm_periods[mcpwm_group][timer_no]))+", not for freq:" +String(pwm_frequency)); return SIMPLEFOC_DRIVER_INIT_FAILED; } CHECK_ERR(mcpwm_timer_start_stop( params->timers[0], MCPWM_TIMER_STOP_EMPTY), "Failed to stop the timer!"); @@ -387,7 +387,7 @@ void* _configurePinsMCPWM(long pwm_frequency, int mcpwm_group, int timer_no, int } uint8_t no_operators = ceil(no_pins / 2.0); - SIMPLEFOC_ESP32_DEBUG("Configuring " + String(no_operators) + " operators."); + SIMPLEFOC_ESP32_DRV_DEBUG("Configuring " + String(no_operators) + " operators."); mcpwm_operator_config_t operator_config = { .group_id = mcpwm_group }; for (int i = 0; i < no_operators; i++) { if (shared_timer && i == 0) { // first operator already configured @@ -400,7 +400,7 @@ void* _configurePinsMCPWM(long pwm_frequency, int mcpwm_group, int timer_no, int // save the last operator in this group last_operator[mcpwm_group] = params->oper[no_operators - 1]; - SIMPLEFOC_ESP32_DEBUG("Configuring " + String(no_pins) + " comparators."); + SIMPLEFOC_ESP32_DRV_DEBUG("Configuring " + String(no_pins) + " comparators."); // Create and configure comparators mcpwm_comparator_config_t comparator_config = {0}; for (int i = 0; i < no_pins; i++) { @@ -409,7 +409,7 @@ void* _configurePinsMCPWM(long pwm_frequency, int mcpwm_group, int timer_no, int CHECK_ERR(mcpwm_comparator_set_compare_value(params->comparator[i], (0)), "Could not set duty on comparator: " + String(i)); } - SIMPLEFOC_ESP32_DEBUG("Configuring " + String(no_pins) + " generators."); + SIMPLEFOC_ESP32_DRV_DEBUG("Configuring " + String(no_pins) + " generators."); // Create and configure generators; mcpwm_generator_config_t generator_config = {}; for (int i = 0; i < no_pins; i++) { @@ -419,19 +419,19 @@ void* _configurePinsMCPWM(long pwm_frequency, int mcpwm_group, int timer_no, int } - SIMPLEFOC_ESP32_DEBUG("Configuring center-aligned pwm."); + SIMPLEFOC_ESP32_DRV_DEBUG("Configuring center-aligned pwm."); for (int i = 0; i < no_pins; i++) { _configureCenterAlign(params->generator[i],params->comparator[i], !SIMPLEFOC_PWM_ACTIVE_HIGH); } - SIMPLEFOC_ESP32_DEBUG("Enabling the timer: "+String(timer_no)); + SIMPLEFOC_ESP32_DRV_DEBUG("Enabling timer: "+String(timer_no)); // Enable and start timer if not shared if (!shared_timer) CHECK_ERR(mcpwm_timer_enable(params->timers[0]), "Failed to enable timer!"); // start the timer CHECK_ERR(mcpwm_timer_start_stop(params->timers[0], MCPWM_TIMER_START_NO_STOP), "Failed to start the timer!"); _delay(1); - SIMPLEFOC_ESP32_DEBUG("MCPWM configured!"); + SIMPLEFOC_ESP32_DRV_DEBUG("MCPWM configured!"); // save the configuration variables for later params->mcpwm_period = pwm_periods[mcpwm_group][timer_no]; group_pins_used[mcpwm_group] += no_pins; @@ -440,8 +440,17 @@ void* _configurePinsMCPWM(long pwm_frequency, int mcpwm_group, int timer_no, int // function setting the duty cycle to the MCPWM pin void _setDutyCycle(mcpwm_cmpr_handle_t cmpr, uint32_t mcpwm_period, float duty_cycle){ - float duty = constrain(duty_cycle, 0.0, 1.0); - mcpwm_comparator_set_compare_value(cmpr, (uint32_t)(mcpwm_period*duty)); + float duty = constrain(duty_cycle, 0.0, 1.0); + mcpwm_comparator_set_compare_value(cmpr, (uint32_t)(mcpwm_period*duty)); +} + +// function setting the duty cycle to the MCPWM pin +void _forcePhaseState(mcpwm_gen_handle_t generator_high, mcpwm_gen_handle_t generator_low, PhaseState phase_state){ + // phase state can be forced + // https://docs.espressif.com/projects/esp-idf/en/v5.1.4/esp32/api-reference/peripherals/mcpwm.html#generator-force-actions + // TODO verify with ACTIVE_HIGH/ACTIVE_LOW flags + mcpwm_generator_set_force_level(generator_high, (phase_state == PHASE_ON || phase_state == PHASE_HI) ? -1 : 0, true); + mcpwm_generator_set_force_level(generator_low, (phase_state == PHASE_ON || phase_state == PHASE_LO) ? -1 : 1, true); } #endif \ No newline at end of file diff --git a/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.h b/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.h index d0b7933e..446d2f25 100644 --- a/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.h +++ b/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.h @@ -11,7 +11,7 @@ #include "esp_idf_version.h" // version check - this mcpwm driver is specific for ESP-IDF 5.x and arduino-esp32 3.x -#if ESP_IDF_VERSION_MAJOR < 5 +#if ESP_IDF_VERSION < ESP_IDF_VERSION_VAL(5, 0, 0) #error SimpleFOC: ESP-IDF version 4 or lower detected. Please update to ESP-IDF 5.x and Arduino-esp32 3.0 (or higher) #endif @@ -32,14 +32,17 @@ typedef struct ESP32MCPWMDriverParams { } ESP32MCPWMDriverParams; -#define SIMPLEFOC_ESP32_DEBUG(str)\ - SimpleFOCDebug::println( "ESP32-DRV: " + String(str));\ +#define SIMPLEFOC_ESP32_DEBUG(tag, str)\ + SimpleFOCDebug::println( "ESP32-"+String(tag)+ ": "+ String(str)); + +#define SIMPLEFOC_ESP32_DRV_DEBUG(str)\ + SIMPLEFOC_ESP32_DEBUG("DRV", str);\ // macro for checking the error of the mcpwm functions // if the function returns an error the function will return SIMPLEFOC_DRIVER_INIT_FAILED #define CHECK_ERR(func_call, message) \ if ((func_call) != ESP_OK) { \ - SIMPLEFOC_ESP32_DEBUG("ERROR - " + String(message)); \ + SIMPLEFOC_ESP32_DRV_DEBUG("ERROR - " + String(message)); \ return SIMPLEFOC_DRIVER_INIT_FAILED; \ } @@ -143,5 +146,13 @@ void* _configurePinsMCPWM(long pwm_frequency, int mcpwm_group, int timer_no, int */ void _setDutyCycle(mcpwm_cmpr_handle_t cmpr, uint32_t mcpwm_period, float duty_cycle); +/** + * function setting the phase state + * @param generator_high - mcpwm generator handle for the high side + * @param generator_low - mcpwm generator handle for the low side + * @param phase_state - phase state + */ +void _forcePhaseState(mcpwm_gen_handle_t generator_high, mcpwm_gen_handle_t generator_low, PhaseState phase_state); + #endif #endif \ No newline at end of file diff --git a/src/drivers/hardware_specific/esp32/esp32_ledc_mcu.cpp b/src/drivers/hardware_specific/esp32/esp32_ledc_mcu.cpp index 3c413725..dc667ab3 100644 --- a/src/drivers/hardware_specific/esp32/esp32_ledc_mcu.cpp +++ b/src/drivers/hardware_specific/esp32/esp32_ledc_mcu.cpp @@ -11,7 +11,7 @@ // version check - this ledc driver is specific for ESP-IDF 5.x and arduino-esp32 3.x -#if ESP_IDF_VERSION_MAJOR < 5 +#if ESP_IDF_VERSION < ESP_IDF_VERSION_VAL(5, 0, 0) #error SimpleFOC: ESP-IDF version 4 or lower detected. Please update to ESP-IDF 5.x and Arduino-esp32 3.0 (or higher) #endif diff --git a/src/drivers/hardware_specific/esp32/esp32_mcu.cpp b/src/drivers/hardware_specific/esp32/esp32_mcpwm_mcu.cpp similarity index 84% rename from src/drivers/hardware_specific/esp32/esp32_mcu.cpp rename to src/drivers/hardware_specific/esp32/esp32_mcpwm_mcu.cpp index 33f9db20..e2c621c5 100644 --- a/src/drivers/hardware_specific/esp32/esp32_mcu.cpp +++ b/src/drivers/hardware_specific/esp32/esp32_mcpwm_mcu.cpp @@ -15,10 +15,10 @@ void* _configure1PWM(long pwm_frequency, const int pinA) { int group, timer; if(!_findBestGroup(1, pwm_frequency, &group, &timer)) { - SIMPLEFOC_ESP32_DEBUG("Not enough pins available for 1PWM!"); + SIMPLEFOC_ESP32_DRV_DEBUG("Not enough pins available for 1PWM!"); return SIMPLEFOC_DRIVER_INIT_FAILED; } - SIMPLEFOC_ESP32_DEBUG("Configuring 1PWM in group: "+String(group)+" on timer: "+String(timer)); + SIMPLEFOC_ESP32_DRV_DEBUG("Configuring 1PWM in group: "+String(group)+" on timer: "+String(timer)); // configure the timer int pins[1] = {pinA}; return _configurePinsMCPWM(pwm_frequency, group, timer, 1, pins); @@ -35,17 +35,17 @@ void* _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { int group, timer; int ret = _findBestGroup(2, pwm_frequency, &group, &timer); if(!ret) { - SIMPLEFOC_ESP32_DEBUG("Not enough pins available for 2PWM!"); + SIMPLEFOC_ESP32_DRV_DEBUG("Not enough pins available for 2PWM!"); return SIMPLEFOC_DRIVER_INIT_FAILED; } if(ret == 1){ // configure the 2pwm on only one group - SIMPLEFOC_ESP32_DEBUG("Configuring 2PWM in group: "+String(group)+" on timer: "+String(timer)); + SIMPLEFOC_ESP32_DRV_DEBUG("Configuring 2PWM in group: "+String(group)+" on timer: "+String(timer)); // configure the timer int pins[2] = {pinA, pinB}; return _configurePinsMCPWM(pwm_frequency, group, timer, 2, pins); }else{ - SIMPLEFOC_ESP32_DEBUG("Configuring 2PWM as two 1PWM drivers"); + SIMPLEFOC_ESP32_DRV_DEBUG("Configuring 2PWM as two 1PWM drivers"); ESP32MCPWMDriverParams* params[2]; // the code is a bit huge for what it does @@ -53,10 +53,10 @@ void* _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { int pins[2][1] = {{pinA}, {pinB}}; for(int i =0; i<2; i++){ int timer = _findLastTimer(i); //find last created timer in group i - SIMPLEFOC_ESP32_DEBUG("Configuring 1PWM in group: "+String(i)+" on timer: "+String(timer)); + SIMPLEFOC_ESP32_DRV_DEBUG("Configuring 1PWM in group: "+String(i)+" on timer: "+String(timer)); void* p = _configurePinsMCPWM(pwm_frequency, i, timer, 1, pins[i]); if(p == SIMPLEFOC_DRIVER_INIT_FAILED){ - SIMPLEFOC_ESP32_DEBUG("Error configuring 1PWM"); + SIMPLEFOC_ESP32_DRV_DEBUG("Error configuring 1PWM"); return SIMPLEFOC_DRIVER_INIT_FAILED; }else{ params[i] = (ESP32MCPWMDriverParams*)p; @@ -87,10 +87,10 @@ void* _configure3PWM(long pwm_frequency, const int pinA, const int pinB, const i int group, timer; if(!_findBestGroup(3, pwm_frequency, &group, &timer)) { - SIMPLEFOC_ESP32_DEBUG("Not enough pins available for 3PWM!"); + SIMPLEFOC_ESP32_DRV_DEBUG("Not enough pins available for 3PWM!"); return SIMPLEFOC_DRIVER_INIT_FAILED; } - SIMPLEFOC_ESP32_DEBUG("Configuring 3PWM in group: "+String(group)+" on timer: "+String(timer)); + SIMPLEFOC_ESP32_DRV_DEBUG("Configuring 3PWM in group: "+String(group)+" on timer: "+String(timer)); // configure the timer int pins[3] = {pinA, pinB, pinC}; return _configurePinsMCPWM(pwm_frequency, group, timer, 3, pins); @@ -108,16 +108,16 @@ void* _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const in int group, timer; int ret = _findBestGroup(4, pwm_frequency, &group, &timer); if(!ret) { - SIMPLEFOC_ESP32_DEBUG("Not enough pins available for 4PWM!"); + SIMPLEFOC_ESP32_DRV_DEBUG("Not enough pins available for 4PWM!"); return SIMPLEFOC_DRIVER_INIT_FAILED; } if(ret == 1){ - SIMPLEFOC_ESP32_DEBUG("Configuring 4PWM in group: "+String(group)+" on timer: "+String(timer)); + SIMPLEFOC_ESP32_DRV_DEBUG("Configuring 4PWM in group: "+String(group)+" on timer: "+String(timer)); // configure the timer int pins[4] = {pinA, pinB, pinC, pinD}; return _configurePinsMCPWM(pwm_frequency, group, timer, 4, pins); }else{ - SIMPLEFOC_ESP32_DEBUG("Configuring 4PWM as two 2PWM drivers"); + SIMPLEFOC_ESP32_DRV_DEBUG("Configuring 4PWM as two 2PWM drivers"); ESP32MCPWMDriverParams* params[2]; // the code is a bit huge for what it does @@ -125,10 +125,10 @@ void* _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const in int pins[2][2] = {{pinA, pinB},{pinC, pinD}}; for(int i =0; i<2; i++){ int timer = _findNextTimer(i); //find next available timer in group i - SIMPLEFOC_ESP32_DEBUG("Configuring 2PWM in group: "+String(i)+" on timer: "+String(timer)); + SIMPLEFOC_ESP32_DRV_DEBUG("Configuring 2PWM in group: "+String(i)+" on timer: "+String(timer)); void* p = _configurePinsMCPWM(pwm_frequency, i, timer, 2, pins[i]); if(p == SIMPLEFOC_DRIVER_INIT_FAILED){ - SIMPLEFOC_ESP32_DEBUG("Error configuring 2PWM"); + SIMPLEFOC_ESP32_DRV_DEBUG("Error configuring 2PWM"); return SIMPLEFOC_DRIVER_INIT_FAILED; }else{ params[i] = (ESP32MCPWMDriverParams*)p; @@ -157,10 +157,10 @@ void* _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, cons int group, timer; if(!_findBestGroup(6, pwm_frequency, &group, &timer)) { - SIMPLEFOC_ESP32_DEBUG("Not enough pins available for 6PWM!"); + SIMPLEFOC_ESP32_DRV_DEBUG("Not enough pins available for 6PWM!"); return SIMPLEFOC_DRIVER_INIT_FAILED; } - SIMPLEFOC_ESP32_DEBUG("Configuring 6PWM in group: "+String(group)+" on timer: "+String(timer)); + SIMPLEFOC_ESP32_DRV_DEBUG("Configuring 6PWM in group: "+String(group)+" on timer: "+String(timer)); // configure the timer int pins[6] = {pinA_h,pinA_l, pinB_h, pinB_l, pinC_h, pinC_l}; return _configure6PWMPinsMCPWM(pwm_frequency, group, timer, dead_zone, pins); @@ -207,6 +207,11 @@ void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, PhaseState *phase_ _setDutyCycle(((ESP32MCPWMDriverParams*)params)->comparator[0], ((ESP32MCPWMDriverParams*)params)->mcpwm_period, dc_a); _setDutyCycle(((ESP32MCPWMDriverParams*)params)->comparator[1], ((ESP32MCPWMDriverParams*)params)->mcpwm_period, dc_b); _setDutyCycle(((ESP32MCPWMDriverParams*)params)->comparator[2], ((ESP32MCPWMDriverParams*)params)->mcpwm_period, dc_c); + + // set the phase state + _forcePhaseState(((ESP32MCPWMDriverParams*)params)->generator[0], ((ESP32MCPWMDriverParams*)params)->generator[1], phase_state[0]); + _forcePhaseState(((ESP32MCPWMDriverParams*)params)->generator[2], ((ESP32MCPWMDriverParams*)params)->generator[3], phase_state[1]); + _forcePhaseState(((ESP32MCPWMDriverParams*)params)->generator[4], ((ESP32MCPWMDriverParams*)params)->generator[5], phase_state[2]); #else uint32_t period = ((ESP32MCPWMDriverParams*)params)->mcpwm_period; float dead_zone = (float)((ESP32MCPWMDriverParams*)params)->dead_zone /2.0f; diff --git a/src/drivers/hardware_specific/esp32/mcpwm_private.h b/src/drivers/hardware_specific/esp32/mcpwm_private.h new file mode 100644 index 00000000..e133a710 --- /dev/null +++ b/src/drivers/hardware_specific/esp32/mcpwm_private.h @@ -0,0 +1,78 @@ +/* + * This is a private declaration of different MCPWM structures and types. + * It has been copied from: https://github.com/espressif/esp-idf/blob/v5.1.4/components/driver/mcpwm/mcpwm_private.h + * + * extracted by askuric 16.06.2024 + * + * SPDX-FileCopyrightText: 2022-2023 Espressif Systems (Shanghai) CO LTD + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#ifndef MCPWM_PRIVATE_H +#define MCPWM_PRIVATE_H + + +#include "freertos/FreeRTOS.h" +#include "esp_intr_alloc.h" +#include "esp_heap_caps.h" +#include "esp_pm.h" +#include "soc/soc_caps.h" +#include "hal/mcpwm_hal.h" +#include "hal/mcpwm_types.h" +#include "driver/mcpwm_types.h" + +#ifdef __cplusplus +extern "C" { +#endif + +typedef struct mcpwm_group_t mcpwm_group_t; +typedef struct mcpwm_timer_t mcpwm_timer_t; +typedef struct mcpwm_cap_timer_t mcpwm_cap_timer_t; +typedef struct mcpwm_oper_t mcpwm_oper_t; +typedef struct mcpwm_gpio_fault_t mcpwm_gpio_fault_t; +typedef struct mcpwm_gpio_sync_src_t mcpwm_gpio_sync_src_t; +typedef struct mcpwm_timer_sync_src_t mcpwm_timer_sync_src_t; + +struct mcpwm_group_t { + int group_id; // group ID, index from 0 + int intr_priority; // MCPWM interrupt priority + mcpwm_hal_context_t hal; // HAL instance is at group level + portMUX_TYPE spinlock; // group level spinlock + uint32_t prescale; // group prescale + uint32_t resolution_hz; // MCPWM group clock resolution: clock_src_hz / clock_prescale = resolution_hz + esp_pm_lock_handle_t pm_lock; // power management lock + soc_module_clk_t clk_src; // peripheral source clock + mcpwm_cap_timer_t *cap_timer; // mcpwm capture timers + mcpwm_timer_t *timers[SOC_MCPWM_TIMERS_PER_GROUP]; // mcpwm timer array + mcpwm_oper_t *operators[SOC_MCPWM_OPERATORS_PER_GROUP]; // mcpwm operator array + mcpwm_gpio_fault_t *gpio_faults[SOC_MCPWM_GPIO_FAULTS_PER_GROUP]; // mcpwm fault detectors array + mcpwm_gpio_sync_src_t *gpio_sync_srcs[SOC_MCPWM_GPIO_SYNCHROS_PER_GROUP]; // mcpwm gpio sync array +}; + +typedef enum { + MCPWM_TIMER_FSM_INIT, + MCPWM_TIMER_FSM_ENABLE, +} mcpwm_timer_fsm_t; + +struct mcpwm_timer_t { + int timer_id; // timer ID, index from 0 + mcpwm_group_t *group; // which group the timer belongs to + mcpwm_timer_fsm_t fsm; // driver FSM + portMUX_TYPE spinlock; // spin lock + intr_handle_t intr; // interrupt handle + uint32_t resolution_hz; // resolution of the timer + uint32_t peak_ticks; // peak ticks that the timer could reach to + mcpwm_timer_sync_src_t *sync_src; // timer sync_src + mcpwm_timer_count_mode_t count_mode; // count mode + mcpwm_timer_event_cb_t on_full; // callback function when MCPWM timer counts to peak value + mcpwm_timer_event_cb_t on_empty; // callback function when MCPWM timer counts to zero + mcpwm_timer_event_cb_t on_stop; // callback function when MCPWM timer stops + void *user_data; // user data which would be passed to the timer callbacks +}; + +#ifdef __cplusplus +} +#endif + +#endif /* MCPWM_PRIVATE_H */ \ No newline at end of file From 4c25ef039a4811882c3bf20014880f2cce66b986 Mon Sep 17 00:00:00 2001 From: askuric Date: Sun, 16 Jun 2024 22:56:59 +0200 Subject: [PATCH 23/57] forgotten mcpwm ifdef --- src/drivers/hardware_specific/esp32/mcpwm_private.h | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/drivers/hardware_specific/esp32/mcpwm_private.h b/src/drivers/hardware_specific/esp32/mcpwm_private.h index e133a710..dbf48970 100644 --- a/src/drivers/hardware_specific/esp32/mcpwm_private.h +++ b/src/drivers/hardware_specific/esp32/mcpwm_private.h @@ -13,6 +13,8 @@ #define MCPWM_PRIVATE_H +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(SIMPLEFOC_ESP32_USELEDC) + #include "freertos/FreeRTOS.h" #include "esp_intr_alloc.h" #include "esp_heap_caps.h" @@ -75,4 +77,6 @@ struct mcpwm_timer_t { } #endif +#endif + #endif /* MCPWM_PRIVATE_H */ \ No newline at end of file From 67248d8825cdb46755e76d32adcea3c096e80605 Mon Sep 17 00:00:00 2001 From: askuric Date: Sun, 16 Jun 2024 23:02:47 +0200 Subject: [PATCH 24/57] forgotten return statement change in stm32 --- .../hardware_specific/stm32/stm32f4/stm32f4_mcu.cpp | 2 +- .../hardware_specific/stm32/stm32f7/stm32f7_mcu.cpp | 2 +- .../hardware_specific/stm32/stm32g4/stm32g4_mcu.cpp | 2 +- .../hardware_specific/stm32/stm32l4/stm32l4_mcu.cpp | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_mcu.cpp b/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_mcu.cpp index 21ac7555..6b597d4e 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_mcu.cpp @@ -45,7 +45,7 @@ void* _driverSyncLowSide(void* _driver_params, void* _cs_params){ Stm32CurrentSenseParams* cs_params = (Stm32CurrentSenseParams*)_cs_params; // if compatible timer has not been found - if (cs_params->timer_handle == NULL) return; + if (cs_params->timer_handle == NULL) return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; // stop all the timers for the driver _stopTimers(driver_params->timers, 6); diff --git a/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_mcu.cpp b/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_mcu.cpp index 458ec638..f5ca70f3 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_mcu.cpp @@ -39,7 +39,7 @@ void* _driverSyncLowSide(void* _driver_params, void* _cs_params){ Stm32CurrentSenseParams* cs_params = (Stm32CurrentSenseParams*)_cs_params; // if compatible timer has not been found - if (cs_params->timer_handle == NULL) return; + if (cs_params->timer_handle == NULL) return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; // stop all the timers for the driver _stopTimers(driver_params->timers, 6); diff --git a/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_mcu.cpp b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_mcu.cpp index 7f9fd371..9c73f6d7 100644 --- a/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_mcu.cpp @@ -47,7 +47,7 @@ void* _driverSyncLowSide(void* _driver_params, void* _cs_params){ Stm32CurrentSenseParams* cs_params = (Stm32CurrentSenseParams*)_cs_params; // if compatible timer has not been found - if (cs_params->timer_handle == NULL) return; + if (cs_params->timer_handle == NULL) return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; // stop all the timers for the driver _stopTimers(driver_params->timers, 6); diff --git a/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_mcu.cpp b/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_mcu.cpp index e5c9470f..5de6432a 100644 --- a/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_mcu.cpp @@ -46,7 +46,7 @@ void* _driverSyncLowSide(void* _driver_params, void* _cs_params){ Stm32CurrentSenseParams* cs_params = (Stm32CurrentSenseParams*)_cs_params; // if compatible timer has not been found - if (cs_params->timer_handle == NULL) return; + if (cs_params->timer_handle == NULL) return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; // stop all the timers for the driver _stopTimers(driver_params->timers, 6); From 69eb707e152a6d09b0fed78660d5ca272ea64d1d Mon Sep 17 00:00:00 2001 From: askuric Date: Sun, 16 Jun 2024 23:06:14 +0200 Subject: [PATCH 25/57] forgotten bluepill --- .../hardware_specific/stm32/stm32f1/stm32f1_mcu.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_mcu.cpp b/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_mcu.cpp index 327c5305..49f2f3d5 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_mcu.cpp @@ -53,7 +53,7 @@ void* _driverSyncLowSide(void* _driver_params, void* _cs_params){ Stm32CurrentSenseParams* cs_params = (Stm32CurrentSenseParams*)_cs_params; // if compatible timer has not been found - if (cs_params->timer_handle == NULL) return; + if (cs_params->timer_handle == NULL) return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; // stop all the timers for the driver _stopTimers(driver_params->timers, 6); From bbd801758a9cf7508763a2eee2cdbfdb2bf3b48a Mon Sep 17 00:00:00 2001 From: askuric Date: Thu, 20 Jun 2024 08:26:11 +0200 Subject: [PATCH 26/57] added the support for phase state for 6pwm in haredware-6pwm mode + returning error if no low-side --- .../hardware_specific/generic_mcu.cpp | 10 +- .../esp32/esp32_driver_mcpwm.cpp | 223 +++++++++++------- .../esp32/esp32_driver_mcpwm.h | 2 +- 3 files changed, 145 insertions(+), 90 deletions(-) diff --git a/src/current_sense/hardware_specific/generic_mcu.cpp b/src/current_sense/hardware_specific/generic_mcu.cpp index 58545b17..ff8c467c 100644 --- a/src/current_sense/hardware_specific/generic_mcu.cpp +++ b/src/current_sense/hardware_specific/generic_mcu.cpp @@ -1,4 +1,5 @@ #include "../hardware_api.h" +#include "../../communication/SimpleFOCDebug.h" // function reading an ADC value and returning the read voltage __attribute__((weak)) float _readADCVoltageInline(const int pinA, const void* cs_params){ @@ -24,13 +25,15 @@ __attribute__((weak)) void* _configureADCInline(const void* driver_params, cons // function reading an ADC value and returning the read voltage __attribute__((weak)) float _readADCVoltageLowSide(const int pinA, const void* cs_params){ - return _readADCVoltageInline(pinA, cs_params); + SIMPLEFOC_DEBUG("ERR: Low-side cs not supported!"); + return 0.0; } // Configure low side for generic mcu // cannot do much but __attribute__((weak)) void* _configureADCLowSide(const void* driver_params, const int pinA,const int pinB,const int pinC){ - return _configureADCInline(driver_params, pinA, pinB, pinC); + SIMPLEFOC_DEBUG("ERR: Low-side cs not supported!"); + return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; } // sync driver and the adc @@ -38,4 +41,7 @@ __attribute__((weak)) void* _driverSyncLowSide(void* driver_params, void* cs_par _UNUSED(driver_params); return cs_params; } + +// function starting the ADC conversion for the high side current sensing +// only necessary for certain types of MCUs __attribute__((weak)) void _startADC3PinConversionLowSide(){ } diff --git a/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.cpp b/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.cpp index b76d17c5..74ed4c74 100644 --- a/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.cpp +++ b/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.cpp @@ -1,57 +1,101 @@ + /* - * MCPWM in Espressif v5.x has - * - 2x groups (units) - * each one has - * - 3 timers - * - 3 operators (that can be associated with any timer) - * which control a 2xPWM signals - * - 1x comparator + 1x generator per PWM signal for independent mode - * - 1x comparator + 2x generator per pair or PWM signals for complementary mode - * - * Independent mode: - * ------------------ - * 6 PWM independent signals per unit - * unit(0/1) > timer(0-2) > operator(0-2) > comparator(0-1) > generator(0-1) > pwm(A/B) - * - * group | timer | operator | comparator | generator | pwm - * -------------------------------------------------------------------------------- - * 0-1 | 0-2 | 0 | 0 | 0 | A - * 0-1 | 0-2 | 0 | 1(0 complementary) | 1 | B - * 0-1 | 0-2 | 1 | 0 | 0 | A - * 0-1 | 0-2 | 1 | 1(0 complementary) | 1 | B - * 0-1 | 0-2 | 2 | 0 | 0 | A - * 0-1 | 0-2 | 2 | 1(0 complementary) | 1 | B - * - * Complementary mode - * ------------------ - * - : 3 pairs of complementary PWM signals per unit - * unit(0/1) > timer(0) > operator(0-2) > comparator(0) > generator(0-1) > pwm(A-B pair) - * - * group | timer | operator | comparator | generator | pwm - * ------------------------------------------------------------------------ - * 0-1 | 0 | 0 | 0 | 0 | A - * 0-1 | 0 | 0 | 0 | 1 | B - * 0-1 | 0 | 1 | 0 | 0 | A - * 0-1 | 0 | 1 | 0 | 1 | B - * 0-1 | 0 | 2 | 0 | 0 | A - * 0-1 | 0 | 2 | 0 | 1 | B - * - * More info - * ---------- - * - timers can be associated with any operator, and multiple operators can be associated with the same timer - * - comparators can be associated with any operator - * - two comparators per operator for independent mode - * - one comparator per operator for complementary mode - * - generators can be associated with any comparator - * - one generator per PWM signal for independent mode - * - two generators per pair of PWM signals for complementary mode - * - dead-time can be set for each generator pair in complementary mode - * - * Docs - * ------- - * More info here: https://www.espressif.com/sites/default/files/documentation/esp32_technical_reference_manual_en.pdf#mcpwm - * and here: // https://docs.espressif.com/projects/esp-idf/en/v5.1.4/esp32/migration-guides/release-5.x/5.0/peripherals.html +* MCPWM in espressif v5.x has +* - 2x groups (units) +* each one has +* - 3 timers +* - 3 operators (that can be associated with any timer) +* which control a 2xPWM signals +* - 1x comparator + 1x generator per PWM signal + + +* Independent mode: +* ------------------ +* 6 PWM independent signals per unit +* unit(0/1) > timer(0-2) > operator(0-2) > comparator(0-1) > generator(0-1) > pwm(A/B) +* +* -------------------------------------- Table View ----------------------------- +* +* group | timer | operator | comparator | generator | pwm +* -------------------------------------------------------------------------------- +* 0-1 | 0-2 | 0 | 0 | 0 | A +* 0-1 | 0-2 | 0 | 1 | 1 | B +* 0-1 | 0-2 | 1 | 0 | 0 | A +* 0-1 | 0-2 | 1 | 1 | 1 | B +* 0-1 | 0-2 | 2 | 0 | 0 | A +* 0-1 | 0-2 | 2 | 1 | 1 | B +* +* ------------------------------------- Example 3PWM ------------------------------ +* ┌─ comparator 0 - generator 0 -> pwm A +* ┌─ operator 0 -| +* | └─ comparator 1 - generator 1 -> pmw B +* unit - timer 0-2 -| +* 0-1 └─ operator 1 - comparator 0 - generator 0 - pwm C +* +* ------------------------------------- Example 2PWM ------------------------------ +* ┌─ comparator 0 - generator 0 -> pwm A +* unit - timer 0-2 - operator 0 -| +* 0-1 └─ comparator 1 - generator 1 -> pmw B +* +* -------------------------------------- Example 4PWM ----------------------------- +* ┌─ comparator 0 - generator 0 -> pwm A +* ┌─ operator 0 -| +* | └─ comparator 1 - generator 1 -> pmw B +* unit - timer 0-2 -| +* 0-1 | ┌─ comparator 0 - generator 0 -> pwm C +* └─ operator 1 -| +* └─ comparator 0 - generator 0 -> pwm D + + +* Complementary mode +* ------------------ +* - : 3 pairs of complementary PWM signals per unit +* unit(0/1) > timer(0) > operator(0-2) > comparator(0-1) > generator(0-1) > pwm(high/low pair) +* +* -------------------------------------- Table View ----------------------------- +* +* group | timer | operator | comparator | generator | pwm +* ------------------------------------------------------------------------ +* 0-1 | 0 | 0 | 0 | 0 | A +* 0-1 | 0 | 0 | 1 | 1 | B +* 0-1 | 0 | 1 | 0 | 0 | A +* 0-1 | 0 | 1 | 1 | 1 | B +* 0-1 | 0 | 2 | 0 | 0 | A +* 0-1 | 0 | 2 | 1 | 1 | B +* +* -------------------------------------- Example 6PWM ----------------------------- +* +* ┌─ comparator 0 - generator 0 -> pwm A_h +* ┌─ operator 0 -| +* | └─ comparator 1 - generator 1 -> pmw A_l +* | +* unit | ┌─ comparator 0 - generator 0 -> pwm B_h +* (group) - timer 0 -|- operator 1 -| +* 0-1 | └─ comparator 1 - generator 1 -> pmw B_l +* | +* | ┌─ comparator 0 - generator 0 -> pwm C_h +* └─ operator 2 -| +* └─ comparator 1 - generator 1 -> pmw C_l +* + + +* More info +* ---------- +* - timers can be associated with any operator, and multiple operators can be associated with the same timer +* - comparators can be associated with any operator +* - two comparators per operator for independent mode +* - one comparator per operator for complementary mode +* - generators can be associated with any comparator +* - one generator per PWM signal for independent mode +* - two generators per pair of PWM signals for complementary mode (not used in simplefoc) +* - dead-time can be set for each generator pair in complementary mode +* +* Docs +* ------- +* More info here: https:*www.espressif.com/sites/default/files/documentation/esp32_technical_reference_manual_en.pdf#mcpwm +* and here: // https://docs.espressif.com/projects/esp-idf/en/v5.1.4/esp32/migration-guides/release-5.x/5.0/peripherals.html */ + #include "../../hardware_api.h" #if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(SIMPLEFOC_ESP32_USELEDC) @@ -195,14 +239,14 @@ int _findBestGroup(int no_pins, long pwm_freq, int* group, int* timer){ // configuring center aligned pwm // More info here: https://docs.espressif.com/projects/esp-idf/en/v5.1.4/esp32/api-reference/peripherals/mcpwm.html#symmetric-dual-edge-active-low -void _configureCenterAlign(mcpwm_gen_handle_t gena, mcpwm_cmpr_handle_t cmpa, bool inverted = false){ +int _configureCenterAlign(mcpwm_gen_handle_t gena, mcpwm_cmpr_handle_t cmpa, bool inverted = false){ if(inverted) - mcpwm_generator_set_actions_on_compare_event(gena, + return mcpwm_generator_set_actions_on_compare_event(gena, MCPWM_GEN_COMPARE_EVENT_ACTION(MCPWM_TIMER_DIRECTION_UP, cmpa, MCPWM_GEN_ACTION_HIGH), MCPWM_GEN_COMPARE_EVENT_ACTION(MCPWM_TIMER_DIRECTION_DOWN, cmpa, MCPWM_GEN_ACTION_LOW), MCPWM_GEN_COMPARE_EVENT_ACTION_END()); else - mcpwm_generator_set_actions_on_compare_event(gena, + return mcpwm_generator_set_actions_on_compare_event(gena, MCPWM_GEN_COMPARE_EVENT_ACTION(MCPWM_TIMER_DIRECTION_UP, cmpa, MCPWM_GEN_ACTION_LOW), MCPWM_GEN_COMPARE_EVENT_ACTION(MCPWM_TIMER_DIRECTION_DOWN, cmpa, MCPWM_GEN_ACTION_HIGH), MCPWM_GEN_COMPARE_EVENT_ACTION_END()); @@ -247,6 +291,9 @@ void* _configure6PWMPinsMCPWM(long pwm_frequency, int mcpwm_group, int timer_no, uint8_t no_operators = 3; // use 3 comparators one per pair of pwms SIMPLEFOC_ESP32_DRV_DEBUG("Configuring " + String(no_operators) + " operators."); mcpwm_operator_config_t operator_config = { .group_id = mcpwm_group }; + operator_config.intr_priority = 0; + operator_config.flags.update_gen_action_on_tep = true; + operator_config.flags.update_gen_action_on_tez = true; for (int i = 0; i < no_operators; i++) { CHECK_ERR(mcpwm_new_operator(&operator_config, ¶ms->oper[i]),"Could not create operator "+String(i)); CHECK_ERR(mcpwm_operator_connect_timer(params->oper[i], params->timers[0]),"Could not connect timer to operator: " + String(i)); @@ -263,6 +310,21 @@ void* _configure6PWMPinsMCPWM(long pwm_frequency, int mcpwm_group, int timer_no, CHECK_ERR(mcpwm_new_comparator(params->oper[i], &comparator_config, ¶ms->comparator[i]),"Could not create comparator: " + String(i)); CHECK_ERR(mcpwm_comparator_set_compare_value(params->comparator[i], (0)), "Could not set duty on comparator: " + String(i)); } + +#else // software dead-time (software 6pwm) +// software dead-time (software 6pwm) + SIMPLEFOC_ESP32_DRV_DEBUG("Configuring 6PWM with software dead-time"); + + int no_pins = 6; + SIMPLEFOC_ESP32_DRV_DEBUG("Configuring " + String(no_pins) + " comparators."); + // Create and configure comparators + mcpwm_comparator_config_t comparator_config = {0}; + for (int i = 0; i < no_pins; i++) { + int oper_index = (int)floor(i / 2); + CHECK_ERR(mcpwm_new_comparator(params->oper[oper_index], &comparator_config, ¶ms->comparator[i]),"Could not create comparator: " + String(i)); + CHECK_ERR(mcpwm_comparator_set_compare_value(params->comparator[i], (0)), "Could not set duty on comparator: " + String(i)); + } +#endif int no_generators = 6; // one per pwm SIMPLEFOC_ESP32_DRV_DEBUG("Configuring " + String(no_generators) + " generators."); @@ -273,10 +335,14 @@ void* _configure6PWMPinsMCPWM(long pwm_frequency, int mcpwm_group, int timer_no, int oper_index = (int)floor(i / 2); CHECK_ERR(mcpwm_new_generator(params->oper[oper_index], &generator_config, ¶ms->generator[i]), "Could not create generator " + String(i)); } - + SIMPLEFOC_ESP32_DRV_DEBUG("Configuring Center-Aligned 6 pwm."); - for (int i = 0; i < 3; i++) { - _configureCenterAlign(params->generator[2*i],params->comparator[i]); + +#if SIMPLEFOC_ESP32_HW_DEADTIME == true // hardware dead-time (hardware 6pwm) + for (int i = 0; i < no_operators; i++) { + CHECK_ERR(_configureCenterAlign(params->generator[2*i],params->comparator[i]), "Failed to configure high-side center align pwm: " + String(2*i)); + CHECK_ERR(_configureCenterAlign(params->generator[2*i+1],params->comparator[i]), "Failed to configure low-side center align pwm: " + String(2*i+1)); + } // only available for 6pwm SIMPLEFOC_ESP32_DRV_DEBUG("Configuring dead-time."); @@ -289,37 +355,17 @@ void* _configure6PWMPinsMCPWM(long pwm_frequency, int mcpwm_group, int timer_no, dt_config_low.posedge_delay_ticks = 0; dt_config_low.negedge_delay_ticks = dead_time; dt_config_low.flags.invert_output = SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH; - for (int i = 0; i < 3; i++) { + for (int i = 0; i < no_operators; i++) { CHECK_ERR(mcpwm_generator_set_dead_time(params->generator[2*i], params->generator[2*i], &dt_config_high),"Could not set dead time for generator: " + String(i)); - CHECK_ERR(mcpwm_generator_set_dead_time(params->generator[2*i], params->generator[2*i+1], &dt_config_low),"Could not set dead time for generator: " + String(i+1)); + CHECK_ERR(mcpwm_generator_set_dead_time(params->generator[2*i+1], params->generator[2*i+1], &dt_config_low),"Could not set dead time for generator: " + String(i+1)); } #else // software dead-time (software 6pwm) - SIMPLEFOC_ESP32_DRV_DEBUG("Configuring 6PWM with software dead-time"); - int no_pins = 6; - SIMPLEFOC_ESP32_DRV_DEBUG("Configuring " + String(no_pins) + " comparators."); - // Create and configure comparators - mcpwm_comparator_config_t comparator_config = {0}; - for (int i = 0; i < no_pins; i++) { - int oper_index = (int)floor(i / 2); - CHECK_ERR(mcpwm_new_comparator(params->oper[oper_index], &comparator_config, ¶ms->comparator[i]),"Could not create comparator: " + String(i)); - CHECK_ERR(mcpwm_comparator_set_compare_value(params->comparator[i], (0)), "Could not set duty on comparator: " + String(i)); - } - - SIMPLEFOC_ESP32_DRV_DEBUG("Configuring " + String(no_pins) + " generators."); - // Create and configure generators; - mcpwm_generator_config_t generator_config = {}; - for (int i = 0; i < no_pins; i++) { - generator_config.gen_gpio_num = pins[i]; - int oper_index = (int)floor(i / 2); - CHECK_ERR(mcpwm_new_generator(params->oper[oper_index], &generator_config, ¶ms->generator[i]), "Could not create generator " + String(i)); - } - - SIMPLEFOC_ESP32_DRV_DEBUG("Configuring center-aligned pwm."); for (int i = 0; i < 3; i++) { - _configureCenterAlign(params->generator[2*i],params->comparator[2*i], !SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH); - _configureCenterAlign(params->generator[2*i+1],params->comparator[2*i+1], SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH); + CHECK_ERR(_configureCenterAlign(params->generator[2*i],params->comparator[2*i], !SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH), "Failed to configure high-side center align pwm: " + String(2*i)); + CHECK_ERR(_configureCenterAlign(params->generator[2*i+1],params->comparator[2*i+1], SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH) , "Failed to configure low-side center align pwm: " + String(2*i+1)); } #endif + SIMPLEFOC_ESP32_DRV_DEBUG("Enabling timer: "+String(timer_no)); // Enable and start timer CHECK_ERR(mcpwm_timer_enable(params->timers[0]), "Failed to enable timer!"); @@ -389,6 +435,9 @@ void* _configurePinsMCPWM(long pwm_frequency, int mcpwm_group, int timer_no, int uint8_t no_operators = ceil(no_pins / 2.0); SIMPLEFOC_ESP32_DRV_DEBUG("Configuring " + String(no_operators) + " operators."); mcpwm_operator_config_t operator_config = { .group_id = mcpwm_group }; + operator_config.intr_priority = 0; + operator_config.flags.update_gen_action_on_tep = true; + operator_config.flags.update_gen_action_on_tez = true; for (int i = 0; i < no_operators; i++) { if (shared_timer && i == 0) { // first operator already configured params->oper[0] = last_operator[mcpwm_group]; @@ -421,7 +470,7 @@ void* _configurePinsMCPWM(long pwm_frequency, int mcpwm_group, int timer_no, int SIMPLEFOC_ESP32_DRV_DEBUG("Configuring center-aligned pwm."); for (int i = 0; i < no_pins; i++) { - _configureCenterAlign(params->generator[i],params->comparator[i], !SIMPLEFOC_PWM_ACTIVE_HIGH); + CHECK_ERR(_configureCenterAlign(params->generator[i],params->comparator[i], !SIMPLEFOC_PWM_ACTIVE_HIGH), "Failed to configure center align pwm: " + String(i)); } SIMPLEFOC_ESP32_DRV_DEBUG("Enabling timer: "+String(timer_no)); @@ -446,9 +495,9 @@ void _setDutyCycle(mcpwm_cmpr_handle_t cmpr, uint32_t mcpwm_period, float duty_c // function setting the duty cycle to the MCPWM pin void _forcePhaseState(mcpwm_gen_handle_t generator_high, mcpwm_gen_handle_t generator_low, PhaseState phase_state){ - // phase state can be forced - // https://docs.espressif.com/projects/esp-idf/en/v5.1.4/esp32/api-reference/peripherals/mcpwm.html#generator-force-actions - // TODO verify with ACTIVE_HIGH/ACTIVE_LOW flags + // phase state is forced in hardware pwm mode + // esp-idf docs: https://docs.espressif.com/projects/esp-idf/en/v5.1.4/esp32/api-reference/peripherals/mcpwm.html#generator-force-actions + // github issue: https://github.com/espressif/esp-idf/issues/12237 mcpwm_generator_set_force_level(generator_high, (phase_state == PHASE_ON || phase_state == PHASE_HI) ? -1 : 0, true); mcpwm_generator_set_force_level(generator_low, (phase_state == PHASE_ON || phase_state == PHASE_LO) ? -1 : 1, true); } diff --git a/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.h b/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.h index 446d2f25..5726e906 100644 --- a/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.h +++ b/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.h @@ -103,7 +103,7 @@ int _findBestGroup(int no_pins, long pwm_freq, int* group, int* timer); * @param cmpa - mcpwm comparator handle * @param inverted - true if the signal is inverted, false otherwise */ -void _configureCenterAlign(mcpwm_gen_handle_t gena, mcpwm_cmpr_handle_t cmpa, bool inverted); +int _configureCenterAlign(mcpwm_gen_handle_t gena, mcpwm_cmpr_handle_t cmpa, bool inverted); /** * function calculating the pwm period From 63fda20fb51a056507a6907a212210771f40e080 Mon Sep 17 00:00:00 2001 From: askuric Date: Thu, 20 Jun 2024 08:26:48 +0200 Subject: [PATCH 27/57] added workflow or s2 --- .github/workflows/esp32.yml | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/.github/workflows/esp32.yml b/.github/workflows/esp32.yml index e0899bf1..db551b41 100644 --- a/.github/workflows/esp32.yml +++ b/.github/workflows/esp32.yml @@ -19,6 +19,7 @@ jobs: - esp32:esp32:esp32doit-devkit-v1 # esp32 - esp32:esp32:esp32s2 # esp32s2 - esp32:esp32:esp32s3 # esp32s3 + - esp32:esp32:esp32c3 # esp32c3 include: @@ -30,6 +31,10 @@ jobs: platform-url: https://espressif.github.io/arduino-esp32/package_esp32_index.json sketch-names: esp32_position_control.ino, esp32_i2c_dual_bus_example.ino + - arduino-boards-fqbn: esp32:esp32:esp32c3 # esp32c3 + platform-url: https://espressif.github.io/arduino-esp32/package_esp32_index.json + sketch-names: esp32_position_control.ino, esp32_i2c_dual_bus_example.ino, stepper_driver_2pwm_standalone.ino, stepper_driver_4pwm_standalone.ino + - arduino-boards-fqbn: esp32:esp32:esp32doit-devkit-v1 # esp32 platform-url: https://espressif.github.io/arduino-esp32/package_esp32_index.json sketch-names: esp32_position_control.ino, esp32_i2c_dual_bus_example.ino, esp32_current_control_low_side.ino, esp32_spi_alt_example.ino From 3c3e9c8cf67ecbdd05dcc3653ead4c651e1907b2 Mon Sep 17 00:00:00 2001 From: askuric Date: Thu, 20 Jun 2024 09:11:22 +0200 Subject: [PATCH 28/57] added the debugging in examples --- .../B_G431B_ESC1/B_G431B_ESC1.ino | 9 ++++++--- .../bluepill_position_control.ino | 8 ++++++-- .../bluepill_position_control.ino | 8 ++++++-- .../full_control_serial/full_control_serial.ino | 9 ++++++--- .../full_control_serial/full_control_serial.ino | 9 ++++++--- .../esp32_current_control_low_side.ino | 10 ++++++---- .../stm32_current_control_low_side.ino | 10 ++++++---- .../teensy4_current_control_low_side.ino | 10 ++++++---- .../esp32_position_control/esp32_position_control.ino | 9 ++++++--- .../esp32_position_control/esp32_position_control.ino | 8 ++++++-- .../position_control/position_control.ino | 6 +++--- .../odrive_example_encoder/odrive_example_encoder.ino | 8 ++++++-- .../odrive_example_spi/odrive_example_spi.ino | 8 ++++++-- .../nano33IoT_velocity_control.ino | 5 +++++ .../single_full_control_example.ino | 9 ++++++--- .../SimpleFOCMini/angle_control/angle_control.ino | 9 ++++++--- .../double_full_control_example.ino | 9 ++++++--- .../single_full_control_example.ino | 9 ++++++--- .../double_full_control_example.ino | 9 ++++++--- .../single_full_control_example.ino | 9 ++++++--- .../single_full_control_example.ino | 9 ++++++--- .../smartstepper_control/smartstepper_control.ino | 8 ++++++-- .../open_loop_velocity_6pwm.ino | 7 ++++++- .../open_loop_velocity_6pwm.ino | 7 ++++++- .../open_loop_position_example.ino | 7 ++++++- .../open_loop_velocity_example.ino | 7 ++++++- .../encoder/angle_control/angle_control.ino | 9 ++++++--- .../magnetic_sensor/angle_control/angle_control.ino | 10 +++++++--- .../encoder/current_control/current_control.ino | 8 ++++++-- .../encoder/voltage_control/voltage_control.ino | 8 ++++++-- .../hall_sensor/voltage_control/voltage_control.ino | 8 ++++++-- .../voltage_control/voltage_control.ino | 8 ++++++-- .../hall_sensor/velocity_control/velocity_control.ino | 8 ++++++-- .../velocity_control/velocity_control.ino | 8 ++++++-- .../full_control_serial/full_control_serial.ino | 9 ++++++--- .../full_control_serial/full_control_serial.ino | 9 ++++++--- .../full_control_serial/full_control_serial.ino | 9 ++++++--- .../osc_esp32_3pwm/osc_esp32_3pwm.ino | 6 +++++- .../osc_esp32_fullcontrol/osc_esp32_fullcontrol.ino | 5 +++++ .../alignment_and_cogging_test.ino | 5 ++++- .../encoder/find_kv_rating/find_kv_rating.ino | 8 ++++++-- .../hall_sensor/find_kv_rating/find_kv_rating.ino | 10 +++++++--- .../magnetic_sensor/find_kv_rating/find_kv_rating.ino | 8 ++++++-- .../find_pole_pairs_number/find_pole_pairs_number.ino | 8 ++++++-- .../find_pole_pairs_number/find_pole_pairs_number.ino | 9 ++++++--- .../find_sensor_offset_and_direction.ino | 7 ++++++- .../commander_tune_custom_loop.ino | 7 ++++++- .../step_dir_motor_example/step_dir_motor_example.ino | 10 +++++++--- .../generic_current_sense/generic_current_sense.ino | 9 ++++++++- .../inline_current_sense_test.ino | 8 +++++++- .../bldc_driver_3pwm_standalone.ino | 6 ++++++ .../bldc_driver_6pwm_standalone.ino | 6 ++++++ .../stepper_driver_2pwm_standalone.ino | 6 ++++++ .../stepper_driver_4pwm_standalone.ino | 6 ++++++ 54 files changed, 322 insertions(+), 112 deletions(-) diff --git a/examples/hardware_specific_examples/B_G431B_ESC1/B_G431B_ESC1.ino b/examples/hardware_specific_examples/B_G431B_ESC1/B_G431B_ESC1.ino index 8d751742..f1eeefd2 100644 --- a/examples/hardware_specific_examples/B_G431B_ESC1/B_G431B_ESC1.ino +++ b/examples/hardware_specific_examples/B_G431B_ESC1/B_G431B_ESC1.ino @@ -26,6 +26,12 @@ void doTarget(char* cmd) { command.motion(&motor, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialize encoder sensor hardware encoder.init(); encoder.enableInterrupts(doA, doB); @@ -76,9 +82,6 @@ void setup() { // maximal velocity of the position control motor.velocity_limit = 4; - - // use monitoring with serial - Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); diff --git a/examples/hardware_specific_examples/Bluepill_examples/encoder/bluepill_position_control/bluepill_position_control.ino b/examples/hardware_specific_examples/Bluepill_examples/encoder/bluepill_position_control/bluepill_position_control.ino index be853f0e..b29e45d8 100644 --- a/examples/hardware_specific_examples/Bluepill_examples/encoder/bluepill_position_control/bluepill_position_control.ino +++ b/examples/hardware_specific_examples/Bluepill_examples/encoder/bluepill_position_control/bluepill_position_control.ino @@ -33,6 +33,12 @@ void doTarget(char* cmd) { command.scalar(&target_angle, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialize encoder sensor hardware encoder.init(); encoder.enableInterrupts(doA, doB, doI); @@ -75,8 +81,6 @@ void setup() { motor.velocity_limit = 4; - // use monitoring with serial - Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); diff --git a/examples/hardware_specific_examples/Bluepill_examples/magnetic_sensor/bluepill_position_control/bluepill_position_control.ino b/examples/hardware_specific_examples/Bluepill_examples/magnetic_sensor/bluepill_position_control/bluepill_position_control.ino index e27f1e7a..caef7ffe 100644 --- a/examples/hardware_specific_examples/Bluepill_examples/magnetic_sensor/bluepill_position_control/bluepill_position_control.ino +++ b/examples/hardware_specific_examples/Bluepill_examples/magnetic_sensor/bluepill_position_control/bluepill_position_control.ino @@ -36,6 +36,12 @@ void doTarget(char* cmd) { command.scalar(&target_angle, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialise magnetic sensor hardware sensor.init(); // link the motor to the sensor @@ -72,8 +78,6 @@ void setup() { // maximal velocity of the position control motor.velocity_limit = 40; - // use monitoring with serial - Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); diff --git a/examples/hardware_specific_examples/DRV8302_driver/3pwm_example/encoder/full_control_serial/full_control_serial.ino b/examples/hardware_specific_examples/DRV8302_driver/3pwm_example/encoder/full_control_serial/full_control_serial.ino index 842f383a..3d90db16 100644 --- a/examples/hardware_specific_examples/DRV8302_driver/3pwm_example/encoder/full_control_serial/full_control_serial.ino +++ b/examples/hardware_specific_examples/DRV8302_driver/3pwm_example/encoder/full_control_serial/full_control_serial.ino @@ -43,6 +43,12 @@ void onMotor(char* cmd){ command.motor(&motor, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialize encoder sensor hardware encoder.init(); encoder.enableInterrupts(doA, doB); @@ -90,9 +96,6 @@ void setup() { // angle loop velocity limit motor.velocity_limit = 50; - // use monitoring with serial for motor init - // monitoring port - Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); diff --git a/examples/hardware_specific_examples/DRV8302_driver/6pwm_example/encoder/full_control_serial/full_control_serial.ino b/examples/hardware_specific_examples/DRV8302_driver/6pwm_example/encoder/full_control_serial/full_control_serial.ino index 2b4cf6f1..af56e067 100644 --- a/examples/hardware_specific_examples/DRV8302_driver/6pwm_example/encoder/full_control_serial/full_control_serial.ino +++ b/examples/hardware_specific_examples/DRV8302_driver/6pwm_example/encoder/full_control_serial/full_control_serial.ino @@ -46,6 +46,12 @@ void onMotor(char* cmd){ command.motor(&motor, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialize encoder sensor hardware encoder.init(); encoder.enableInterrupts(doA, doB); @@ -91,9 +97,6 @@ void setup() { // angle loop velocity limit motor.velocity_limit = 50; - // use monitoring with serial for motor init - // monitoring port - Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); diff --git a/examples/hardware_specific_examples/DRV8302_driver/esp32_current_control_low_side/esp32_current_control_low_side.ino b/examples/hardware_specific_examples/DRV8302_driver/esp32_current_control_low_side/esp32_current_control_low_side.ino index 84033b42..7d7fe14f 100644 --- a/examples/hardware_specific_examples/DRV8302_driver/esp32_current_control_low_side/esp32_current_control_low_side.ino +++ b/examples/hardware_specific_examples/DRV8302_driver/esp32_current_control_low_side/esp32_current_control_low_side.ino @@ -51,6 +51,12 @@ void onMotor(char* cmd){ command.motor(&motor, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialize encoder sensor hardware encoder.init(); encoder.enableInterrupts(doA, doB); @@ -115,10 +121,6 @@ void setup() { motor.voltage_limit = 12.0; // 12 Volt limit motor.current_limit = 2.0; // 2 Amp current limit - - // use monitoring with serial for motor init - // monitoring port - Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); motor.monitor_variables = _MON_CURR_Q | _MON_CURR_D; // monitor the two currents d and q diff --git a/examples/hardware_specific_examples/DRV8302_driver/stm32_current_control_low_side/stm32_current_control_low_side.ino b/examples/hardware_specific_examples/DRV8302_driver/stm32_current_control_low_side/stm32_current_control_low_side.ino index 3e3f04ab..e1b4c392 100644 --- a/examples/hardware_specific_examples/DRV8302_driver/stm32_current_control_low_side/stm32_current_control_low_side.ino +++ b/examples/hardware_specific_examples/DRV8302_driver/stm32_current_control_low_side/stm32_current_control_low_side.ino @@ -51,6 +51,12 @@ void onMotor(char* cmd){ command.motor(&motor, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialize encoder sensor hardware encoder.init(); encoder.enableInterrupts(doA, doB); @@ -115,10 +121,6 @@ void setup() { motor.voltage_limit = 12.0; // 12 Volt limit motor.current_limit = 2.0; // 2 Amp current limit - - // use monitoring with serial for motor init - // monitoring port - Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); motor.monitor_variables = _MON_CURR_Q | _MON_CURR_D; // monitor the two currents d and q diff --git a/examples/hardware_specific_examples/DRV8302_driver/teensy4_current_control_low_side/teensy4_current_control_low_side.ino b/examples/hardware_specific_examples/DRV8302_driver/teensy4_current_control_low_side/teensy4_current_control_low_side.ino index 841134d8..c9b4516f 100644 --- a/examples/hardware_specific_examples/DRV8302_driver/teensy4_current_control_low_side/teensy4_current_control_low_side.ino +++ b/examples/hardware_specific_examples/DRV8302_driver/teensy4_current_control_low_side/teensy4_current_control_low_side.ino @@ -54,6 +54,12 @@ void onMotor(char* cmd){ command.motor(&motor, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialize encoder sensor hardware encoder.init(); encoder.enableInterrupts(doA, doB); @@ -116,10 +122,6 @@ void setup() { motor.voltage_limit = 12.0; // 12 Volt limit motor.current_limit = 2.0; // 2 Amp current limit - - // use monitoring with serial for motor init - // monitoring port - Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); motor.monitor_variables = _MON_CURR_Q | _MON_CURR_D; // monitor the two currents d and q diff --git a/examples/hardware_specific_examples/ESP32/encoder/esp32_position_control/esp32_position_control.ino b/examples/hardware_specific_examples/ESP32/encoder/esp32_position_control/esp32_position_control.ino index f6dac940..7f6b33ce 100644 --- a/examples/hardware_specific_examples/ESP32/encoder/esp32_position_control/esp32_position_control.ino +++ b/examples/hardware_specific_examples/ESP32/encoder/esp32_position_control/esp32_position_control.ino @@ -24,6 +24,12 @@ void doTarget(char* cmd) { command.scalar(&target_angle, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialize encoder sensor hardware encoder.init(); encoder.enableInterrupts(doA, doB); @@ -66,9 +72,6 @@ void setup() { // maximal velocity of the position control motor.velocity_limit = 4; - - // use monitoring with serial - Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); diff --git a/examples/hardware_specific_examples/ESP32/magnetic_sensor/esp32_position_control/esp32_position_control.ino b/examples/hardware_specific_examples/ESP32/magnetic_sensor/esp32_position_control/esp32_position_control.ino index f025895b..9ba9604a 100644 --- a/examples/hardware_specific_examples/ESP32/magnetic_sensor/esp32_position_control/esp32_position_control.ino +++ b/examples/hardware_specific_examples/ESP32/magnetic_sensor/esp32_position_control/esp32_position_control.ino @@ -33,6 +33,12 @@ void doTarget(char* cmd) { command.scalar(&target_angle, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialise magnetic sensor hardware sensor.init(); // link the motor to the sensor @@ -69,8 +75,6 @@ void setup() { // maximal velocity of the position control motor.velocity_limit = 40; - // use monitoring with serial - Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); diff --git a/examples/hardware_specific_examples/HMBGC_example/position_control/position_control.ino b/examples/hardware_specific_examples/HMBGC_example/position_control/position_control.ino index 5b5c8e40..d9107654 100644 --- a/examples/hardware_specific_examples/HMBGC_example/position_control/position_control.ino +++ b/examples/hardware_specific_examples/HMBGC_example/position_control/position_control.ino @@ -42,6 +42,9 @@ void doTarget(char* cmd) { command.scalar(&target_angle, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // initialise encoder hardware encoder.init(); // interrupt initialization @@ -85,9 +88,6 @@ void setup() { // maximal velocity of the position control motor.velocity_limit = 4; - - // use monitoring with serial - Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); diff --git a/examples/hardware_specific_examples/Odrive_examples/odrive_example_encoder/odrive_example_encoder.ino b/examples/hardware_specific_examples/Odrive_examples/odrive_example_encoder/odrive_example_encoder.ino index b89c215f..504ab9c8 100644 --- a/examples/hardware_specific_examples/Odrive_examples/odrive_example_encoder/odrive_example_encoder.ino +++ b/examples/hardware_specific_examples/Odrive_examples/odrive_example_encoder/odrive_example_encoder.ino @@ -70,6 +70,12 @@ void doI(){encoder.handleIndex();} void setup(){ + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // pwm frequency to be used [Hz] driver.pwm_frequency = 20000; // power supply voltage [V] @@ -96,8 +102,6 @@ void setup(){ // alignment voltage limit motor.voltage_sensor_align = 0.5; - - Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); motor.monitor_variables = _MON_CURR_Q | _MON_CURR_D; diff --git a/examples/hardware_specific_examples/Odrive_examples/odrive_example_spi/odrive_example_spi.ino b/examples/hardware_specific_examples/Odrive_examples/odrive_example_spi/odrive_example_spi.ino index 2aed6bfb..7abcde54 100644 --- a/examples/hardware_specific_examples/Odrive_examples/odrive_example_spi/odrive_example_spi.ino +++ b/examples/hardware_specific_examples/Odrive_examples/odrive_example_spi/odrive_example_spi.ino @@ -69,6 +69,12 @@ SPIClass SPI_3(SPI3_MOSO, SPI3_MISO, SPI3_SCL); void setup(){ + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // pwm frequency to be used [Hz] driver.pwm_frequency = 20000; // power supply voltage [V] @@ -94,8 +100,6 @@ void setup(){ // alignment voltage limit motor.voltage_sensor_align = 0.5; - - Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); motor.monitor_variables = _MON_CURR_Q | _MON_CURR_D; diff --git a/examples/hardware_specific_examples/SAMD_examples/nano33IoT/nano33IoT_velocity_control/nano33IoT_velocity_control.ino b/examples/hardware_specific_examples/SAMD_examples/nano33IoT/nano33IoT_velocity_control/nano33IoT_velocity_control.ino index 70ec28b0..650050ef 100644 --- a/examples/hardware_specific_examples/SAMD_examples/nano33IoT/nano33IoT_velocity_control/nano33IoT_velocity_control.ino +++ b/examples/hardware_specific_examples/SAMD_examples/nano33IoT/nano33IoT_velocity_control/nano33IoT_velocity_control.ino @@ -23,7 +23,12 @@ void doTarget(char* cmd) { command.scalar(&target_velocity, cmd); } void setup() { + // use monitoring with serial Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + delay(1000); Serial.println("Initializing..."); diff --git a/examples/hardware_specific_examples/SimpleFOC-PowerShield/version_v02/single_full_control_example/single_full_control_example.ino b/examples/hardware_specific_examples/SimpleFOC-PowerShield/version_v02/single_full_control_example/single_full_control_example.ino index eb52f51b..bc387437 100644 --- a/examples/hardware_specific_examples/SimpleFOC-PowerShield/version_v02/single_full_control_example/single_full_control_example.ino +++ b/examples/hardware_specific_examples/SimpleFOC-PowerShield/version_v02/single_full_control_example/single_full_control_example.ino @@ -21,6 +21,12 @@ void doTarget(char* cmd){ command.scalar(&motor.target, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialize encoder sensor hardware encoder.init(); encoder.enableInterrupts(doA, doB); @@ -56,9 +62,6 @@ void setup() { // angle loop velocity limit motor.velocity_limit = 20; - // use monitoring with serial for motor init - // monitoring port - Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); motor.monitor_downsample = 0; // disable intially diff --git a/examples/hardware_specific_examples/SimpleFOCMini/angle_control/angle_control.ino b/examples/hardware_specific_examples/SimpleFOCMini/angle_control/angle_control.ino index 9102c89a..8eb122a0 100644 --- a/examples/hardware_specific_examples/SimpleFOCMini/angle_control/angle_control.ino +++ b/examples/hardware_specific_examples/SimpleFOCMini/angle_control/angle_control.ino @@ -38,6 +38,12 @@ Commander command = Commander(Serial); void doMotor(char* cmd) { command.motor(&motor, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // if SimpleFOCMini is stacked in arduino headers // on pins 12,11,10,9,8 // pin 12 is used as ground @@ -84,9 +90,6 @@ void setup() { // maximal velocity of the position control motor.velocity_limit = 4; - - // use monitoring with serial - Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); diff --git a/examples/hardware_specific_examples/SimpleFOCShield/version_v1/double_full_control_example/double_full_control_example.ino b/examples/hardware_specific_examples/SimpleFOCShield/version_v1/double_full_control_example/double_full_control_example.ino index c048956b..fb5e1563 100644 --- a/examples/hardware_specific_examples/SimpleFOCShield/version_v1/double_full_control_example/double_full_control_example.ino +++ b/examples/hardware_specific_examples/SimpleFOCShield/version_v1/double_full_control_example/double_full_control_example.ino @@ -28,6 +28,12 @@ void doMotor2(char* cmd){ command.motor(&motor2, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialize encoder sensor hardware encoder1.init(); encoder1.enableInterrupts(doA1, doB1); @@ -72,9 +78,6 @@ void setup() { motor1.velocity_limit = 20; motor2.velocity_limit = 20; - // use monitoring with serial for motor init - // monitoring port - Serial.begin(115200); // comment out if not needed motor1.useMonitoring(Serial); motor2.useMonitoring(Serial); diff --git a/examples/hardware_specific_examples/SimpleFOCShield/version_v1/single_full_control_example/single_full_control_example.ino b/examples/hardware_specific_examples/SimpleFOCShield/version_v1/single_full_control_example/single_full_control_example.ino index 10e26c9f..3faa38b2 100644 --- a/examples/hardware_specific_examples/SimpleFOCShield/version_v1/single_full_control_example/single_full_control_example.ino +++ b/examples/hardware_specific_examples/SimpleFOCShield/version_v1/single_full_control_example/single_full_control_example.ino @@ -17,6 +17,12 @@ void doMotor(char* cmd){ command.motor(&motor, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialize encoder sensor hardware encoder.init(); encoder.enableInterrupts(doA, doB); @@ -48,9 +54,6 @@ void setup() { // angle loop velocity limit motor.velocity_limit = 20; - // use monitoring with serial for motor init - // monitoring port - Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); motor.monitor_downsample = 0; // disable intially diff --git a/examples/hardware_specific_examples/SimpleFOCShield/version_v2/double_full_control_example/double_full_control_example.ino b/examples/hardware_specific_examples/SimpleFOCShield/version_v2/double_full_control_example/double_full_control_example.ino index b40ab62a..8eb3a86d 100644 --- a/examples/hardware_specific_examples/SimpleFOCShield/version_v2/double_full_control_example/double_full_control_example.ino +++ b/examples/hardware_specific_examples/SimpleFOCShield/version_v2/double_full_control_example/double_full_control_example.ino @@ -39,6 +39,12 @@ void doTarget2(char* cmd){ command.scalar(&motor2.target, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialize encoder sensor hardware encoder1.init(); encoder1.enableInterrupts(doA1, doB1); @@ -88,9 +94,6 @@ void setup() { motor1.velocity_limit = 20; motor2.velocity_limit = 20; - // use monitoring with serial for motor init - // monitoring port - Serial.begin(115200); // comment out if not needed motor1.useMonitoring(Serial); motor2.useMonitoring(Serial); diff --git a/examples/hardware_specific_examples/SimpleFOCShield/version_v2/single_full_control_example/single_full_control_example.ino b/examples/hardware_specific_examples/SimpleFOCShield/version_v2/single_full_control_example/single_full_control_example.ino index 76a7296a..b65a9cb0 100644 --- a/examples/hardware_specific_examples/SimpleFOCShield/version_v2/single_full_control_example/single_full_control_example.ino +++ b/examples/hardware_specific_examples/SimpleFOCShield/version_v2/single_full_control_example/single_full_control_example.ino @@ -22,6 +22,12 @@ void doMotion(char* cmd){ command.motion(&motor, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialize encoder sensor hardware encoder.init(); encoder.enableInterrupts(doA, doB); @@ -55,9 +61,6 @@ void setup() { // angle loop velocity limit motor.velocity_limit = 20; - // use monitoring with serial for motor init - // monitoring port - Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); motor.monitor_downsample = 0; // disable intially diff --git a/examples/hardware_specific_examples/SimpleFOCShield/version_v3/single_full_control_example/single_full_control_example.ino b/examples/hardware_specific_examples/SimpleFOCShield/version_v3/single_full_control_example/single_full_control_example.ino index 02593286..9c80a36f 100644 --- a/examples/hardware_specific_examples/SimpleFOCShield/version_v3/single_full_control_example/single_full_control_example.ino +++ b/examples/hardware_specific_examples/SimpleFOCShield/version_v3/single_full_control_example/single_full_control_example.ino @@ -22,6 +22,12 @@ void doMotion(char* cmd){ command.motion(&motor, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialize encoder sensor hardware encoder.init(); encoder.enableInterrupts(doA, doB); @@ -55,9 +61,6 @@ void setup() { // angle loop velocity limit motor.velocity_limit = 20; - // use monitoring with serial for motor init - // monitoring port - Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); motor.monitor_downsample = 0; // disable intially diff --git a/examples/hardware_specific_examples/Smart_Stepper/smartstepper_control/smartstepper_control.ino b/examples/hardware_specific_examples/Smart_Stepper/smartstepper_control/smartstepper_control.ino index 80f2fe64..18047108 100644 --- a/examples/hardware_specific_examples/Smart_Stepper/smartstepper_control/smartstepper_control.ino +++ b/examples/hardware_specific_examples/Smart_Stepper/smartstepper_control/smartstepper_control.ino @@ -18,6 +18,12 @@ void doMotor(char* cmd) { command.motor(&motor, cmd); } void setup() { + // use monitoring with SerialUSB + SerialUSB.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&SerialUSB); + // initialise magnetic sensor hardware sensor.init(); // link the motor to the sensor @@ -31,8 +37,6 @@ void setup() { // set motion control loop to be used motor.controller = MotionControlType::torque; - // use monitoring with SerialUSB - SerialUSB.begin(115200); // comment out if not needed motor.useMonitoring(SerialUSB); diff --git a/examples/hardware_specific_examples/Teensy/Teensy3/open_loop_velocity_6pwm/open_loop_velocity_6pwm.ino b/examples/hardware_specific_examples/Teensy/Teensy3/open_loop_velocity_6pwm/open_loop_velocity_6pwm.ino index 40924ee3..9143dde7 100644 --- a/examples/hardware_specific_examples/Teensy/Teensy3/open_loop_velocity_6pwm/open_loop_velocity_6pwm.ino +++ b/examples/hardware_specific_examples/Teensy/Teensy3/open_loop_velocity_6pwm/open_loop_velocity_6pwm.ino @@ -18,6 +18,12 @@ void doLimit(char* cmd) { command.scalar(&motor.voltage_limit, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // driver config // power supply voltage [V] driver.voltage_power_supply = 12; @@ -48,7 +54,6 @@ void setup() { command.add('T', doTarget, "target velocity"); command.add('L', doLimit, "voltage limit"); - Serial.begin(115200); Serial.println("Motor ready!"); Serial.println("Set target velocity [rad/s]"); _delay(1000); diff --git a/examples/hardware_specific_examples/Teensy/Teensy4/open_loop_velocity_6pwm/open_loop_velocity_6pwm.ino b/examples/hardware_specific_examples/Teensy/Teensy4/open_loop_velocity_6pwm/open_loop_velocity_6pwm.ino index 9c6eea03..5ff53311 100644 --- a/examples/hardware_specific_examples/Teensy/Teensy4/open_loop_velocity_6pwm/open_loop_velocity_6pwm.ino +++ b/examples/hardware_specific_examples/Teensy/Teensy4/open_loop_velocity_6pwm/open_loop_velocity_6pwm.ino @@ -33,6 +33,12 @@ void doLimit(char* cmd) { command.scalar(&motor.voltage_limit, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // driver config // power supply voltage [V] driver.voltage_power_supply = 12; @@ -63,7 +69,6 @@ void setup() { command.add('T', doTarget, "target velocity"); command.add('L', doLimit, "voltage limit"); - Serial.begin(115200); Serial.println("Motor ready!"); Serial.println("Set target velocity [rad/s]"); _delay(1000); diff --git a/examples/motion_control/open_loop_motor_control/open_loop_position_example/open_loop_position_example.ino b/examples/motion_control/open_loop_motor_control/open_loop_position_example/open_loop_position_example.ino index 5a915dd8..6bcfdff9 100644 --- a/examples/motion_control/open_loop_motor_control/open_loop_position_example/open_loop_position_example.ino +++ b/examples/motion_control/open_loop_motor_control/open_loop_position_example/open_loop_position_example.ino @@ -23,6 +23,12 @@ void doVelocity(char* cmd) { command.scalar(&motor.velocity_limit, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // driver config // power supply voltage [V] driver.voltage_power_supply = 12; @@ -53,7 +59,6 @@ void setup() { command.add('L', doLimit, "voltage limit"); command.add('V', doLimit, "movement velocity"); - Serial.begin(115200); Serial.println("Motor ready!"); Serial.println("Set target position [rad]"); _delay(1000); diff --git a/examples/motion_control/open_loop_motor_control/open_loop_velocity_example/open_loop_velocity_example.ino b/examples/motion_control/open_loop_motor_control/open_loop_velocity_example/open_loop_velocity_example.ino index 43fc6f73..075dc9c6 100644 --- a/examples/motion_control/open_loop_motor_control/open_loop_velocity_example/open_loop_velocity_example.ino +++ b/examples/motion_control/open_loop_motor_control/open_loop_velocity_example/open_loop_velocity_example.ino @@ -23,6 +23,12 @@ void doLimit(char* cmd) { command.scalar(&motor.voltage_limit, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // driver config // power supply voltage [V] driver.voltage_power_supply = 12; @@ -50,7 +56,6 @@ void setup() { command.add('T', doTarget, "target velocity"); command.add('L', doLimit, "voltage limit"); - Serial.begin(115200); Serial.println("Motor ready!"); Serial.println("Set target velocity [rad/s]"); _delay(1000); diff --git a/examples/motion_control/position_motion_control/encoder/angle_control/angle_control.ino b/examples/motion_control/position_motion_control/encoder/angle_control/angle_control.ino index ccb3980e..03e6ea75 100644 --- a/examples/motion_control/position_motion_control/encoder/angle_control/angle_control.ino +++ b/examples/motion_control/position_motion_control/encoder/angle_control/angle_control.ino @@ -48,6 +48,12 @@ void doTarget(char* cmd) { command.scalar(&target_angle, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialize encoder sensor hardware encoder.init(); encoder.enableInterrupts(doA, doB); @@ -88,9 +94,6 @@ void setup() { // maximal velocity of the position control motor.velocity_limit = 4; - - // use monitoring with serial - Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); diff --git a/examples/motion_control/position_motion_control/magnetic_sensor/angle_control/angle_control.ino b/examples/motion_control/position_motion_control/magnetic_sensor/angle_control/angle_control.ino index 752030c9..f0f9b27c 100644 --- a/examples/motion_control/position_motion_control/magnetic_sensor/angle_control/angle_control.ino +++ b/examples/motion_control/position_motion_control/magnetic_sensor/angle_control/angle_control.ino @@ -31,6 +31,12 @@ void doTarget(char* cmd) { command.scalar(&target_angle, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialise magnetic sensor hardware sensor.init(); // link the motor to the sensor @@ -67,9 +73,7 @@ void setup() { motor.P_angle.P = 20; // maximal velocity of the position control motor.velocity_limit = 20; - - // use monitoring with serial - Serial.begin(115200); + // comment out if not needed motor.useMonitoring(Serial); diff --git a/examples/motion_control/torque_control/encoder/current_control/current_control.ino b/examples/motion_control/torque_control/encoder/current_control/current_control.ino index c9123401..82aec86a 100644 --- a/examples/motion_control/torque_control/encoder/current_control/current_control.ino +++ b/examples/motion_control/torque_control/encoder/current_control/current_control.ino @@ -27,6 +27,12 @@ void doTarget(char* cmd) { command.scalar(&target_current, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialize encoder sensor hardware encoder.init(); encoder.enableInterrupts(doA, doB); @@ -70,8 +76,6 @@ void setup() { // motor.LPF_current_q.Tf = 0.002f; // 1ms default // motor.LPF_current_d.Tf = 0.002f; // 1ms default - // use monitoring with serial - Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); diff --git a/examples/motion_control/torque_control/encoder/voltage_control/voltage_control.ino b/examples/motion_control/torque_control/encoder/voltage_control/voltage_control.ino index 66ff4569..219637c9 100644 --- a/examples/motion_control/torque_control/encoder/voltage_control/voltage_control.ino +++ b/examples/motion_control/torque_control/encoder/voltage_control/voltage_control.ino @@ -33,6 +33,12 @@ void doTarget(char* cmd) { command.scalar(&target_voltage, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialize encoder sensor hardware encoder.init(); encoder.enableInterrupts(doA, doB); @@ -55,8 +61,6 @@ void setup() { // set motion control loop to be used motor.controller = MotionControlType::torque; - // use monitoring with serial - Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); diff --git a/examples/motion_control/torque_control/hall_sensor/voltage_control/voltage_control.ino b/examples/motion_control/torque_control/hall_sensor/voltage_control/voltage_control.ino index 7273d156..c72c9224 100644 --- a/examples/motion_control/torque_control/hall_sensor/voltage_control/voltage_control.ino +++ b/examples/motion_control/torque_control/hall_sensor/voltage_control/voltage_control.ino @@ -35,6 +35,12 @@ void doTarget(char* cmd) { command.scalar(&target_voltage, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialize encoder sensor hardware sensor.init(); sensor.enableInterrupts(doA, doB, doC); @@ -57,8 +63,6 @@ void setup() { // set motion control loop to be used motor.controller = MotionControlType::torque; - // use monitoring with serial - Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); diff --git a/examples/motion_control/torque_control/magnetic_sensor/voltage_control/voltage_control.ino b/examples/motion_control/torque_control/magnetic_sensor/voltage_control/voltage_control.ino index d869f4dd..dbb56080 100644 --- a/examples/motion_control/torque_control/magnetic_sensor/voltage_control/voltage_control.ino +++ b/examples/motion_control/torque_control/magnetic_sensor/voltage_control/voltage_control.ino @@ -30,6 +30,12 @@ void doTarget(char* cmd) { command.scalar(&target_voltage, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialise magnetic sensor hardware sensor.init(); // link the motor to the sensor @@ -47,8 +53,6 @@ void setup() { // set motion control loop to be used motor.controller = MotionControlType::torque; - // use monitoring with serial - Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); diff --git a/examples/motion_control/velocity_motion_control/hall_sensor/velocity_control/velocity_control.ino b/examples/motion_control/velocity_motion_control/hall_sensor/velocity_control/velocity_control.ino index 1d39173a..4fb47f02 100644 --- a/examples/motion_control/velocity_motion_control/hall_sensor/velocity_control/velocity_control.ino +++ b/examples/motion_control/velocity_motion_control/hall_sensor/velocity_control/velocity_control.ino @@ -47,6 +47,12 @@ void doTarget(char* cmd) { command.scalar(&target_velocity, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialize sensor sensor hardware sensor.init(); sensor.enableInterrupts(doA, doB); //, doC); @@ -84,8 +90,6 @@ void setup() { // velocity low pass filtering time constant motor.LPF_velocity.Tf = 0.01f; - // use monitoring with serial - Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); diff --git a/examples/motion_control/velocity_motion_control/magnetic_sensor/velocity_control/velocity_control.ino b/examples/motion_control/velocity_motion_control/magnetic_sensor/velocity_control/velocity_control.ino index 98e09425..40299b8f 100644 --- a/examples/motion_control/velocity_motion_control/magnetic_sensor/velocity_control/velocity_control.ino +++ b/examples/motion_control/velocity_motion_control/magnetic_sensor/velocity_control/velocity_control.ino @@ -33,6 +33,12 @@ void doTarget(char* cmd) { command.scalar(&target_velocity, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialise magnetic sensor hardware sensor.init(); // link the motor to the sensor @@ -66,8 +72,6 @@ void setup() { // the lower the less filtered motor.LPF_velocity.Tf = 0.01f; - // use monitoring with serial - Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); diff --git a/examples/motor_commands_serial_examples/encoder/full_control_serial/full_control_serial.ino b/examples/motor_commands_serial_examples/encoder/full_control_serial/full_control_serial.ino index 186d257e..d6ddc6b4 100644 --- a/examples/motor_commands_serial_examples/encoder/full_control_serial/full_control_serial.ino +++ b/examples/motor_commands_serial_examples/encoder/full_control_serial/full_control_serial.ino @@ -34,6 +34,12 @@ void onMotor(char* cmd){ command.motor(&motor, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialize encoder sensor hardware encoder.init(); encoder.enableInterrupts(doA, doB); @@ -68,9 +74,6 @@ void setup() { // angle loop velocity limit motor.velocity_limit = 50; - // use monitoring with serial for motor init - // monitoring port - Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); diff --git a/examples/motor_commands_serial_examples/hall_sensor/full_control_serial/full_control_serial.ino b/examples/motor_commands_serial_examples/hall_sensor/full_control_serial/full_control_serial.ino index df7b76ac..ff6fe7f3 100644 --- a/examples/motor_commands_serial_examples/hall_sensor/full_control_serial/full_control_serial.ino +++ b/examples/motor_commands_serial_examples/hall_sensor/full_control_serial/full_control_serial.ino @@ -41,6 +41,12 @@ void onMotor(char* cmd){ command.motor(&motor, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialize encoder sensor hardware sensor.init(); sensor.enableInterrupts(doA, doB); //, doC); @@ -76,9 +82,6 @@ void setup() { // angle loop velocity limit motor.velocity_limit = 50; - // use monitoring with serial for motor init - // monitoring port - Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); diff --git a/examples/motor_commands_serial_examples/magnetic_sensor/full_control_serial/full_control_serial.ino b/examples/motor_commands_serial_examples/magnetic_sensor/full_control_serial/full_control_serial.ino index 098f6888..e863d8c9 100644 --- a/examples/motor_commands_serial_examples/magnetic_sensor/full_control_serial/full_control_serial.ino +++ b/examples/motor_commands_serial_examples/magnetic_sensor/full_control_serial/full_control_serial.ino @@ -33,6 +33,12 @@ void onMotor(char* cmd){ command.motor(&motor, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialise magnetic sensor hardware sensor.init(); // link the motor to the sensor @@ -66,9 +72,6 @@ void setup() { // angle loop velocity limit motor.velocity_limit = 50; - // use monitoring with serial for motor init - // monitoring port - Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); diff --git a/examples/osc_control_examples/osc_esp32_3pwm/osc_esp32_3pwm.ino b/examples/osc_control_examples/osc_esp32_3pwm/osc_esp32_3pwm.ino index afb95294..14d4867c 100644 --- a/examples/osc_control_examples/osc_esp32_3pwm/osc_esp32_3pwm.ino +++ b/examples/osc_control_examples/osc_esp32_3pwm/osc_esp32_3pwm.ino @@ -67,8 +67,12 @@ BLDCDriver3PWM driver = BLDCDriver3PWM(25, 26, 27); Commander command = Commander(Serial); void setup() { + // use monitoring with serial Serial.begin(115200); - + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + WiFi.begin(ssid, pass); Serial.print("Connecting WiFi "); diff --git a/examples/osc_control_examples/osc_esp32_fullcontrol/osc_esp32_fullcontrol.ino b/examples/osc_control_examples/osc_esp32_fullcontrol/osc_esp32_fullcontrol.ino index 7c6aa4f0..940b59c2 100644 --- a/examples/osc_control_examples/osc_esp32_fullcontrol/osc_esp32_fullcontrol.ino +++ b/examples/osc_control_examples/osc_esp32_fullcontrol/osc_esp32_fullcontrol.ino @@ -104,7 +104,12 @@ void setup() { digitalWrite(26, 0); digitalWrite(27, 0); + // use monitoring with serial Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + delay(200); WiFi.begin(ssid, pass); diff --git a/examples/utils/calibration/alignment_and_cogging_test/alignment_and_cogging_test.ino b/examples/utils/calibration/alignment_and_cogging_test/alignment_and_cogging_test.ino index 7324edf0..9c523c22 100644 --- a/examples/utils/calibration/alignment_and_cogging_test/alignment_and_cogging_test.ino +++ b/examples/utils/calibration/alignment_and_cogging_test/alignment_and_cogging_test.ino @@ -80,8 +80,11 @@ void testAlignmentAndCogging(int direction) { void setup() { + // use monitoring with serial Serial.begin(115200); - while (!Serial) ; + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); // driver config driver.voltage_power_supply = 12; diff --git a/examples/utils/calibration/find_kv_rating/encoder/find_kv_rating/find_kv_rating.ino b/examples/utils/calibration/find_kv_rating/encoder/find_kv_rating/find_kv_rating.ino index c304d195..c39657b1 100644 --- a/examples/utils/calibration/find_kv_rating/encoder/find_kv_rating/find_kv_rating.ino +++ b/examples/utils/calibration/find_kv_rating/encoder/find_kv_rating/find_kv_rating.ino @@ -42,6 +42,12 @@ void calcKV(char* cmd) { void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialize encoder sensor hardware sensor.init(); sensor.enableInterrupts(doA, doB); @@ -62,8 +68,6 @@ void setup() { // set motion control loop to be used motor.controller = MotionControlType::torque; - // use monitoring with serial - Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); diff --git a/examples/utils/calibration/find_kv_rating/hall_sensor/find_kv_rating/find_kv_rating.ino b/examples/utils/calibration/find_kv_rating/hall_sensor/find_kv_rating/find_kv_rating.ino index 575968f4..4eadd376 100644 --- a/examples/utils/calibration/find_kv_rating/hall_sensor/find_kv_rating/find_kv_rating.ino +++ b/examples/utils/calibration/find_kv_rating/hall_sensor/find_kv_rating/find_kv_rating.ino @@ -39,6 +39,12 @@ void calcKV(char* cmd) { void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialize encoder sensor hardware sensor.init(); sensor.enableInterrupts(doA, doB, doC); @@ -58,9 +64,7 @@ void setup() { // set motion control loop to be used motor.controller = MotionControlType::torque; - - // use monitoring with serial - Serial.begin(115200); + // comment out if not needed motor.useMonitoring(Serial); diff --git a/examples/utils/calibration/find_kv_rating/magnetic_sensor/find_kv_rating/find_kv_rating.ino b/examples/utils/calibration/find_kv_rating/magnetic_sensor/find_kv_rating/find_kv_rating.ino index a9d29d86..1df631af 100644 --- a/examples/utils/calibration/find_kv_rating/magnetic_sensor/find_kv_rating/find_kv_rating.ino +++ b/examples/utils/calibration/find_kv_rating/magnetic_sensor/find_kv_rating/find_kv_rating.ino @@ -37,6 +37,12 @@ void calcKV(char* cmd) { void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialize encoder sensor hardware sensor.init(); // link the motor to the sensor @@ -56,8 +62,6 @@ void setup() { // set motion control loop to be used motor.controller = MotionControlType::torque; - // use monitoring with serial - Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); diff --git a/examples/utils/calibration/find_pole_pair_number/encoder/find_pole_pairs_number/find_pole_pairs_number.ino b/examples/utils/calibration/find_pole_pair_number/encoder/find_pole_pairs_number/find_pole_pairs_number.ino index aac879cd..ad8e69ce 100644 --- a/examples/utils/calibration/find_pole_pair_number/encoder/find_pole_pairs_number/find_pole_pairs_number.ino +++ b/examples/utils/calibration/find_pole_pair_number/encoder/find_pole_pairs_number/find_pole_pairs_number.ino @@ -32,6 +32,12 @@ void doB(){encoder.handleB();} void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialise encoder hardware encoder.init(); // hardware interrupt enable @@ -48,8 +54,6 @@ void setup() { // initialize motor motor.init(); - // monitoring port - Serial.begin(115200); // pole pairs calculation routine Serial.println("Pole pairs (PP) estimator"); diff --git a/examples/utils/calibration/find_pole_pair_number/magnetic_sensor/find_pole_pairs_number/find_pole_pairs_number.ino b/examples/utils/calibration/find_pole_pair_number/magnetic_sensor/find_pole_pairs_number/find_pole_pairs_number.ino index e98a4529..b44bc0bb 100644 --- a/examples/utils/calibration/find_pole_pair_number/magnetic_sensor/find_pole_pairs_number/find_pole_pairs_number.ino +++ b/examples/utils/calibration/find_pole_pair_number/magnetic_sensor/find_pole_pairs_number/find_pole_pairs_number.ino @@ -33,6 +33,12 @@ MagneticSensorSPI sensor = MagneticSensorSPI(10, 14, 0x3FFF); void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialise magnetic sensor hardware sensor.init(); // link the motor to the sensor @@ -47,9 +53,6 @@ void setup() { // initialize motor hardware motor.init(); - // monitoring port - Serial.begin(115200); - // pole pairs calculation routine Serial.println("Pole pairs (PP) estimator"); Serial.println("-\n"); diff --git a/examples/utils/calibration/find_sensor_offset_and_direction/find_sensor_offset_and_direction.ino b/examples/utils/calibration/find_sensor_offset_and_direction/find_sensor_offset_and_direction.ino index 27163dfb..407469fc 100644 --- a/examples/utils/calibration/find_sensor_offset_and_direction/find_sensor_offset_and_direction.ino +++ b/examples/utils/calibration/find_sensor_offset_and_direction/find_sensor_offset_and_direction.ino @@ -29,6 +29,12 @@ BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8); void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // power supply voltage driver.voltage_power_supply = 12; driver.init(); @@ -54,7 +60,6 @@ void setup() { motor.initFOC(); - Serial.begin(115200); Serial.println("Sensor zero offset is:"); Serial.println(motor.zero_electric_angle, 4); Serial.println("Sensor natural direction is: "); diff --git a/examples/utils/communication_test/commander/commander_tune_custom_loop/commander_tune_custom_loop.ino b/examples/utils/communication_test/commander/commander_tune_custom_loop/commander_tune_custom_loop.ino index cbeb6ecb..074538ad 100644 --- a/examples/utils/communication_test/commander/commander_tune_custom_loop/commander_tune_custom_loop.ino +++ b/examples/utils/communication_test/commander/commander_tune_custom_loop/commander_tune_custom_loop.ino @@ -29,6 +29,12 @@ void doTarget(char* cmd) { command.scalar(&target_velocity, cmd); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialize encoder sensor hardware encoder.init(); encoder.enableInterrupts(doA, doB); @@ -46,7 +52,6 @@ void setup() { motor.controller = MotionControlType::torque; // use monitoring with serial - Serial.begin(115200); motor.useMonitoring(Serial); // initialize motor motor.init(); diff --git a/examples/utils/communication_test/step_dir/step_dir_motor_example/step_dir_motor_example.ino b/examples/utils/communication_test/step_dir/step_dir_motor_example/step_dir_motor_example.ino index f8978577..4cd87970 100644 --- a/examples/utils/communication_test/step_dir/step_dir_motor_example/step_dir_motor_example.ino +++ b/examples/utils/communication_test/step_dir/step_dir_motor_example/step_dir_motor_example.ino @@ -24,6 +24,12 @@ void onStep() { step_dir.handle(); } void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialize encoder sensor hardware encoder.init(); encoder.enableInterrupts(doA, doB); @@ -64,9 +70,7 @@ void setup() { motor.P_angle.P = 10; // maximal velocity of the position control motor.velocity_limit = 100; - - // use monitoring with serial - Serial.begin(115200); + // comment out if not needed motor.useMonitoring(Serial); diff --git a/examples/utils/current_sense_test/generic_current_sense/generic_current_sense.ino b/examples/utils/current_sense_test/generic_current_sense/generic_current_sense.ino index e999de23..2448a51c 100644 --- a/examples/utils/current_sense_test/generic_current_sense/generic_current_sense.ino +++ b/examples/utils/current_sense_test/generic_current_sense/generic_current_sense.ino @@ -31,6 +31,14 @@ GenericCurrentSense current_sense = GenericCurrentSense(readCurrentSense, initCu void setup() { + + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + + // if callbacks are not provided in the constructor // they can be assigned directly: //current_sense.readCallback = readCurrentSense; @@ -40,7 +48,6 @@ void setup() { current_sense.init(); - Serial.begin(115200); Serial.println("Current sense ready."); } diff --git a/examples/utils/current_sense_test/inline_current_sense_test/inline_current_sense_test.ino b/examples/utils/current_sense_test/inline_current_sense_test/inline_current_sense_test.ino index c1a5139b..195021eb 100644 --- a/examples/utils/current_sense_test/inline_current_sense_test/inline_current_sense_test.ino +++ b/examples/utils/current_sense_test/inline_current_sense_test/inline_current_sense_test.ino @@ -11,13 +11,19 @@ InlineCurrentSense current_sense = InlineCurrentSense(0.01f, 50.0f, A0, A2); void setup() { + + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // initialise the current sensing current_sense.init(); // for SimpleFOCShield v2.01/v2.0.2 current_sense.gain_b *= -1; - Serial.begin(115200); Serial.println("Current sense ready."); } diff --git a/examples/utils/driver_standalone_test/bldc_driver_3pwm_standalone/bldc_driver_3pwm_standalone.ino b/examples/utils/driver_standalone_test/bldc_driver_3pwm_standalone/bldc_driver_3pwm_standalone.ino index 1235fccd..82716353 100644 --- a/examples/utils/driver_standalone_test/bldc_driver_3pwm_standalone/bldc_driver_3pwm_standalone.ino +++ b/examples/utils/driver_standalone_test/bldc_driver_3pwm_standalone/bldc_driver_3pwm_standalone.ino @@ -7,6 +7,12 @@ BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8); void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // pwm frequency to be used [Hz] // for atmega328 fixed to 32kHz // esp32/stm32/teensy configurable diff --git a/examples/utils/driver_standalone_test/bldc_driver_6pwm_standalone/bldc_driver_6pwm_standalone.ino b/examples/utils/driver_standalone_test/bldc_driver_6pwm_standalone/bldc_driver_6pwm_standalone.ino index 478d3974..1ed37441 100644 --- a/examples/utils/driver_standalone_test/bldc_driver_6pwm_standalone/bldc_driver_6pwm_standalone.ino +++ b/examples/utils/driver_standalone_test/bldc_driver_6pwm_standalone/bldc_driver_6pwm_standalone.ino @@ -6,6 +6,12 @@ BLDCDriver6PWM driver = BLDCDriver6PWM(5, 6, 9,10, 3, 11, 8); void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // pwm frequency to be used [Hz] // for atmega328 fixed to 32kHz // esp32/stm32/teensy configurable diff --git a/examples/utils/driver_standalone_test/stepper_driver_2pwm_standalone/stepper_driver_2pwm_standalone.ino b/examples/utils/driver_standalone_test/stepper_driver_2pwm_standalone/stepper_driver_2pwm_standalone.ino index 4627efa9..889143e2 100644 --- a/examples/utils/driver_standalone_test/stepper_driver_2pwm_standalone/stepper_driver_2pwm_standalone.ino +++ b/examples/utils/driver_standalone_test/stepper_driver_2pwm_standalone/stepper_driver_2pwm_standalone.ino @@ -13,6 +13,12 @@ StepperDriver2PWM driver = StepperDriver2PWM(3, in1, 10 , in2, 11, 12); void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // pwm frequency to be used [Hz] // for atmega328 fixed to 32kHz // esp32/stm32/teensy configurable diff --git a/examples/utils/driver_standalone_test/stepper_driver_4pwm_standalone/stepper_driver_4pwm_standalone.ino b/examples/utils/driver_standalone_test/stepper_driver_4pwm_standalone/stepper_driver_4pwm_standalone.ino index e028a737..95dcc589 100644 --- a/examples/utils/driver_standalone_test/stepper_driver_4pwm_standalone/stepper_driver_4pwm_standalone.ino +++ b/examples/utils/driver_standalone_test/stepper_driver_4pwm_standalone/stepper_driver_4pwm_standalone.ino @@ -8,6 +8,12 @@ StepperDriver4PWM driver = StepperDriver4PWM(5, 6, 9,10, 7, 8); void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + // pwm frequency to be used [Hz] // for atmega328 fixed to 32kHz // esp32/stm32/teensy configurable From 52e8d9df049c82f6457ef37303b90beb6efc6327 Mon Sep 17 00:00:00 2001 From: askuric Date: Fri, 21 Jun 2024 13:59:59 +0200 Subject: [PATCH 29/57] added the support for esp32s3 to read all the current phases in one interrupt --- .../esp32/esp32_mcpwm_mcu.cpp | 45 +++++++++++++++---- .../esp32/esp32_driver_mcpwm.cpp | 2 +- 2 files changed, 38 insertions(+), 9 deletions(-) diff --git a/src/current_sense/hardware_specific/esp32/esp32_mcpwm_mcu.cpp b/src/current_sense/hardware_specific/esp32/esp32_mcpwm_mcu.cpp index 6081f87e..6e25e164 100644 --- a/src/current_sense/hardware_specific/esp32/esp32_mcpwm_mcu.cpp +++ b/src/current_sense/hardware_specific/esp32/esp32_mcpwm_mcu.cpp @@ -22,8 +22,28 @@ #include #include +#define SIMPLEFOC_ESP32_INTERRUPT_DEBUG + #ifdef SIMPLEFOC_ESP32_INTERRUPT_DEBUG #include "driver/gpio.h" + + +// if the MCU is not ESP32S3, the ADC read time is too long to +// sample all three phase currents in one interrupt +// so we will sample one phase per interrupt +#ifdef CONFIG_IDF_TARGET_ESP32S3 +#define SIMPLEFOC_SAMPLE_ONCE_PER_INTERRUPT +#endif + + +#ifdef CONFIG_IDF_TARGET_ESP32S3 +#define DEBUGPIN 16 +#define GPIO_NUM GPIO_NUM_16 +#else +#define DEBUGPIN 19 +#define GPIO_NUM GPIO_NUM_19 +#endif + #endif #define _ADC_VOLTAGE 3.3f @@ -132,9 +152,11 @@ void* _configureADCLowSide(const void* driver_params, const int pinA,const int p return params; } + + void* _driverSyncLowSide(void* driver_params, void* cs_params){ #ifdef SIMPLEFOC_ESP32_INTERRUPT_DEBUG - pinMode(19, OUTPUT); + pinMode(DEBUGPIN, OUTPUT); #endif ESP32MCPWMDriverParams *p = (ESP32MCPWMDriverParams*)driver_params; mcpwm_timer_t* t = (mcpwm_timer_t*) p->timers[0]; @@ -150,22 +172,29 @@ void* _driverSyncLowSide(void* driver_params, void* cs_params){ // mcpwm_timer_event_callbacks_t can be used to set the callback // for three timer events // - on_full - low-side - // - on_empty - high-side + // - on_empty - high-side // - on_sync - sync event (not used with simplefoc) auto cbs = mcpwm_timer_event_callbacks_t{ .on_full = [](mcpwm_timer_handle_t tim, const mcpwm_timer_event_data_t* edata, void* user_data){ ESP32MCPWMCurrentSenseParams *p = (ESP32MCPWMCurrentSenseParams*)user_data; -#ifdef SIMPLEFOC_ESP32_INTERRUPT_DEBUG - gpio_set_level(GPIO_NUM_19,1); //cca 250ns for on+off +#ifdef SIMPLEFOC_ESP32_INTERRUPT_DEBUG // debugging toggle pin to measure the time of the interrupt with oscilloscope + gpio_set_level(GPIO_NUM,1); //cca 250ns for on+off #endif + +#ifdef SIMPLEFOC_SAMPLE_ONCE_PER_INTERRUPT // sample the phase currents one at a time + // ex. ESP32's adc read takes around 10us which is very long // increment buffer index p->buffer_index = (p->buffer_index + 1) % p->no_adc_channels; - // sample the phase currents one at a time - // adc read takes around 10us which is very long // so we are sampling one phase per call p->adc_buffer[p->buffer_index] = adcRead(p->pins[p->buffer_index]); -#ifdef SIMPLEFOC_ESP32_INTERRUPT_DEBUG - gpio_set_level(GPIO_NUM_19,0); //cca 250ns for on+off +#else // sample all available phase currents at once + // ex. ESP32S3's adc read takes around 1us which is good enough + for(int i=0; i < p->no_adc_channels; i++) + p->adc_buffer[p->buffer_index] = adcRead(p->pins[p->buffer_index]); +#endif + +#ifdef SIMPLEFOC_ESP32_INTERRUPT_DEBUG // debugging toggle pin to measure the time of the interrupt with oscilloscope + gpio_set_level(GPIO_NUM,0); //cca 250ns for on+off #endif return true; }, diff --git a/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.cpp b/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.cpp index 74ed4c74..4ab02993 100644 --- a/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.cpp +++ b/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.cpp @@ -489,7 +489,7 @@ void* _configurePinsMCPWM(long pwm_frequency, int mcpwm_group, int timer_no, int // function setting the duty cycle to the MCPWM pin void _setDutyCycle(mcpwm_cmpr_handle_t cmpr, uint32_t mcpwm_period, float duty_cycle){ - float duty = constrain(duty_cycle, 0.0, 1.0); + float duty = _constrain(duty_cycle, 0.0, 1.0); mcpwm_comparator_set_compare_value(cmpr, (uint32_t)(mcpwm_period*duty)); } From 910c24ff7e26007235b77a6c8a6a16ae43c454ec Mon Sep 17 00:00:00 2001 From: askuric Date: Fri, 21 Jun 2024 14:03:09 +0200 Subject: [PATCH 30/57] forgotten change for bg431 --- src/current_sense/hardware_specific/stm32/b_g431/b_g431_mcu.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/current_sense/hardware_specific/stm32/b_g431/b_g431_mcu.cpp b/src/current_sense/hardware_specific/stm32/b_g431/b_g431_mcu.cpp index 8cfda6d7..46cb20be 100644 --- a/src/current_sense/hardware_specific/stm32/b_g431/b_g431_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/b_g431/b_g431_mcu.cpp @@ -27,7 +27,7 @@ volatile uint16_t adcBuffer2[ADC_BUF_LEN_2] = {0}; // Buffer for store the resul // function reading an ADC value and returning the read voltage // As DMA is being used just return the DMA result -float _readADCVoltageInline(const int pin, const void* cs_params){ +float _readADCVoltageLowSide(const int pin, const void* cs_params){ uint32_t raw_adc = 0; if(pin == PA2) // = ADC1_IN3 = phase U (OP1_OUT) on B-G431B-ESC1 raw_adc = adcBuffer1[1]; From 220050447102d58134c766a8e12a5dca90f1b539 Mon Sep 17 00:00:00 2001 From: askuric Date: Fri, 21 Jun 2024 14:21:37 +0200 Subject: [PATCH 31/57] s3 bugfix - it cannot read faster than 10us per adc sample --- .../hardware_specific/esp32/esp32_mcpwm_mcu.cpp | 17 ++--------------- 1 file changed, 2 insertions(+), 15 deletions(-) diff --git a/src/current_sense/hardware_specific/esp32/esp32_mcpwm_mcu.cpp b/src/current_sense/hardware_specific/esp32/esp32_mcpwm_mcu.cpp index 6e25e164..49f549dc 100644 --- a/src/current_sense/hardware_specific/esp32/esp32_mcpwm_mcu.cpp +++ b/src/current_sense/hardware_specific/esp32/esp32_mcpwm_mcu.cpp @@ -28,14 +28,6 @@ #include "driver/gpio.h" -// if the MCU is not ESP32S3, the ADC read time is too long to -// sample all three phase currents in one interrupt -// so we will sample one phase per interrupt -#ifdef CONFIG_IDF_TARGET_ESP32S3 -#define SIMPLEFOC_SAMPLE_ONCE_PER_INTERRUPT -#endif - - #ifdef CONFIG_IDF_TARGET_ESP32S3 #define DEBUGPIN 16 #define GPIO_NUM GPIO_NUM_16 @@ -181,17 +173,12 @@ void* _driverSyncLowSide(void* driver_params, void* cs_params){ gpio_set_level(GPIO_NUM,1); //cca 250ns for on+off #endif -#ifdef SIMPLEFOC_SAMPLE_ONCE_PER_INTERRUPT // sample the phase currents one at a time - // ex. ESP32's adc read takes around 10us which is very long + // sample the phase currents one at a time + // ESP's adc read takes around 10us which is very long // increment buffer index p->buffer_index = (p->buffer_index + 1) % p->no_adc_channels; // so we are sampling one phase per call p->adc_buffer[p->buffer_index] = adcRead(p->pins[p->buffer_index]); -#else // sample all available phase currents at once - // ex. ESP32S3's adc read takes around 1us which is good enough - for(int i=0; i < p->no_adc_channels; i++) - p->adc_buffer[p->buffer_index] = adcRead(p->pins[p->buffer_index]); -#endif #ifdef SIMPLEFOC_ESP32_INTERRUPT_DEBUG // debugging toggle pin to measure the time of the interrupt with oscilloscope gpio_set_level(GPIO_NUM,0); //cca 250ns for on+off From e92567e941aad2b989ff61a5b56bedf4ccfd536e Mon Sep 17 00:00:00 2001 From: askuric Date: Fri, 21 Jun 2024 15:41:23 +0200 Subject: [PATCH 32/57] bugfix for #295 and #320 + added #346 --- .../hardware_specific/esp32/esp32s_adc_driver.cpp | 14 ++++++-------- 1 file changed, 6 insertions(+), 8 deletions(-) diff --git a/src/current_sense/hardware_specific/esp32/esp32s_adc_driver.cpp b/src/current_sense/hardware_specific/esp32/esp32s_adc_driver.cpp index a212d57b..a8541568 100644 --- a/src/current_sense/hardware_specific/esp32/esp32s_adc_driver.cpp +++ b/src/current_sense/hardware_specific/esp32/esp32s_adc_driver.cpp @@ -111,7 +111,7 @@ void __analogSetPinAttenuation(uint8_t pin, uint8_t attenuation) return ; } __analogInit(); - if(channel > 7){ + if(channel > 9){ SET_PERI_REG_BITS(SENS_SAR_ATTEN2_REG, 3, attenuation, ((channel - 10) * 2)); } else { SET_PERI_REG_BITS(SENS_SAR_ATTEN1_REG, 3, attenuation, (channel * 2)); @@ -152,9 +152,8 @@ bool IRAM_ATTR __adcStart(uint8_t pin){ } if(channel > 9){ - channel -= 10; CLEAR_PERI_REG_MASK(SENS_SAR_MEAS2_CTRL2_REG, SENS_MEAS2_START_SAR_M); - SET_PERI_REG_BITS(SENS_SAR_MEAS2_CTRL2_REG, SENS_SAR2_EN_PAD, (1 << channel), SENS_SAR2_EN_PAD_S); + SET_PERI_REG_BITS(SENS_SAR_MEAS2_CTRL2_REG, SENS_SAR2_EN_PAD, (1 << (channel - 10)), SENS_SAR2_EN_PAD_S); SET_PERI_REG_MASK(SENS_SAR_MEAS2_CTRL2_REG, SENS_MEAS2_START_SAR_M); } else { CLEAR_PERI_REG_MASK(SENS_SAR_MEAS1_CTRL2_REG, SENS_MEAS1_START_SAR_M); @@ -171,7 +170,7 @@ bool IRAM_ATTR __adcBusy(uint8_t pin){ return false;//not adc pin } - if(channel > 7){ + if(channel > 9){ return (GET_PERI_REG_MASK(SENS_SAR_MEAS2_CTRL2_REG, SENS_MEAS2_DONE_SAR) == 0); } return (GET_PERI_REG_MASK(SENS_SAR_MEAS1_CTRL2_REG, SENS_MEAS1_DONE_SAR) == 0); @@ -185,7 +184,7 @@ uint16_t IRAM_ATTR __adcEnd(uint8_t pin) if(channel < 0){ return 0;//not adc pin } - if(channel > 7){ + if(channel > 9){ while (GET_PERI_REG_MASK(SENS_SAR_MEAS2_CTRL2_REG, SENS_MEAS2_DONE_SAR) == 0); //wait for conversion value = GET_PERI_REG_BITS2(SENS_SAR_MEAS2_CTRL2_REG, SENS_MEAS2_DATA_SAR, SENS_MEAS2_DATA_SAR_S); } else { @@ -223,9 +222,8 @@ uint16_t IRAM_ATTR adcRead(uint8_t pin) __analogInit(); if(channel > 9){ - channel -= 10; CLEAR_PERI_REG_MASK(SENS_SAR_MEAS2_CTRL2_REG, SENS_MEAS2_START_SAR_M); - SET_PERI_REG_BITS(SENS_SAR_MEAS2_CTRL2_REG, SENS_SAR2_EN_PAD, (1 << channel), SENS_SAR2_EN_PAD_S); + SET_PERI_REG_BITS(SENS_SAR_MEAS2_CTRL2_REG, SENS_SAR2_EN_PAD,(1 << (channel - 10)), SENS_SAR2_EN_PAD_S); SET_PERI_REG_MASK(SENS_SAR_MEAS2_CTRL2_REG, SENS_MEAS2_START_SAR_M); } else { CLEAR_PERI_REG_MASK(SENS_SAR_MEAS1_CTRL2_REG, SENS_MEAS1_START_SAR_M); @@ -235,7 +233,7 @@ uint16_t IRAM_ATTR adcRead(uint8_t pin) uint16_t value = 0; - if(channel > 7){ + if(channel > 9){ while (GET_PERI_REG_MASK(SENS_SAR_MEAS2_CTRL2_REG, SENS_MEAS2_DONE_SAR) == 0); //wait for conversion value = GET_PERI_REG_BITS2(SENS_SAR_MEAS2_CTRL2_REG, SENS_MEAS2_DATA_SAR, SENS_MEAS2_DATA_SAR_S); } else { From 0b367c37f772edf067467c6098f853d39f314c8e Mon Sep 17 00:00:00 2001 From: askuric Date: Fri, 21 Jun 2024 19:16:55 +0200 Subject: [PATCH 33/57] refactured esp32 fast adc driver - simplified a lot --- .../esp32/esp32_adc_driver.cpp | 285 ++++++------------ .../esp32/esp32_adc_driver.h | 90 +----- .../esp32/esp32_mcpwm_mcu.cpp | 41 ++- .../esp32/esp32s_adc_driver.cpp | 256 ---------------- 4 files changed, 139 insertions(+), 533 deletions(-) delete mode 100644 src/current_sense/hardware_specific/esp32/esp32s_adc_driver.cpp diff --git a/src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp b/src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp index 7f0cc310..5ad01b2a 100644 --- a/src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp +++ b/src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp @@ -1,89 +1,20 @@ #include "esp32_adc_driver.h" -#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(CONFIG_IDF_TARGET_ESP32S2) && !defined(CONFIG_IDF_TARGET_ESP32S3) && !defined(SIMPLEFOC_ESP32_USELEDC) - -#include "freertos/FreeRTOS.h" -#include "freertos/task.h" -#include "rom/ets_sys.h" -#include "esp_attr.h" -//#include "esp_intr.h" deprecated -#include "esp_intr_alloc.h" -#include "soc/rtc_io_reg.h" -#include "soc/rtc_cntl_reg.h" -#include "soc/sens_reg.h" - -static uint8_t __analogAttenuation = 3;//11db -static uint8_t __analogWidth = 3;//12 bits -static uint8_t __analogCycles = 8; -static uint8_t __analogSamples = 0;//1 sample -static uint8_t __analogClockDiv = 1; - -// Width of returned answer () -static uint8_t __analogReturnedWidth = 12; - -void __analogSetWidth(uint8_t bits){ - if(bits < 9){ - bits = 9; - } else if(bits > 12){ - bits = 12; - } - __analogReturnedWidth = bits; - __analogWidth = bits - 9; - SET_PERI_REG_BITS(SENS_SAR_START_FORCE_REG, SENS_SAR1_BIT_WIDTH, __analogWidth, SENS_SAR1_BIT_WIDTH_S); - SET_PERI_REG_BITS(SENS_SAR_READ_CTRL_REG, SENS_SAR1_SAMPLE_BIT, __analogWidth, SENS_SAR1_SAMPLE_BIT_S); - - SET_PERI_REG_BITS(SENS_SAR_START_FORCE_REG, SENS_SAR2_BIT_WIDTH, __analogWidth, SENS_SAR2_BIT_WIDTH_S); - SET_PERI_REG_BITS(SENS_SAR_READ_CTRL2_REG, SENS_SAR2_SAMPLE_BIT, __analogWidth, SENS_SAR2_SAMPLE_BIT_S); -} -void __analogSetCycles(uint8_t cycles){ - __analogCycles = cycles; - SET_PERI_REG_BITS(SENS_SAR_READ_CTRL_REG, SENS_SAR1_SAMPLE_CYCLE, __analogCycles, SENS_SAR1_SAMPLE_CYCLE_S); - SET_PERI_REG_BITS(SENS_SAR_READ_CTRL2_REG, SENS_SAR2_SAMPLE_CYCLE, __analogCycles, SENS_SAR2_SAMPLE_CYCLE_S); -} +// maybe go to the fast ADC in future even outside the MCPWM (we'd have to remove the SOC_MCPWM_SUPPORTED flag) +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(SIMPLEFOC_ESP32_USELEDC) -void __analogSetSamples(uint8_t samples){ - if(!samples){ - return; - } - __analogSamples = samples - 1; - SET_PERI_REG_BITS(SENS_SAR_READ_CTRL_REG, SENS_SAR1_SAMPLE_NUM, __analogSamples, SENS_SAR1_SAMPLE_NUM_S); - SET_PERI_REG_BITS(SENS_SAR_READ_CTRL2_REG, SENS_SAR2_SAMPLE_NUM, __analogSamples, SENS_SAR2_SAMPLE_NUM_S); -} -void __analogSetClockDiv(uint8_t clockDiv){ - if(!clockDiv){ - return; - } - __analogClockDiv = clockDiv; - SET_PERI_REG_BITS(SENS_SAR_READ_CTRL_REG, SENS_SAR1_CLK_DIV, __analogClockDiv, SENS_SAR1_CLK_DIV_S); - SET_PERI_REG_BITS(SENS_SAR_READ_CTRL2_REG, SENS_SAR2_CLK_DIV, __analogClockDiv, SENS_SAR2_CLK_DIV_S); -} +#ifdef CONFIG_IDF_TARGET_ESP32 // if esp32 variant -void __analogSetAttenuation(uint8_t attenuation) -{ - __analogAttenuation = attenuation & 3; - uint32_t att_data = 0; - int i = 10; - while(i--){ - att_data |= __analogAttenuation << (i * 2); - } - WRITE_PERI_REG(SENS_SAR_ATTEN1_REG, att_data & 0xFFFF);//ADC1 has 8 channels - WRITE_PERI_REG(SENS_SAR_ATTEN2_REG, att_data); -} - -void IRAM_ATTR __analogInit(){ - static bool initialized = false; - if(initialized){ - return; - } - - __analogSetAttenuation(__analogAttenuation); - __analogSetCycles(__analogCycles); - __analogSetSamples(__analogSamples + 1);//in samples - __analogSetClockDiv(__analogClockDiv); - __analogSetWidth(__analogWidth + 9);//in bits +#include "soc/sens_reg.h" +// configure the ADCs in RTC mode +// saves about 3us per call +// going from 12us to 9us +void __configFastADCs(){ + + // configure both ADCs in RTC mode SET_PERI_REG_MASK(SENS_SAR_READ_CTRL_REG, SENS_SAR1_DATA_INV); SET_PERI_REG_MASK(SENS_SAR_READ_CTRL2_REG, SENS_SAR2_DATA_INV); @@ -100,59 +31,17 @@ void IRAM_ATTR __analogInit(){ SET_PERI_REG_BITS(SENS_SAR_MEAS_WAIT1_REG, SENS_SAR_AMP_WAIT2, 0x1, SENS_SAR_AMP_WAIT2_S); SET_PERI_REG_BITS(SENS_SAR_MEAS_WAIT2_REG, SENS_SAR_AMP_WAIT3, 0x1, SENS_SAR_AMP_WAIT3_S); while (GET_PERI_REG_BITS2(SENS_SAR_SLAVE_ADDR1_REG, 0x7, SENS_MEAS_STATUS_S) != 0); //wait det_fsm== - - initialized = true; -} - -void __analogSetPinAttenuation(uint8_t pin, uint8_t attenuation) -{ - int8_t channel = digitalPinToAnalogChannel(pin); - if(channel < 0 || attenuation > 3){ - return ; - } - __analogInit(); - if(channel > 7){ - SET_PERI_REG_BITS(SENS_SAR_ATTEN2_REG, 3, attenuation, ((channel - 10) * 2)); - } else { - SET_PERI_REG_BITS(SENS_SAR_ATTEN1_REG, 3, attenuation, (channel * 2)); - } } -bool IRAM_ATTR __adcAttachPin(uint8_t pin){ - - int8_t channel = digitalPinToAnalogChannel(pin); - if(channel < 0){ - return false;//not adc pin - } - - int8_t pad = digitalPinToTouchChannel(pin); - if(pad >= 0){ - uint32_t touch = READ_PERI_REG(SENS_SAR_TOUCH_ENABLE_REG); - if(touch & (1 << pad)){ - touch &= ~((1 << (pad + SENS_TOUCH_PAD_OUTEN2_S)) - | (1 << (pad + SENS_TOUCH_PAD_OUTEN1_S)) - | (1 << (pad + SENS_TOUCH_PAD_WORKEN_S))); - WRITE_PERI_REG(SENS_SAR_TOUCH_ENABLE_REG, touch); - } - } else if(pin == 25){ - CLEAR_PERI_REG_MASK(RTC_IO_PAD_DAC1_REG, RTC_IO_PDAC1_XPD_DAC | RTC_IO_PDAC1_DAC_XPD_FORCE); //stop dac1 - } else if(pin == 26){ - CLEAR_PERI_REG_MASK(RTC_IO_PAD_DAC2_REG, RTC_IO_PDAC2_XPD_DAC | RTC_IO_PDAC2_DAC_XPD_FORCE); //stop dac2 - } - - pinMode(pin, ANALOG); - - __analogInit(); - return true; -} - -bool IRAM_ATTR __adcStart(uint8_t pin){ +uint16_t IRAM_ATTR adcRead(uint8_t pin) +{ int8_t channel = digitalPinToAnalogChannel(pin); if(channel < 0){ return false;//not adc pin } + // start teh ADC conversion if(channel > 9){ CLEAR_PERI_REG_MASK(SENS_SAR_MEAS_START2_REG, SENS_MEAS2_START_SAR_M); SET_PERI_REG_BITS(SENS_SAR_MEAS_START2_REG, SENS_SAR2_EN_PAD, (1 << (channel - 10)), SENS_SAR2_EN_PAD_S); @@ -162,57 +51,51 @@ bool IRAM_ATTR __adcStart(uint8_t pin){ SET_PERI_REG_BITS(SENS_SAR_MEAS_START1_REG, SENS_SAR1_EN_PAD, (1 << channel), SENS_SAR1_EN_PAD_S); SET_PERI_REG_MASK(SENS_SAR_MEAS_START1_REG, SENS_MEAS1_START_SAR_M); } - return true; -} - -bool IRAM_ATTR __adcBusy(uint8_t pin){ - - int8_t channel = digitalPinToAnalogChannel(pin); - if(channel < 0){ - return false;//not adc pin - } - - if(channel > 7){ - return (GET_PERI_REG_MASK(SENS_SAR_MEAS_START2_REG, SENS_MEAS2_DONE_SAR) == 0); - } - return (GET_PERI_REG_MASK(SENS_SAR_MEAS_START1_REG, SENS_MEAS1_DONE_SAR) == 0); -} - -uint16_t IRAM_ATTR __adcEnd(uint8_t pin) -{ uint16_t value = 0; - int8_t channel = digitalPinToAnalogChannel(pin); - if(channel < 0){ - return 0;//not adc pin - } + if(channel > 7){ - while (GET_PERI_REG_MASK(SENS_SAR_MEAS_START2_REG, SENS_MEAS2_DONE_SAR) == 0); //wait for conversion + //wait for conversion + while (GET_PERI_REG_MASK(SENS_SAR_MEAS_START2_REG, SENS_MEAS2_DONE_SAR) == 0); + // read the value value = GET_PERI_REG_BITS2(SENS_SAR_MEAS_START2_REG, SENS_MEAS2_DATA_SAR, SENS_MEAS2_DATA_SAR_S); } else { - while (GET_PERI_REG_MASK(SENS_SAR_MEAS_START1_REG, SENS_MEAS1_DONE_SAR) == 0); //wait for conversion + //wait for conversion + while (GET_PERI_REG_MASK(SENS_SAR_MEAS_START1_REG, SENS_MEAS1_DONE_SAR) == 0); + // read the value value = GET_PERI_REG_BITS2(SENS_SAR_MEAS_START1_REG, SENS_MEAS1_DATA_SAR, SENS_MEAS1_DATA_SAR_S); } - // Shift result if necessary - uint8_t from = __analogWidth + 9; - if (from == __analogReturnedWidth) { - return value; - } - if (from > __analogReturnedWidth) { - return value >> (from - __analogReturnedWidth); - } - return value << (__analogReturnedWidth - from); + // return value + return value; } -void __analogReadResolution(uint8_t bits) -{ - if(!bits || bits > 16){ - return; - } - __analogSetWidth(bits); // hadware from 9 to 12 - __analogReturnedWidth = bits; // software from 1 to 16 -} +#elif (defined(CONFIG_IDF_TARGET_ESP32S2) || defined(CONFIG_IDF_TARGET_ESP32S3)) // if esp32 s2 or s3 variants + +#include "soc/sens_reg.h" + + +// configure the ADCs in RTC mode +// no real gain +// void __configFastADCs(){ + +// SET_PERI_REG_MASK(SENS_SAR_READER1_CTRL_REG, SENS_SAR1_DATA_INV); +// SET_PERI_REG_MASK(SENS_SAR_READER2_CTRL_REG, SENS_SAR2_DATA_INV); + +// SET_PERI_REG_MASK(SENS_SAR_MEAS1_CTRL2_REG, SENS_MEAS1_START_FORCE_M); //SAR ADC1 controller (in RTC) is started by SW +// SET_PERI_REG_MASK(SENS_SAR_MEAS1_CTRL2_REG, SENS_SAR1_EN_PAD_FORCE_M); //SAR ADC1 pad enable bitmap is controlled by SW +// SET_PERI_REG_MASK(SENS_SAR_MEAS2_CTRL2_REG, SENS_MEAS2_START_FORCE_M); //SAR ADC2 controller (in RTC) is started by SW +// SET_PERI_REG_MASK(SENS_SAR_MEAS2_CTRL2_REG, SENS_SAR2_EN_PAD_FORCE_M); //SAR ADC2 pad enable bitmap is controlled by SW + +// CLEAR_PERI_REG_MASK(SENS_SAR_POWER_XPD_SAR_REG, SENS_FORCE_XPD_SAR_M); //force XPD_SAR=0, use XPD_FSM +// SET_PERI_REG_BITS(SENS_SAR_POWER_XPD_SAR_REG, SENS_FORCE_XPD_AMP, 0x2, SENS_FORCE_XPD_AMP_S); //force XPD_AMP=0 + +// CLEAR_PERI_REG_MASK(SENS_SAR_AMP_CTRL3_REG, 0xfff << SENS_AMP_RST_FB_FSM_S); //clear FSM +// SET_PERI_REG_BITS(SENS_SAR_AMP_CTRL1_REG, SENS_SAR_AMP_WAIT1, 0x1, SENS_SAR_AMP_WAIT1_S); +// SET_PERI_REG_BITS(SENS_SAR_AMP_CTRL1_REG, SENS_SAR_AMP_WAIT2, 0x1, SENS_SAR_AMP_WAIT2_S); +// SET_PERI_REG_BITS(SENS_SAR_POWER_XPD_SAR_REG, SENS_SAR_AMP_WAIT3, 0x1, SENS_SAR_AMP_WAIT3_S); +// while (GET_PERI_REG_BITS2(SENS_SAR_SLAVE_ADDR1_REG, 0x7, SENS_SARADC_MEAS_STATUS_S) != 0); //wait det_fsm== +// } uint16_t IRAM_ATTR adcRead(uint8_t pin) { @@ -221,38 +104,66 @@ uint16_t IRAM_ATTR adcRead(uint8_t pin) return false;//not adc pin } - __analogInit(); - + // start the ADC conversion if(channel > 9){ - CLEAR_PERI_REG_MASK(SENS_SAR_MEAS_START2_REG, SENS_MEAS2_START_SAR_M); - SET_PERI_REG_BITS(SENS_SAR_MEAS_START2_REG, SENS_SAR2_EN_PAD, (1 << (channel - 10)), SENS_SAR2_EN_PAD_S); - SET_PERI_REG_MASK(SENS_SAR_MEAS_START2_REG, SENS_MEAS2_START_SAR_M); + CLEAR_PERI_REG_MASK(SENS_SAR_MEAS2_CTRL2_REG, SENS_MEAS2_START_SAR_M); + SET_PERI_REG_BITS(SENS_SAR_MEAS2_CTRL2_REG, SENS_SAR2_EN_PAD, (1 << (channel - 10)), SENS_SAR2_EN_PAD_S); + SET_PERI_REG_MASK(SENS_SAR_MEAS2_CTRL2_REG, SENS_MEAS2_START_SAR_M); } else { - CLEAR_PERI_REG_MASK(SENS_SAR_MEAS_START1_REG, SENS_MEAS1_START_SAR_M); - SET_PERI_REG_BITS(SENS_SAR_MEAS_START1_REG, SENS_SAR1_EN_PAD, (1 << channel), SENS_SAR1_EN_PAD_S); - SET_PERI_REG_MASK(SENS_SAR_MEAS_START1_REG, SENS_MEAS1_START_SAR_M); + CLEAR_PERI_REG_MASK(SENS_SAR_MEAS1_CTRL2_REG, SENS_MEAS1_START_SAR_M); + SET_PERI_REG_BITS(SENS_SAR_MEAS1_CTRL2_REG, SENS_SAR1_EN_PAD, (1 << channel), SENS_SAR1_EN_PAD_S); + SET_PERI_REG_MASK(SENS_SAR_MEAS1_CTRL2_REG, SENS_MEAS1_START_SAR_M); } uint16_t value = 0; - if(channel > 7){ - while (GET_PERI_REG_MASK(SENS_SAR_MEAS_START2_REG, SENS_MEAS2_DONE_SAR) == 0); //wait for conversion - value = GET_PERI_REG_BITS2(SENS_SAR_MEAS_START2_REG, SENS_MEAS2_DATA_SAR, SENS_MEAS2_DATA_SAR_S); + if(channel > 9){ + //wait for conversion + while (GET_PERI_REG_MASK(SENS_SAR_MEAS2_CTRL2_REG, SENS_MEAS2_DONE_SAR) == 0); + // read the value + value = GET_PERI_REG_BITS2(SENS_SAR_MEAS2_CTRL2_REG, SENS_MEAS2_DATA_SAR, SENS_MEAS2_DATA_SAR_S); } else { - while (GET_PERI_REG_MASK(SENS_SAR_MEAS_START1_REG, SENS_MEAS1_DONE_SAR) == 0); //wait for conversion - value = GET_PERI_REG_BITS2(SENS_SAR_MEAS_START1_REG, SENS_MEAS1_DATA_SAR, SENS_MEAS1_DATA_SAR_S); + //wait for conversion + while (GET_PERI_REG_MASK(SENS_SAR_MEAS1_CTRL2_REG, SENS_MEAS1_DONE_SAR) == 0); + // read teh value + value = GET_PERI_REG_BITS2(SENS_SAR_MEAS1_CTRL2_REG, SENS_MEAS1_DATA_SAR, SENS_MEAS1_DATA_SAR_S); } - // Shift result if necessary - uint8_t from = __analogWidth + 9; - if (from == __analogReturnedWidth) { - return value; - } - if (from > __analogReturnedWidth) { - return value >> (from - __analogReturnedWidth); - } - return value << (__analogReturnedWidth - from); + return value; } +#else // if others just use analogRead + +uint16_t IRAM_ATTR adcRead(uint8_t pin){ + return analogRead(pin); +} + +#endif + + +// configure the ADC for the pin +bool IRAM_ATTR adcInit(uint8_t pin){ + static bool initialized = false; + + int8_t channel = digitalPinToAnalogChannel(pin); + if(channel < 0){ + return false;//not adc pin + } + + if(! initialized){ + analogSetAttenuation(SIMPLEFOC_ADC_ATTEN); + analogReadResolution(SIMPLEFOC_ADC_RES); + } + pinMode(pin, ANALOG); + analogSetPinAttenuation(pin, SIMPLEFOC_ADC_ATTEN); + analogRead(pin); + +#ifdef CONFIG_IDF_TARGET_ESP32 // if esp32 variant + __configFastADCs(); +#endif + + initialized = true; + return true; +} #endif diff --git a/src/current_sense/hardware_specific/esp32/esp32_adc_driver.h b/src/current_sense/hardware_specific/esp32/esp32_adc_driver.h index f76c003e..9a91d568 100644 --- a/src/current_sense/hardware_specific/esp32/esp32_adc_driver.h +++ b/src/current_sense/hardware_specific/esp32/esp32_adc_driver.h @@ -1,88 +1,30 @@ - - #ifndef SIMPLEFOC_ESP32_HAL_ADC_DRIVER_H_ #define SIMPLEFOC_ESP32_HAL_ADC_DRIVER_H_ #include "Arduino.h" -#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(SIMPLEFOC_ESP32_USELEDC) -/* - * Get ADC value for pin - * */ -uint16_t adcRead(uint8_t pin); - -/* - * Set the resolution of analogRead return values. Default is 12 bits (range from 0 to 4096). - * If between 9 and 12, it will equal the set hardware resolution, else value will be shifted. - * Range is 1 - 16 - * - * Note: compatibility with Arduino SAM - */ -void __analogReadResolution(uint8_t bits); - -/* - * Sets the sample bits and read resolution - * Default is 12bit (0 - 4095) - * Range is 9 - 12 - * */ -void __analogSetWidth(uint8_t bits); - -/* - * Set number of cycles per sample - * Default is 8 and seems to do well - * Range is 1 - 255 - * */ -void __analogSetCycles(uint8_t cycles); - -/* - * Set number of samples in the range. - * Default is 1 - * Range is 1 - 255 - * This setting splits the range into - * "samples" pieces, which could look - * like the sensitivity has been multiplied - * that many times - * */ -void __analogSetSamples(uint8_t samples); - -/* - * Set the divider for the ADC clock. - * Default is 1 - * Range is 1 - 255 - * */ -void __analogSetClockDiv(uint8_t clockDiv); +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(SIMPLEFOC_ESP32_USELEDC) -/* - * Set the attenuation for all channels - * Default is 11db - * */ -void __analogSetAttenuation(uint8_t attenuation); - -/* - * Set the attenuation for particular pin - * Default is 11db - * */ -void __analogSetPinAttenuation(uint8_t pin, uint8_t attenuation); -/* - * Attach pin to ADC (will also clear any other analog mode that could be on) - * */ -bool __adcAttachPin(uint8_t pin); +#define SIMPLEFOC_ADC_ATTEN ADC_11db +#define SIMPLEFOC_ADC_RES 12 -/* - * Start ADC conversion on attached pin's bus +/** + * Get ADC value for pin + * @param pin - pin number + * @return ADC value (0 - 4095) * */ -bool __adcStart(uint8_t pin); +uint16_t adcRead(uint8_t pin); -/* - * Check if conversion on the pin's ADC bus is currently running - * */ -bool __adcBusy(uint8_t pin); +/** + * Initialize ADC pin + * @param pin - pin number + * + * @return true if success + * false if pin is not an ADC pin + */ +bool adcInit(uint8_t pin); -/* - * Get the result of the conversion (will wait if it have not finished) - * */ -uint16_t __adcEnd(uint8_t pin); #endif /* SIMPLEFOC_ESP32_HAL_ADC_DRIVER_H_ */ #endif /* ESP32 */ \ No newline at end of file diff --git a/src/current_sense/hardware_specific/esp32/esp32_mcpwm_mcu.cpp b/src/current_sense/hardware_specific/esp32/esp32_mcpwm_mcu.cpp index 49f549dc..6760de23 100644 --- a/src/current_sense/hardware_specific/esp32/esp32_mcpwm_mcu.cpp +++ b/src/current_sense/hardware_specific/esp32/esp32_mcpwm_mcu.cpp @@ -73,16 +73,23 @@ float _readADCVoltageInline(const int pinA, const void* cs_params){ // function reading an ADC value and returning the read voltage void* _configureADCInline(const void* driver_params, const int pinA, const int pinB, const int pinC){ - - if( _isset(pinA) ) pinMode(pinA, INPUT); - if( _isset(pinB) ) pinMode(pinB, INPUT); - if( _isset(pinC) ) pinMode(pinC, INPUT); - ESP32MCPWMCurrentSenseParams* params = new ESP32MCPWMCurrentSenseParams { .pins = { pinA, pinB, pinC }, .adc_voltage_conv = (_ADC_VOLTAGE)/(_ADC_RESOLUTION) }; + // initialize the ADC pins + // fail if the pin is not an ADC pin + for (int i = 0; i < 3; i++){ + if(_isset(params->pins[i])){ + pinMode(params->pins[i], ANALOG); + if(!adcInit(params->pins[i])) { + SIMPLEFOC_ESP32_CS_DEBUG("Failed to initialise ADC pin: "+String(params->pins[i]) + String(", maybe not an ADC pin?")); + return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; + } + } + } + return params; } @@ -123,19 +130,21 @@ void* _configureADCLowSide(const void* driver_params, const int pinA,const int p return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; } + ESP32MCPWMCurrentSenseParams* params = new ESP32MCPWMCurrentSenseParams{}; int no_adc_channels = 0; - if( _isset(pinA) ){ - pinMode(pinA, INPUT); - params->pins[no_adc_channels++] = pinA; - } - if( _isset(pinB) ){ - pinMode(pinB, INPUT); - params->pins[no_adc_channels++] = pinB; - } - if( _isset(pinC) ){ - pinMode(pinC, INPUT); - params->pins[no_adc_channels++] = pinC; + + // initialize the ADC pins + // fail if the pin is not an ADC pin + int adc_pins[3] = {pinA, pinB, pinC}; + for (int i = 0; i < 3; i++){ + if(_isset(adc_pins[i])){ + if(!adcInit(adc_pins[i])){ + SIMPLEFOC_ESP32_CS_DEBUG("Failed to initialise ADC pin: "+String(adc_pins[i]) + String(", maybe not an ADC pin?")); + return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; + } + params->pins[no_adc_channels++] = adc_pins[i]; + } } t->user_data = params; diff --git a/src/current_sense/hardware_specific/esp32/esp32s_adc_driver.cpp b/src/current_sense/hardware_specific/esp32/esp32s_adc_driver.cpp deleted file mode 100644 index a8541568..00000000 --- a/src/current_sense/hardware_specific/esp32/esp32s_adc_driver.cpp +++ /dev/null @@ -1,256 +0,0 @@ -#include "esp32_adc_driver.h" - -#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && (defined(CONFIG_IDF_TARGET_ESP32S2) || defined(CONFIG_IDF_TARGET_ESP32S3)) && !defined(SIMPLEFOC_ESP32_USELEDC) - -#include "freertos/FreeRTOS.h" -#include "freertos/task.h" -#include "rom/ets_sys.h" -#include "esp_attr.h" -//#include "esp_intr.h" // deprecated -#include "esp_intr_alloc.h" -#include "soc/rtc_io_reg.h" -#include "soc/rtc_cntl_reg.h" -#include "soc/sens_reg.h" - -static uint8_t __analogAttenuation = 3;//11db -static uint8_t __analogWidth = 3;//12 bits -static uint8_t __analogCycles = 8; -static uint8_t __analogSamples = 0;//1 sample -static uint8_t __analogClockDiv = 1; - -// Width of returned answer () -static uint8_t __analogReturnedWidth = 12; - -void __analogSetWidth(uint8_t bits){ - if(bits < 9){ - bits = 9; - } else if(bits > 12){ - bits = 12; - } - __analogReturnedWidth = bits; - __analogWidth = bits - 9; - // SET_PERI_REG_BITS(SENS_SAR_START_FORCE_REG, SENS_SAR1_BIT_WIDTH, __analogWidth, SENS_SAR1_BIT_WIDTH_S); - // SET_PERI_REG_BITS(SENS_SAR_READER1_CTRL_REG, SENS_SAR1_SAMPLE_BIT, __analogWidth, SENS_SAR1_SAMPLE_BIT_S); - - // SET_PERI_REG_BITS(SENS_SAR_START_FORCE_REG, SENS_SAR2_BIT_WIDTH, __analogWidth, SENS_SAR2_BIT_WIDTH_S); - // SET_PERI_REG_BITS(SENS_SAR_READER2_CTRL_REG, SENS_SAR2_SAMPLE_BIT, __analogWidth, SENS_SAR2_SAMPLE_BIT_S); -} - -void __analogSetCycles(uint8_t cycles){ - __analogCycles = cycles; - // SET_PERI_REG_BITS(SENS_SAR_READER1_CTRL_REG, SENS_SAR1_SAMPLE_CYCLE, __analogCycles, SENS_SAR1_SAMPLE_CYCLE_S); - // SET_PERI_REG_BITS(SENS_SAR_READER2_CTRL_REG, SENS_SAR2_SAMPLE_CYCLE, __analogCycles, SENS_SAR2_SAMPLE_CYCLE_S); -} - -void __analogSetSamples(uint8_t samples){ - if(!samples){ - return; - } - __analogSamples = samples - 1; - SET_PERI_REG_BITS(SENS_SAR_READER1_CTRL_REG, SENS_SAR1_SAMPLE_NUM, __analogSamples, SENS_SAR1_SAMPLE_NUM_S); - SET_PERI_REG_BITS(SENS_SAR_READER2_CTRL_REG, SENS_SAR2_SAMPLE_NUM, __analogSamples, SENS_SAR2_SAMPLE_NUM_S); -} - -void __analogSetClockDiv(uint8_t clockDiv){ - if(!clockDiv){ - return; - } - __analogClockDiv = clockDiv; - SET_PERI_REG_BITS(SENS_SAR_READER1_CTRL_REG, SENS_SAR1_CLK_DIV, __analogClockDiv, SENS_SAR1_CLK_DIV_S); - SET_PERI_REG_BITS(SENS_SAR_READER2_CTRL_REG, SENS_SAR2_CLK_DIV, __analogClockDiv, SENS_SAR2_CLK_DIV_S); -} - -void __analogSetAttenuation(uint8_t attenuation) -{ - __analogAttenuation = attenuation & 3; - uint32_t att_data = 0; - int i = 10; - while(i--){ - att_data |= __analogAttenuation << (i * 2); - } - WRITE_PERI_REG(SENS_SAR_ATTEN1_REG, att_data & 0xFFFF);//ADC1 has 8 channels - WRITE_PERI_REG(SENS_SAR_ATTEN2_REG, att_data); -} - -void IRAM_ATTR __analogInit(){ - static bool initialized = false; - if(initialized){ - return; - } - - __analogSetAttenuation(__analogAttenuation); - __analogSetCycles(__analogCycles); - __analogSetSamples(__analogSamples + 1);//in samples - __analogSetClockDiv(__analogClockDiv); - __analogSetWidth(__analogWidth + 9);//in bits - - SET_PERI_REG_MASK(SENS_SAR_READER1_CTRL_REG, SENS_SAR1_DATA_INV); - SET_PERI_REG_MASK(SENS_SAR_READER2_CTRL_REG, SENS_SAR2_DATA_INV); - - SET_PERI_REG_MASK(SENS_SAR_MEAS1_CTRL2_REG, SENS_MEAS1_START_FORCE_M); //SAR ADC1 controller (in RTC) is started by SW - SET_PERI_REG_MASK(SENS_SAR_MEAS1_CTRL2_REG, SENS_SAR1_EN_PAD_FORCE_M); //SAR ADC1 pad enable bitmap is controlled by SW - SET_PERI_REG_MASK(SENS_SAR_MEAS2_CTRL2_REG, SENS_MEAS2_START_FORCE_M); //SAR ADC2 controller (in RTC) is started by SW - SET_PERI_REG_MASK(SENS_SAR_MEAS2_CTRL2_REG, SENS_SAR2_EN_PAD_FORCE_M); //SAR ADC2 pad enable bitmap is controlled by SW - - CLEAR_PERI_REG_MASK(SENS_SAR_POWER_XPD_SAR_REG, SENS_FORCE_XPD_SAR_M); //force XPD_SAR=0, use XPD_FSM - SET_PERI_REG_BITS(SENS_SAR_POWER_XPD_SAR_REG, SENS_FORCE_XPD_AMP, 0x2, SENS_FORCE_XPD_AMP_S); //force XPD_AMP=0 - - CLEAR_PERI_REG_MASK(SENS_SAR_AMP_CTRL3_REG, 0xfff << SENS_AMP_RST_FB_FSM_S); //clear FSM - SET_PERI_REG_BITS(SENS_SAR_AMP_CTRL1_REG, SENS_SAR_AMP_WAIT1, 0x1, SENS_SAR_AMP_WAIT1_S); - SET_PERI_REG_BITS(SENS_SAR_AMP_CTRL1_REG, SENS_SAR_AMP_WAIT2, 0x1, SENS_SAR_AMP_WAIT2_S); - SET_PERI_REG_BITS(SENS_SAR_POWER_XPD_SAR_REG, SENS_SAR_AMP_WAIT3, 0x1, SENS_SAR_AMP_WAIT3_S); - while (GET_PERI_REG_BITS2(SENS_SAR_SLAVE_ADDR1_REG, 0x7, SENS_SARADC_MEAS_STATUS_S) != 0); //wait det_fsm== - - initialized = true; -} - -void __analogSetPinAttenuation(uint8_t pin, uint8_t attenuation) -{ - int8_t channel = digitalPinToAnalogChannel(pin); - if(channel < 0 || attenuation > 3){ - return ; - } - __analogInit(); - if(channel > 9){ - SET_PERI_REG_BITS(SENS_SAR_ATTEN2_REG, 3, attenuation, ((channel - 10) * 2)); - } else { - SET_PERI_REG_BITS(SENS_SAR_ATTEN1_REG, 3, attenuation, (channel * 2)); - } -} - -bool IRAM_ATTR __adcAttachPin(uint8_t pin){ - - int8_t channel = digitalPinToAnalogChannel(pin); - if(channel < 0){ - return false;//not adc pin - } - - int8_t pad = digitalPinToTouchChannel(pin); - if(pad >= 0){ - uint32_t touch = READ_PERI_REG(SENS_SAR_TOUCH_CONF_REG); - if(touch & (1 << pad)){ - touch &= ~((1 << (pad + SENS_TOUCH_OUTEN_S))); - WRITE_PERI_REG(SENS_SAR_TOUCH_CONF_REG, touch); - } - } else if(pin == 25){ - CLEAR_PERI_REG_MASK(RTC_IO_PAD_DAC1_REG, RTC_IO_PDAC1_XPD_DAC | RTC_IO_PDAC1_DAC_XPD_FORCE); //stop dac1 - } else if(pin == 26){ - CLEAR_PERI_REG_MASK(RTC_IO_PAD_DAC2_REG, RTC_IO_PDAC2_XPD_DAC | RTC_IO_PDAC2_DAC_XPD_FORCE); //stop dac2 - } - - pinMode(pin, ANALOG); - - __analogInit(); - return true; -} - -bool IRAM_ATTR __adcStart(uint8_t pin){ - - int8_t channel = digitalPinToAnalogChannel(pin); - if(channel < 0){ - return false;//not adc pin - } - - if(channel > 9){ - CLEAR_PERI_REG_MASK(SENS_SAR_MEAS2_CTRL2_REG, SENS_MEAS2_START_SAR_M); - SET_PERI_REG_BITS(SENS_SAR_MEAS2_CTRL2_REG, SENS_SAR2_EN_PAD, (1 << (channel - 10)), SENS_SAR2_EN_PAD_S); - SET_PERI_REG_MASK(SENS_SAR_MEAS2_CTRL2_REG, SENS_MEAS2_START_SAR_M); - } else { - CLEAR_PERI_REG_MASK(SENS_SAR_MEAS1_CTRL2_REG, SENS_MEAS1_START_SAR_M); - SET_PERI_REG_BITS(SENS_SAR_MEAS1_CTRL2_REG, SENS_SAR1_EN_PAD, (1 << channel), SENS_SAR1_EN_PAD_S); - SET_PERI_REG_MASK(SENS_SAR_MEAS1_CTRL2_REG, SENS_MEAS1_START_SAR_M); - } - return true; -} - -bool IRAM_ATTR __adcBusy(uint8_t pin){ - - int8_t channel = digitalPinToAnalogChannel(pin); - if(channel < 0){ - return false;//not adc pin - } - - if(channel > 9){ - return (GET_PERI_REG_MASK(SENS_SAR_MEAS2_CTRL2_REG, SENS_MEAS2_DONE_SAR) == 0); - } - return (GET_PERI_REG_MASK(SENS_SAR_MEAS1_CTRL2_REG, SENS_MEAS1_DONE_SAR) == 0); -} - -uint16_t IRAM_ATTR __adcEnd(uint8_t pin) -{ - - uint16_t value = 0; - int8_t channel = digitalPinToAnalogChannel(pin); - if(channel < 0){ - return 0;//not adc pin - } - if(channel > 9){ - while (GET_PERI_REG_MASK(SENS_SAR_MEAS2_CTRL2_REG, SENS_MEAS2_DONE_SAR) == 0); //wait for conversion - value = GET_PERI_REG_BITS2(SENS_SAR_MEAS2_CTRL2_REG, SENS_MEAS2_DATA_SAR, SENS_MEAS2_DATA_SAR_S); - } else { - while (GET_PERI_REG_MASK(SENS_SAR_MEAS1_CTRL2_REG, SENS_MEAS1_DONE_SAR) == 0); //wait for conversion - value = GET_PERI_REG_BITS2(SENS_SAR_MEAS1_CTRL2_REG, SENS_MEAS1_DATA_SAR, SENS_MEAS1_DATA_SAR_S); - } - - // Shift result if necessary - uint8_t from = __analogWidth + 9; - if (from == __analogReturnedWidth) { - return value; - } - if (from > __analogReturnedWidth) { - return value >> (from - __analogReturnedWidth); - } - return value << (__analogReturnedWidth - from); -} - -void __analogReadResolution(uint8_t bits) -{ - if(!bits || bits > 16){ - return; - } - __analogSetWidth(bits); // hadware from 9 to 12 - __analogReturnedWidth = bits; // software from 1 to 16 -} - -uint16_t IRAM_ATTR adcRead(uint8_t pin) -{ - int8_t channel = digitalPinToAnalogChannel(pin); - if(channel < 0){ - return false;//not adc pin - } - - __analogInit(); - - if(channel > 9){ - CLEAR_PERI_REG_MASK(SENS_SAR_MEAS2_CTRL2_REG, SENS_MEAS2_START_SAR_M); - SET_PERI_REG_BITS(SENS_SAR_MEAS2_CTRL2_REG, SENS_SAR2_EN_PAD,(1 << (channel - 10)), SENS_SAR2_EN_PAD_S); - SET_PERI_REG_MASK(SENS_SAR_MEAS2_CTRL2_REG, SENS_MEAS2_START_SAR_M); - } else { - CLEAR_PERI_REG_MASK(SENS_SAR_MEAS1_CTRL2_REG, SENS_MEAS1_START_SAR_M); - SET_PERI_REG_BITS(SENS_SAR_MEAS1_CTRL2_REG, SENS_SAR1_EN_PAD, (1 << channel), SENS_SAR1_EN_PAD_S); - SET_PERI_REG_MASK(SENS_SAR_MEAS1_CTRL2_REG, SENS_MEAS1_START_SAR_M); - } - - uint16_t value = 0; - - if(channel > 9){ - while (GET_PERI_REG_MASK(SENS_SAR_MEAS2_CTRL2_REG, SENS_MEAS2_DONE_SAR) == 0); //wait for conversion - value = GET_PERI_REG_BITS2(SENS_SAR_MEAS2_CTRL2_REG, SENS_MEAS2_DATA_SAR, SENS_MEAS2_DATA_SAR_S); - } else { - while (GET_PERI_REG_MASK(SENS_SAR_MEAS1_CTRL2_REG, SENS_MEAS1_DONE_SAR) == 0); //wait for conversion - value = GET_PERI_REG_BITS2(SENS_SAR_MEAS1_CTRL2_REG, SENS_MEAS1_DATA_SAR, SENS_MEAS1_DATA_SAR_S); - } - - // Shift result if necessary - uint8_t from = __analogWidth + 9; - if (from == __analogReturnedWidth) { - return value; - } - if (from > __analogReturnedWidth) { - return value >> (from - __analogReturnedWidth); - } - return value << (__analogReturnedWidth - from); -} - - -#endif \ No newline at end of file From d89fa262ff40bdf9078f21d683ee05c4324e970f Mon Sep 17 00:00:00 2001 From: askuric Date: Fri, 21 Jun 2024 19:18:49 +0200 Subject: [PATCH 34/57] added the error debugging --- .../hardware_specific/esp32/esp32_mcpwm_mcu.cpp | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/src/current_sense/hardware_specific/esp32/esp32_mcpwm_mcu.cpp b/src/current_sense/hardware_specific/esp32/esp32_mcpwm_mcu.cpp index 6760de23..53426900 100644 --- a/src/current_sense/hardware_specific/esp32/esp32_mcpwm_mcu.cpp +++ b/src/current_sense/hardware_specific/esp32/esp32_mcpwm_mcu.cpp @@ -84,12 +84,12 @@ void* _configureADCInline(const void* driver_params, const int pinA, const int p if(_isset(params->pins[i])){ pinMode(params->pins[i], ANALOG); if(!adcInit(params->pins[i])) { - SIMPLEFOC_ESP32_CS_DEBUG("Failed to initialise ADC pin: "+String(params->pins[i]) + String(", maybe not an ADC pin?")); + SIMPLEFOC_ESP32_CS_DEBUG("ERROR: Failed to initialise ADC pin: "+String(params->pins[i]) + String(", maybe not an ADC pin?")); return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; } } } - + return params; } @@ -109,6 +109,7 @@ float _readADCVoltageLowSide(const int pin, const void* cs_params){ return p->adc_buffer[no_channel] * p->adc_voltage_conv; else no_channel++; } + SIMPLEFOC_DEBUG("ERROR: ADC pin not found in the buffer!"); // not found return 0; } @@ -126,7 +127,7 @@ void* _configureADCLowSide(const void* driver_params, const int pinA,const int p // check if low side callback is already set // if it is, return error if(t->on_full != nullptr){ - SIMPLEFOC_ESP32_CS_DEBUG("Low side callback is already set. Cannot set it again for timer: "+String(t->timer_id)+", group: "+String(t->group->group_id)); + SIMPLEFOC_ESP32_CS_DEBUG("ERROR: Low side callback is already set. Cannot set it again for timer: "+String(t->timer_id)+", group: "+String(t->group->group_id)); return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; } @@ -140,7 +141,7 @@ void* _configureADCLowSide(const void* driver_params, const int pinA,const int p for (int i = 0; i < 3; i++){ if(_isset(adc_pins[i])){ if(!adcInit(adc_pins[i])){ - SIMPLEFOC_ESP32_CS_DEBUG("Failed to initialise ADC pin: "+String(adc_pins[i]) + String(", maybe not an ADC pin?")); + SIMPLEFOC_ESP32_CS_DEBUG("ERROR: Failed to initialise ADC pin: "+String(adc_pins[i]) + String(", maybe not an ADC pin?")); return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; } params->pins[no_adc_channels++] = adc_pins[i]; @@ -165,7 +166,7 @@ void* _driverSyncLowSide(void* driver_params, void* cs_params){ // check if low side callback is already set // if it is, return error if(t->on_full != nullptr){ - SIMPLEFOC_ESP32_CS_DEBUG("Low side callback is already set. Cannot set it again for timer: "+String(t->timer_id)+", group: "+String(t->group->group_id)); + SIMPLEFOC_ESP32_CS_DEBUG("ERROR: Low side callback is already set. Cannot set it again for timer: "+String(t->timer_id)+", group: "+String(t->group->group_id)); return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; } From 710edb4aa584a46900205bf96d9df68e8f8e84c0 Mon Sep 17 00:00:00 2001 From: askuric Date: Fri, 21 Jun 2024 19:33:37 +0200 Subject: [PATCH 35/57] more refactoring of the current sensing --- .../esp32/esp32_adc_driver.cpp | 11 ++- .../esp32/esp32_adc_driver.h | 4 - .../esp32/esp32_ledc_mcu.cpp | 27 ------- .../esp32/esp32_mcpwm_mcu.cpp | 75 +++---------------- .../hardware_specific/esp32/esp32_mcu.cpp | 43 +++++++++++ .../hardware_specific/esp32/esp32_mcu.h | 37 +++++++++ 6 files changed, 98 insertions(+), 99 deletions(-) delete mode 100644 src/current_sense/hardware_specific/esp32/esp32_ledc_mcu.cpp create mode 100644 src/current_sense/hardware_specific/esp32/esp32_mcu.cpp create mode 100644 src/current_sense/hardware_specific/esp32/esp32_mcu.h diff --git a/src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp b/src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp index 5ad01b2a..649343f1 100644 --- a/src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp +++ b/src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp @@ -1,8 +1,10 @@ #include "esp32_adc_driver.h" +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) +#include "esp32_mcu.h" -// maybe go to the fast ADC in future even outside the MCPWM (we'd have to remove the SOC_MCPWM_SUPPORTED flag) -#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(SIMPLEFOC_ESP32_USELEDC) +#define SIMPLEFOC_ADC_ATTEN ADC_11db +#define SIMPLEFOC_ADC_RES 12 #ifdef CONFIG_IDF_TARGET_ESP32 // if esp32 variant @@ -38,6 +40,7 @@ uint16_t IRAM_ATTR adcRead(uint8_t pin) { int8_t channel = digitalPinToAnalogChannel(pin); if(channel < 0){ + SIMPLEFOC_ESP32_CS_DEBUG("ERROR: Not ADC pin: "+String(pin)); return false;//not adc pin } @@ -76,7 +79,7 @@ uint16_t IRAM_ATTR adcRead(uint8_t pin) // configure the ADCs in RTC mode -// no real gain +// no real gain - see if we do something with it later // void __configFastADCs(){ // SET_PERI_REG_MASK(SENS_SAR_READER1_CTRL_REG, SENS_SAR1_DATA_INV); @@ -101,6 +104,7 @@ uint16_t IRAM_ATTR adcRead(uint8_t pin) { int8_t channel = digitalPinToAnalogChannel(pin); if(channel < 0){ + SIMPLEFOC_ESP32_CS_DEBUG("ERROR: Not ADC pin: "+String(pin)); return false;//not adc pin } @@ -147,6 +151,7 @@ bool IRAM_ATTR adcInit(uint8_t pin){ int8_t channel = digitalPinToAnalogChannel(pin); if(channel < 0){ + SIMPLEFOC_ESP32_CS_DEBUG("ERROR: Not ADC pin: "+String(pin)); return false;//not adc pin } diff --git a/src/current_sense/hardware_specific/esp32/esp32_adc_driver.h b/src/current_sense/hardware_specific/esp32/esp32_adc_driver.h index 9a91d568..f41904d8 100644 --- a/src/current_sense/hardware_specific/esp32/esp32_adc_driver.h +++ b/src/current_sense/hardware_specific/esp32/esp32_adc_driver.h @@ -5,10 +5,6 @@ #if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(SIMPLEFOC_ESP32_USELEDC) - -#define SIMPLEFOC_ADC_ATTEN ADC_11db -#define SIMPLEFOC_ADC_RES 12 - /** * Get ADC value for pin * @param pin - pin number diff --git a/src/current_sense/hardware_specific/esp32/esp32_ledc_mcu.cpp b/src/current_sense/hardware_specific/esp32/esp32_ledc_mcu.cpp deleted file mode 100644 index 3487dbd7..00000000 --- a/src/current_sense/hardware_specific/esp32/esp32_ledc_mcu.cpp +++ /dev/null @@ -1,27 +0,0 @@ -#include "../../hardware_api.h" -#include "../../../drivers/hardware_api.h" - -#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && (!defined(SOC_MCPWM_SUPPORTED) || defined(SIMPLEFOC_ESP32_USELEDC)) - -#include "esp32_adc_driver.h" - -#define _ADC_VOLTAGE 3.3f -#define _ADC_RESOLUTION 4095.0f - -// function reading an ADC value and returning the read voltage -void* _configureADCInline(const void* driver_params, const int pinA,const int pinB,const int pinC){ - _UNUSED(driver_params); - - pinMode(pinA, INPUT); - pinMode(pinB, INPUT); - if( _isset(pinC) ) pinMode(pinC, INPUT); - - GenericCurrentSenseParams* params = new GenericCurrentSenseParams { - .pins = { pinA, pinB, pinC }, - .adc_voltage_conv = (_ADC_VOLTAGE)/(_ADC_RESOLUTION) - }; - - return params; -} - -#endif diff --git a/src/current_sense/hardware_specific/esp32/esp32_mcpwm_mcu.cpp b/src/current_sense/hardware_specific/esp32/esp32_mcpwm_mcu.cpp index 53426900..7efb6294 100644 --- a/src/current_sense/hardware_specific/esp32/esp32_mcpwm_mcu.cpp +++ b/src/current_sense/hardware_specific/esp32/esp32_mcpwm_mcu.cpp @@ -1,7 +1,5 @@ #include "../../hardware_api.h" #include "../../../drivers/hardware_api.h" -#include "../../../drivers/hardware_specific/esp32/esp32_driver_mcpwm.h" -#include "../../../drivers/hardware_specific/esp32/mcpwm_private.h" #if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(SIMPLEFOC_ESP32_USELEDC) @@ -12,22 +10,23 @@ #error SimpleFOC: ESP-IDF version 4 or lower detected. Please update to ESP-IDF 5.x and Arduino-esp32 3.0 (or higher) #endif - -#include "esp32_adc_driver.h" +#include "esp32_mcu.cpp" +#include "../../../drivers/hardware_specific/esp32/esp32_driver_mcpwm.h" +#include "../../../drivers/hardware_specific/esp32/mcpwm_private.h" #include "driver/mcpwm_prelude.h" #include "soc/mcpwm_reg.h" #include "soc/mcpwm_struct.h" -#include -#include -#define SIMPLEFOC_ESP32_INTERRUPT_DEBUG + +// adding a debug toggle pin to measure the time of the interrupt with oscilloscope + +// #define SIMPLEFOC_ESP32_INTERRUPT_DEBUG #ifdef SIMPLEFOC_ESP32_INTERRUPT_DEBUG #include "driver/gpio.h" - #ifdef CONFIG_IDF_TARGET_ESP32S3 #define DEBUGPIN 16 #define GPIO_NUM GPIO_NUM_16 @@ -38,60 +37,6 @@ #endif -#define _ADC_VOLTAGE 3.3f -#define _ADC_RESOLUTION 4095.0f - - - -#define SIMPLEFOC_ESP32_CS_DEBUG(str)\ - SIMPLEFOC_ESP32_DEBUG("CS", str);\ - -#define CHECK_CS_ERR(func_call, message) \ - if ((func_call) != ESP_OK) { \ - SIMPLEFOC_ESP32_CS_DEBUG("ERROR - " + String(message)); \ - return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; \ - } - -typedef struct ESP32MCPWMCurrentSenseParams { - int pins[3]; - float adc_voltage_conv; - int adc_buffer[3] = {}; - int buffer_index = 0; - int no_adc_channels = 0; -} ESP32MCPWMCurrentSenseParams; - - -/** - * Inline adc reading implementation -*/ -// function reading an ADC value and returning the read voltage -float _readADCVoltageInline(const int pinA, const void* cs_params){ - uint32_t raw_adc = adcRead(pinA); - return raw_adc * ((ESP32MCPWMCurrentSenseParams*)cs_params)->adc_voltage_conv; -} - -// function reading an ADC value and returning the read voltage -void* _configureADCInline(const void* driver_params, const int pinA, const int pinB, const int pinC){ - - ESP32MCPWMCurrentSenseParams* params = new ESP32MCPWMCurrentSenseParams { - .pins = { pinA, pinB, pinC }, - .adc_voltage_conv = (_ADC_VOLTAGE)/(_ADC_RESOLUTION) - }; - - // initialize the ADC pins - // fail if the pin is not an ADC pin - for (int i = 0; i < 3; i++){ - if(_isset(params->pins[i])){ - pinMode(params->pins[i], ANALOG); - if(!adcInit(params->pins[i])) { - SIMPLEFOC_ESP32_CS_DEBUG("ERROR: Failed to initialise ADC pin: "+String(params->pins[i]) + String(", maybe not an ADC pin?")); - return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; - } - } - } - - return params; -} /** @@ -101,7 +46,7 @@ void* _configureADCInline(const void* driver_params, const int pinA, const int p // function reading an ADC value and returning the read voltage float _readADCVoltageLowSide(const int pin, const void* cs_params){ - ESP32MCPWMCurrentSenseParams* p = (ESP32MCPWMCurrentSenseParams*)cs_params; + ESP32CurrentSenseParams* p = (ESP32CurrentSenseParams*)cs_params; int no_channel = 0; for(int i=0; i < 3; i++){ if(!_isset(p->pins[i])) continue; @@ -132,7 +77,7 @@ void* _configureADCLowSide(const void* driver_params, const int pinA,const int p } - ESP32MCPWMCurrentSenseParams* params = new ESP32MCPWMCurrentSenseParams{}; + ESP32CurrentSenseParams* params = new ESP32CurrentSenseParams{}; int no_adc_channels = 0; // initialize the ADC pins @@ -178,7 +123,7 @@ void* _driverSyncLowSide(void* driver_params, void* cs_params){ // - on_sync - sync event (not used with simplefoc) auto cbs = mcpwm_timer_event_callbacks_t{ .on_full = [](mcpwm_timer_handle_t tim, const mcpwm_timer_event_data_t* edata, void* user_data){ - ESP32MCPWMCurrentSenseParams *p = (ESP32MCPWMCurrentSenseParams*)user_data; + ESP32CurrentSenseParams *p = (ESP32CurrentSenseParams*)user_data; #ifdef SIMPLEFOC_ESP32_INTERRUPT_DEBUG // debugging toggle pin to measure the time of the interrupt with oscilloscope gpio_set_level(GPIO_NUM,1); //cca 250ns for on+off #endif diff --git a/src/current_sense/hardware_specific/esp32/esp32_mcu.cpp b/src/current_sense/hardware_specific/esp32/esp32_mcu.cpp new file mode 100644 index 00000000..9cf3a271 --- /dev/null +++ b/src/current_sense/hardware_specific/esp32/esp32_mcu.cpp @@ -0,0 +1,43 @@ +#include "../../hardware_api.h" +#include "../../../drivers/hardware_api.h" + +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) + +#include "esp32_adc_driver.h" +#include "esp32_mcu.h" + + +/** + * Inline adc reading implementation +*/ +// function reading an ADC value and returning the read voltage +float _readADCVoltageInline(const int pinA, const void* cs_params){ + uint32_t raw_adc = adcRead(pinA); + return raw_adc * ((ESP32CurrentSenseParams*)cs_params)->adc_voltage_conv; +} + +// function reading an ADC value and returning the read voltage +void* _configureADCInline(const void* driver_params, const int pinA, const int pinB, const int pinC){ + + ESP32CurrentSenseParams* params = new ESP32CurrentSenseParams { + .pins = { pinA, pinB, pinC }, + .adc_voltage_conv = (_ADC_VOLTAGE)/(_ADC_RESOLUTION) + }; + + // initialize the ADC pins + // fail if the pin is not an ADC pin + for (int i = 0; i < 3; i++){ + if(_isset(params->pins[i])){ + pinMode(params->pins[i], ANALOG); + if(!adcInit(params->pins[i])) { + SIMPLEFOC_ESP32_CS_DEBUG("ERROR: Failed to initialise ADC pin: "+String(params->pins[i]) + String(", maybe not an ADC pin?")); + return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; + } + } + } + + return params; +} + + +#endif diff --git a/src/current_sense/hardware_specific/esp32/esp32_mcu.h b/src/current_sense/hardware_specific/esp32/esp32_mcu.h new file mode 100644 index 00000000..8966d9ef --- /dev/null +++ b/src/current_sense/hardware_specific/esp32/esp32_mcu.h @@ -0,0 +1,37 @@ +#ifndef ESP32_MCU_CURRENT_SENSING_H +#define ESP32_MCU_CURRENT_SENSING_H + + +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) + +#include "../../hardware_api.h" +#include "../../../drivers/hardware_api.h" + +#include "esp32_adc_driver.h" + + +// esp32 current sense parameters +typedef struct ESP32CurrentSenseParams { + int pins[3]; + float adc_voltage_conv; + int adc_buffer[3] = {}; + int buffer_index = 0; + int no_adc_channels = 0; +} ESP32CurrentSenseParams; + +// macros for debugging wuing the simplefoc debug system +#define SIMPLEFOC_ESP32_CS_DEBUG(str)\ + SimpleFOCDebug::println( "ESP32-CS"+String(tag)+ ": "+ String(str));\ + +#define CHECK_CS_ERR(func_call, message) \ + if ((func_call) != ESP_OK) { \ + SIMPLEFOC_ESP32_CS_DEBUG("ERROR - " + String(message)); \ + return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; \ + } + + +#define _ADC_VOLTAGE 3.3f +#define _ADC_RESOLUTION 4095.0f + +#endif // ESP_H && ARDUINO_ARCH_ESP32 +#endif \ No newline at end of file From bc2f84815adf47ee08abe9741fe748756fd24f84 Mon Sep 17 00:00:00 2001 From: askuric Date: Fri, 21 Jun 2024 19:37:27 +0200 Subject: [PATCH 36/57] typo in debug macro --- src/current_sense/hardware_specific/esp32/esp32_mcu.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/current_sense/hardware_specific/esp32/esp32_mcu.h b/src/current_sense/hardware_specific/esp32/esp32_mcu.h index 8966d9ef..b50c3113 100644 --- a/src/current_sense/hardware_specific/esp32/esp32_mcu.h +++ b/src/current_sense/hardware_specific/esp32/esp32_mcu.h @@ -21,7 +21,7 @@ typedef struct ESP32CurrentSenseParams { // macros for debugging wuing the simplefoc debug system #define SIMPLEFOC_ESP32_CS_DEBUG(str)\ - SimpleFOCDebug::println( "ESP32-CS"+String(tag)+ ": "+ String(str));\ + SimpleFOCDebug::println( "ESP32-CS: "+ String(str));\ #define CHECK_CS_ERR(func_call, message) \ if ((func_call) != ESP_OK) { \ From a1bd20ef7ddca155ee224cdc76f81c105b90d59f Mon Sep 17 00:00:00 2001 From: askuric Date: Sat, 22 Jun 2024 12:35:09 +0200 Subject: [PATCH 37/57] say the pin name if the generator fails --- src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.cpp b/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.cpp index 4ab02993..ad1ff0b2 100644 --- a/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.cpp +++ b/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.cpp @@ -333,7 +333,7 @@ void* _configure6PWMPinsMCPWM(long pwm_frequency, int mcpwm_group, int timer_no, for (int i = 0; i < no_generators; i++) { generator_config.gen_gpio_num = pins[i]; int oper_index = (int)floor(i / 2); - CHECK_ERR(mcpwm_new_generator(params->oper[oper_index], &generator_config, ¶ms->generator[i]), "Could not create generator " + String(i)); + CHECK_ERR(mcpwm_new_generator(params->oper[oper_index], &generator_config, ¶ms->generator[i]),"Could not create generator " + String(i) +String(" on pin: ")+String(pins[i])); } SIMPLEFOC_ESP32_DRV_DEBUG("Configuring Center-Aligned 6 pwm."); @@ -464,7 +464,7 @@ void* _configurePinsMCPWM(long pwm_frequency, int mcpwm_group, int timer_no, int for (int i = 0; i < no_pins; i++) { generator_config.gen_gpio_num = pins[i]; int oper_index = shared_timer ? (int)floor((i + 1) / 2) : (int)floor(i / 2); - CHECK_ERR(mcpwm_new_generator(params->oper[oper_index], &generator_config, ¶ms->generator[i]), "Could not create generator " + String(i)); + CHECK_ERR(mcpwm_new_generator(params->oper[oper_index], &generator_config, ¶ms->generator[i]), "Could not create generator " + String(i) +String(" on pin: ")+String(pins[i])); } From 8ce92e0692ec13a5c981c9dc4dc35eb018ce7e73 Mon Sep 17 00:00:00 2001 From: askuric Date: Sat, 22 Jun 2024 12:36:01 +0200 Subject: [PATCH 38/57] restructured imports --- .../hardware_specific/esp32/esp32_adc_driver.cpp | 6 ++++-- .../hardware_specific/esp32/esp32_adc_driver.h | 2 -- .../hardware_specific/esp32/esp32_mcpwm_mcu.cpp | 4 +--- src/current_sense/hardware_specific/esp32/esp32_mcu.cpp | 7 +------ src/current_sense/hardware_specific/esp32/esp32_mcu.h | 4 ++-- 5 files changed, 8 insertions(+), 15 deletions(-) diff --git a/src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp b/src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp index 649343f1..b9594918 100644 --- a/src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp +++ b/src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp @@ -1,8 +1,8 @@ -#include "esp32_adc_driver.h" -#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) #include "esp32_mcu.h" +#include "esp32_adc_driver.h" +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) #define SIMPLEFOC_ADC_ATTEN ADC_11db #define SIMPLEFOC_ADC_RES 12 @@ -138,6 +138,8 @@ uint16_t IRAM_ATTR adcRead(uint8_t pin) #else // if others just use analogRead +#pragma message("SimpleFOC: Using analogRead for ADC reading, no fast ADC configuration available!") + uint16_t IRAM_ATTR adcRead(uint8_t pin){ return analogRead(pin); } diff --git a/src/current_sense/hardware_specific/esp32/esp32_adc_driver.h b/src/current_sense/hardware_specific/esp32/esp32_adc_driver.h index f41904d8..688b6d5a 100644 --- a/src/current_sense/hardware_specific/esp32/esp32_adc_driver.h +++ b/src/current_sense/hardware_specific/esp32/esp32_adc_driver.h @@ -1,8 +1,6 @@ #ifndef SIMPLEFOC_ESP32_HAL_ADC_DRIVER_H_ #define SIMPLEFOC_ESP32_HAL_ADC_DRIVER_H_ -#include "Arduino.h" - #if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(SIMPLEFOC_ESP32_USELEDC) /** diff --git a/src/current_sense/hardware_specific/esp32/esp32_mcpwm_mcu.cpp b/src/current_sense/hardware_specific/esp32/esp32_mcpwm_mcu.cpp index 7efb6294..33d547db 100644 --- a/src/current_sense/hardware_specific/esp32/esp32_mcpwm_mcu.cpp +++ b/src/current_sense/hardware_specific/esp32/esp32_mcpwm_mcu.cpp @@ -1,5 +1,4 @@ -#include "../../hardware_api.h" -#include "../../../drivers/hardware_api.h" +#include "esp32_mcu.h" #if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(SIMPLEFOC_ESP32_USELEDC) @@ -10,7 +9,6 @@ #error SimpleFOC: ESP-IDF version 4 or lower detected. Please update to ESP-IDF 5.x and Arduino-esp32 3.0 (or higher) #endif -#include "esp32_mcu.cpp" #include "../../../drivers/hardware_specific/esp32/esp32_driver_mcpwm.h" #include "../../../drivers/hardware_specific/esp32/mcpwm_private.h" diff --git a/src/current_sense/hardware_specific/esp32/esp32_mcu.cpp b/src/current_sense/hardware_specific/esp32/esp32_mcu.cpp index 9cf3a271..e5ed3fbf 100644 --- a/src/current_sense/hardware_specific/esp32/esp32_mcu.cpp +++ b/src/current_sense/hardware_specific/esp32/esp32_mcu.cpp @@ -1,11 +1,6 @@ -#include "../../hardware_api.h" -#include "../../../drivers/hardware_api.h" - -#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) - -#include "esp32_adc_driver.h" #include "esp32_mcu.h" +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) /** * Inline adc reading implementation diff --git a/src/current_sense/hardware_specific/esp32/esp32_mcu.h b/src/current_sense/hardware_specific/esp32/esp32_mcu.h index b50c3113..9207fb6a 100644 --- a/src/current_sense/hardware_specific/esp32/esp32_mcu.h +++ b/src/current_sense/hardware_specific/esp32/esp32_mcu.h @@ -1,12 +1,12 @@ #ifndef ESP32_MCU_CURRENT_SENSING_H #define ESP32_MCU_CURRENT_SENSING_H +#include "../../hardware_api.h" #if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) -#include "../../hardware_api.h" -#include "../../../drivers/hardware_api.h" +#include "../../../drivers/hardware_api.h" #include "esp32_adc_driver.h" From a734d0db08dea870364065c334318d46bfad37ff Mon Sep 17 00:00:00 2001 From: askuric Date: Sat, 22 Jun 2024 13:13:42 +0200 Subject: [PATCH 39/57] typo in the defines --- .../hardware_specific/esp32/esp32_adc_driver.cpp | 6 +++--- .../hardware_specific/esp32/esp32_adc_driver.h | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp b/src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp index b9594918..caf29c19 100644 --- a/src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp +++ b/src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp @@ -7,7 +7,7 @@ #define SIMPLEFOC_ADC_RES 12 -#ifdef CONFIG_IDF_TARGET_ESP32 // if esp32 variant +#if CONFIG_IDF_TARGET_ESP32 // if esp32 variant #include "soc/sens_reg.h" @@ -73,7 +73,7 @@ uint16_t IRAM_ATTR adcRead(uint8_t pin) return value; } -#elif (defined(CONFIG_IDF_TARGET_ESP32S2) || defined(CONFIG_IDF_TARGET_ESP32S3)) // if esp32 s2 or s3 variants +#elif CONFIG_IDF_TARGET_ESP32S2 || CONFIG_IDF_TARGET_ESP32S3 // if esp32 s2 or s3 variants #include "soc/sens_reg.h" @@ -165,7 +165,7 @@ bool IRAM_ATTR adcInit(uint8_t pin){ analogSetPinAttenuation(pin, SIMPLEFOC_ADC_ATTEN); analogRead(pin); -#ifdef CONFIG_IDF_TARGET_ESP32 // if esp32 variant +#if CONFIG_IDF_TARGET_ESP32 // if esp32 variant __configFastADCs(); #endif diff --git a/src/current_sense/hardware_specific/esp32/esp32_adc_driver.h b/src/current_sense/hardware_specific/esp32/esp32_adc_driver.h index 688b6d5a..cad441fc 100644 --- a/src/current_sense/hardware_specific/esp32/esp32_adc_driver.h +++ b/src/current_sense/hardware_specific/esp32/esp32_adc_driver.h @@ -1,7 +1,7 @@ #ifndef SIMPLEFOC_ESP32_HAL_ADC_DRIVER_H_ #define SIMPLEFOC_ESP32_HAL_ADC_DRIVER_H_ -#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(SIMPLEFOC_ESP32_USELEDC) +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) /** * Get ADC value for pin From e81af5dbdf97e49efc0893654d179e3b03ea27f8 Mon Sep 17 00:00:00 2001 From: dekutree64 <54822734+dekutree64@users.noreply.github.com> Date: Sat, 29 Jun 2024 18:22:52 -0500 Subject: [PATCH 40/57] Fix for #415 sin/cos integer overflow on 16-bit CPUs Terrible bug in the fast sin/cos added last year, resulting in rough sine waves and a spike from +1 to -1 at pi/2 --- src/common/foc_utils.cpp | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/src/common/foc_utils.cpp b/src/common/foc_utils.cpp index fa937d15..7ae372f7 100644 --- a/src/common/foc_utils.cpp +++ b/src/common/foc_utils.cpp @@ -9,20 +9,21 @@ __attribute__((weak)) float _sin(float a){ // 16 bit precision on sine value, 8 bit fractional value for interpolation, 6bit LUT size // resulting precision compared to stdlib sine is 0.00006480 (RMS difference in range -PI,PI for 3217 steps) static uint16_t sine_array[65] = {0,804,1608,2411,3212,4011,4808,5602,6393,7180,7962,8740,9512,10279,11039,11793,12540,13279,14010,14733,15447,16151,16846,17531,18205,18868,19520,20160,20788,21403,22006,22595,23170,23732,24279,24812,25330,25833,26320,26791,27246,27684,28106,28511,28899,29269,29622,29957,30274,30572,30853,31114,31357,31581,31786,31972,32138,32286,32413,32522,32610,32679,32729,32758,32768}; - unsigned int i = (unsigned int)(a * (64*4*256/_2PI)); - int t1, t2, frac = i & 0xff; + int32_t t1, t2; + unsigned int i = (unsigned int)(a * (64*4*256.0f/_2PI)); + int frac = i & 0xff; i = (i >> 8) & 0xff; if (i < 64) { - t1 = sine_array[i]; t2 = sine_array[i+1]; + t1 = (int32_t)sine_array[i]; t2 = (int32_t)sine_array[i+1]; } else if(i < 128) { - t1 = sine_array[128 - i]; t2 = sine_array[127 - i]; + t1 = (int32_t)sine_array[128 - i]; t2 = (int32_t)sine_array[127 - i]; } else if(i < 192) { - t1 = -sine_array[-128 + i]; t2 = -sine_array[-127 + i]; + t1 = -(int32_t)sine_array[-128 + i]; t2 = -(int32_t)sine_array[-127 + i]; } else { - t1 = -sine_array[256 - i]; t2 = -sine_array[255 - i]; + t1 = -(int32_t)sine_array[256 - i]; t2 = -(int32_t)sine_array[255 - i]; } return (1.0f/32768.0f) * (t1 + (((t2 - t1) * frac) >> 8)); } From bf75d206ad11d0c0e56f5074395e17f545702d69 Mon Sep 17 00:00:00 2001 From: dekutree64 <54822734+dekutree64@users.noreply.github.com> Date: Sat, 29 Jun 2024 18:32:51 -0500 Subject: [PATCH 41/57] Removed accidental addition of LinearHall.h I thought I discarded this change after moving LinearHall back to drivers, but somehow it got included anyway --- src/SimpleFOC.h | 1 - 1 file changed, 1 deletion(-) diff --git a/src/SimpleFOC.h b/src/SimpleFOC.h index 9e1c80c6..4e6815e5 100644 --- a/src/SimpleFOC.h +++ b/src/SimpleFOC.h @@ -104,7 +104,6 @@ void loop() { #include "sensors/MagneticSensorAnalog.h" #include "sensors/MagneticSensorPWM.h" #include "sensors/HallSensor.h" -#include "sensors/LinearHall.h" #include "sensors/GenericSensor.h" #include "drivers/BLDCDriver3PWM.h" #include "drivers/BLDCDriver6PWM.h" From d2a409338f9d796c4ff86ecad0f42b576d05e522 Mon Sep 17 00:00:00 2001 From: askuric Date: Sat, 13 Jul 2024 14:40:43 +0200 Subject: [PATCH 42/57] one too many electrical angle --- src/StepperMotor.cpp | 5 ----- 1 file changed, 5 deletions(-) diff --git a/src/StepperMotor.cpp b/src/StepperMotor.cpp index df710aad..38e8999f 100644 --- a/src/StepperMotor.cpp +++ b/src/StepperMotor.cpp @@ -301,11 +301,6 @@ void StepperMotor::loopFOC() { // if disabled do nothing if(!enabled) return; - // Needs the update() to be called first - // This function will not have numerical issues because it uses Sensor::getMechanicalAngle() - // which is in range 0-2PI - electrical_angle = electricalAngle(); - // Needs the update() to be called first // This function will not have numerical issues because it uses Sensor::getMechanicalAngle() // which is in range 0-2PI From eb4fd7f6ce2690f95d66e5644a4c9c04a3c179df Mon Sep 17 00:00:00 2001 From: askuric Date: Sat, 13 Jul 2024 14:41:35 +0200 Subject: [PATCH 43/57] update of pwm duty cycle on timer zero --- src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.cpp b/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.cpp index ad1ff0b2..a481c6ff 100644 --- a/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.cpp +++ b/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.cpp @@ -306,6 +306,7 @@ void* _configure6PWMPinsMCPWM(long pwm_frequency, int mcpwm_group, int timer_no, SIMPLEFOC_ESP32_DRV_DEBUG("Configuring " + String(no_operators) + " comparators."); // Create and configure comparators mcpwm_comparator_config_t comparator_config = {0}; + comparator_config.flags.update_cmp_on_tez = true; for (int i = 0; i < no_operators; i++) { CHECK_ERR(mcpwm_new_comparator(params->oper[i], &comparator_config, ¶ms->comparator[i]),"Could not create comparator: " + String(i)); CHECK_ERR(mcpwm_comparator_set_compare_value(params->comparator[i], (0)), "Could not set duty on comparator: " + String(i)); @@ -319,6 +320,7 @@ void* _configure6PWMPinsMCPWM(long pwm_frequency, int mcpwm_group, int timer_no, SIMPLEFOC_ESP32_DRV_DEBUG("Configuring " + String(no_pins) + " comparators."); // Create and configure comparators mcpwm_comparator_config_t comparator_config = {0}; + comparator_config.flags.update_cmp_on_tez = true; for (int i = 0; i < no_pins; i++) { int oper_index = (int)floor(i / 2); CHECK_ERR(mcpwm_new_comparator(params->oper[oper_index], &comparator_config, ¶ms->comparator[i]),"Could not create comparator: " + String(i)); @@ -452,6 +454,7 @@ void* _configurePinsMCPWM(long pwm_frequency, int mcpwm_group, int timer_no, int SIMPLEFOC_ESP32_DRV_DEBUG("Configuring " + String(no_pins) + " comparators."); // Create and configure comparators mcpwm_comparator_config_t comparator_config = {0}; + comparator_config.flags.update_cmp_on_tez = true; for (int i = 0; i < no_pins; i++) { int oper_index = shared_timer ? (int)floor((i + 1) / 2) : (int)floor(i / 2); CHECK_ERR(mcpwm_new_comparator(params->oper[oper_index], &comparator_config, ¶ms->comparator[i]),"Could not create comparator: " + String(i)); From 842b3615eec1b74cdb4729c03a2c64d21d7135d8 Mon Sep 17 00:00:00 2001 From: askuric Date: Sun, 14 Jul 2024 13:15:36 +0200 Subject: [PATCH 44/57] new current sense alignement --- src/common/base_classes/CurrentSense.cpp | 283 +++++++++++++++-------- 1 file changed, 187 insertions(+), 96 deletions(-) diff --git a/src/common/base_classes/CurrentSense.cpp b/src/common/base_classes/CurrentSense.cpp index 4e6c3386..e354dfaf 100644 --- a/src/common/base_classes/CurrentSense.cpp +++ b/src/common/base_classes/CurrentSense.cpp @@ -174,113 +174,204 @@ PhaseCurrent_s CurrentSense::readAverageCurrents(int N) { // 4 - success but pins reconfigured and gains inverted int CurrentSense::alignBLDCDriver(float voltage, BLDCDriver* bldc_driver){ - int exit_flag = 1; - if(_isset(pinA)){ - // set phase A active and phases B and C down - bldc_driver->setPwm(voltage, 0, 0); - _delay(500); - PhaseCurrent_s c = readAverageCurrents(); - bldc_driver->setPwm(0, 0, 0); - // align phase A - float ab_ratio = c.b ? fabs(c.a / c.b) : 0; - float ac_ratio = c.c ? fabs(c.a / c.c) : 0; - if(_isset(pinB) && ab_ratio > 1.5f ){ // should be ~2 - gain_a *= _sign(c.a); - }else if(_isset(pinC) && ac_ratio > 1.5f ){ // should be ~2 - gain_a *= _sign(c.a); - }else if(_isset(pinB) && ab_ratio < 0.7f ){ // should be ~0.5 - SIMPLEFOC_DEBUG("CS: Switch A-B"); - // switch phase A and B + bool phases_switched = 0; + bool phases_inverted = 0; + + + // set phase A active and phases B and C down + bldc_driver->setPwm(voltage, 0, 0); + _delay(500); + PhaseCurrent_s c_a = readAverageCurrents(); + // check if currents are to low (lower than 100mA) + // TODO calculate the 100mA threshold from the ADC resolution + // if yes throw an error and return 0 + // either the current sense is not connected or the current is + // too low for calibration purposes (one should raise the motor.voltage_sensor_align) + if((_isset(pinA) && fabs(c_a.a) < 0.1f) + || (_isset(pinB) && fabs(c_a.b) < 0.1f) + || (_isset(pinC) && fabs(c_a.c) < 0.1f)){ + SIMPLEFOC_DEBUG("CS: Err too low current, rise voltage!"); + return 0; // measurement current too low + } + + // set phase B active and phases A and C down + bldc_driver->setPwm(0, voltage, 0); + _delay(500); + PhaseCurrent_s c_b = readAverageCurrents(); + bldc_driver->setPwm(0, 0, 0); + + // now we have to determine + // 1) which pin correspond to which phase of the bldc driver + // 2) if the currents measured have good polarity + // + // > when we apply a voltage to a phase A of the driver what we expect to measure is the current I on the phase A + // and -I/2 on the phase B and I/2 on the phase C + + // find the highest magnitude in c_a + // and make sure it's around 2 (1.5 at least) times higher than the other two + float ca[3] = {fabs(c_a.a), fabs(c_a.b), fabs(c_a.c)}; + uint8_t max_i = -1; // max index + float max_c = 0; // max current + float max_c_ratio = 0; // max current ratio + for(int i = 0; i < 3; i++){ + if(!ca[i]) continue; // current not measured + if(ca[i] > max_c){ + max_c = ca[i]; + max_i = i; + for(int j = 0; j < 3; j++){ + if(i == j) continue; + if(!ca[j]) continue; // current not measured + float ratio = max_c / ca[j]; + if(ratio > max_c_ratio) max_c_ratio = ratio; + } + } + } + + // check the current magnitude ratios + // 1) if there is one current that is approximately 2 times higher than the other two + // this is the A current + // 2) if the max current is not at least 1.5 times higher than the other two + // we have two cases: + // - either we only measure two currents and the third one is not measured - then phase A is not measured + // - or the current sense is not connected properly + + if(max_c_ratio >=1.5f){ + switch (max_i){ + case 1: // phase B is the max current + SIMPLEFOC_DEBUG("CS: Switch A-B"); + // switch phase A and B + _swap(pinA, pinB); + _swap(offset_ia, offset_ib); + _swap(gain_a, gain_b); + _swap(c_a.b, c_a.b); + _swap(c_b.a, c_b.b); // for the next phase of alignment + phases_switched = true; // signal that pins have been switched + break; + case 2: // phase C is the max current + SIMPLEFOC_DEBUG("CS: Switch A-C"); + // switch phase A and C + _swap(pinA, pinC); + _swap(offset_ia, offset_ic); + _swap(gain_a, gain_c); + _swap(c_a.a, c_a.c); + _swap(c_b.a, c_b.c); // for the next phase of alignment + phases_switched = true;// signal that pins have been switched + break; + } + // check if the current is negative and invert the gain if so + if( _sign(c_a.a) < 0 ){ + SIMPLEFOC_DEBUG("CS: Inv A"); + gain_a *= -1; + phases_inverted = true; // signal that pins have been inverted + } + }else if(_isset(pinA) && _isset(pinB) && _isset(pinC)){ + // if all three currents are measured and none of them is significantly higher + // we have a problem with the current sense + SIMPLEFOC_DEBUG("CS: Err A - all currents same magnitude!"); + return 0; + }else{ //phase A is not measured so put the _NC to the phase A + if(_isset(pinA) && !_isset(pinB)){ + SIMPLEFOC_DEBUG("CS: Switch A-(B)NC"); _swap(pinA, pinB); _swap(offset_ia, offset_ib); _swap(gain_a, gain_b); - gain_a *= _sign(c.b); - exit_flag = 2; // signal that pins have been switched - }else if(_isset(pinC) && ac_ratio < 0.7f ){ // should be ~0.5 - SIMPLEFOC_DEBUG("CS: Switch A-C"); - // switch phase A and C + _swap(c_a.b, c_a.b); + _swap(c_b.a, c_b.b); // for the next phase of alignment + phases_switched = true; // signal that pins have been switched + }else if(_isset(pinA) && !_isset(pinC)){ + SIMPLEFOC_DEBUG("CS: Switch A-(C)NC"); _swap(pinA, pinC); _swap(offset_ia, offset_ic); _swap(gain_a, gain_c); - gain_a *= _sign(c.c); - exit_flag = 2;// signal that pins have been switched - }else{ - SIMPLEFOC_DEBUG("CS: Err read A"); - // error in current sense - phase either not measured or bad connection - return 0; + _swap(c_a.b, c_a.c); + _swap(c_b.a, c_b.c); // for the next phase of alignment + phases_switched = true; // signal that pins have been switched } } - - if(_isset(pinB)){ - // set phase B active and phases A and C down - bldc_driver->setPwm(0, voltage, 0); - _delay(500); - PhaseCurrent_s c = readAverageCurrents(); - bldc_driver->setPwm(0, 0, 0); - float ba_ratio = c.a ? fabs(c.b / c.a) : 0; - float bc_ratio = c.c ? fabs(c.b / c.c) : 0; - if(_isset(pinA) && ba_ratio > 1.5f ){ // should be ~2); - gain_b *= _sign(c.b); - }else if(_isset(pinC) && bc_ratio > 1.5f ){ // should be ~2 - gain_b *= _sign(c.b); - }else if(_isset(pinA) && ba_ratio < 0.7f ){ // it should be ~0.5 - SIMPLEFOC_DEBUG("CS: Switch B-A"); - // switch phase A and B - _swap(pinB, pinA); - _swap(offset_ib, offset_ia); - _swap(gain_b, gain_a); - gain_b *= _sign(c.a); - exit_flag = 2; // signal that pins have been switched - }else if(_isset(pinC) && bc_ratio < 0.7f ){ // should be ~0.5 - SIMPLEFOC_DEBUG("CS: Switch B-C"); + // at this point the current sensing on phase A can be either: + // - aligned with the driver phase A + // - or the phase A is not measured and the _NC is connected to the phase A + // + // In either case A is done, now we have to check the phase B and C + + // check the phase B + // find the highest magnitude in c_b + // and make sure it's around 2 (1.5 at least) times higher than the other two + float cb[3] = {fabs(c_b.a), fabs(c_b.b), fabs(c_b.c)}; + max_i = -1; // max index + max_c = 0; // max current + max_c_ratio = 0; // max current ratio + for(int i = 0; i < 3; i++){ + if(!cb[i]) continue; // current not measured + if(cb[i] > max_c){ + max_c = cb[i]; + max_i = i; + for(int j = 0; j < 3; j++){ + if(i == j) continue; + if(!cb[j]) continue; // current not measured + float ratio = max_c / cb[j]; + if(ratio > max_c_ratio) max_c_ratio = ratio; + } + } + } + if(max_c_ratio >= 1.5f){ + switch (max_i){ + case 0: // phase A is the max current + // this is an error as phase A is already aligned + SIMPLEFOC_DEBUG("CS: Err align B"); + return 0; + case 2: // phase C is the max current + SIMPLEFOC_DEBUG("CS: Switch B-C"); + _swap(pinB, pinC); + _swap(offset_ib, offset_ic); + _swap(gain_b, gain_c); + _swap(c_b.b, c_b.c); + phases_switched = true; // signal that pins have been switched + break; + } + // check if the current is negative and invert the gain if so + if( _sign(c_b.b) < 0 ){ + SIMPLEFOC_DEBUG("CS: Inv B"); + gain_b *= -1; + phases_inverted = true; // signal that pins have been inverted + } + }else if(_isset(pinB) && _isset(pinC)){ + // if all three currents are measured and none of them is significantly higher + // we have a problem with the current sense + SIMPLEFOC_DEBUG("CS: Err B - all currents same magnitude!"); + return 0; + }else{ //phase B is not measured so put the _NC to the phase B + if(_isset(pinB) && !_isset(pinC)){ + SIMPLEFOC_DEBUG("CS: Switch B-(C)NC"); _swap(pinB, pinC); _swap(offset_ib, offset_ic); _swap(gain_b, gain_c); - gain_b *= _sign(c.c); - exit_flag = 2; // signal that pins have been switched - }else{ - SIMPLEFOC_DEBUG("CS: Error read B"); - // error in current sense - phase either not measured or bad connection - return 0; - } + _swap(c_b.b, c_b.c); + phases_switched = true; // signal that pins have been switched + } } - - // if phase C measured + // at this point the current sensing on phase A and B can be either: + // - aligned with the driver phase A and B + // - or the phase A and B are not measured and the _NC is connected to the phase A and B + // + // In either case A and B is done, now we have to check the phase C + // phase C is also aligned if it is measured (not _NC) + // we have to check if the current is negative and invert the gain if so if(_isset(pinC)){ - // set phase C active and phases A and B down - bldc_driver->setPwm(0, 0, voltage); - _delay(500); - PhaseCurrent_s c = readAverageCurrents(); - bldc_driver->setPwm(0, 0, 0); - float ca_ratio = c.a ? fabs(c.c / c.a) : 0; - float cb_ratio = c.b ? fabs(c.c / c.b) : 0; - if(_isset(pinA) && ca_ratio > 1.5f ){ // should be ~2 - gain_c *= _sign(c.c); - }else if(_isset(pinB) && cb_ratio > 1.5f ){ // should be ~2 - gain_c *= _sign(c.c); - }else if(_isset(pinA) && ca_ratio < 0.7f ){ // it should be ~0.5 - SIMPLEFOC_DEBUG("CS: Switch C-A"); - // switch phase A and C - _swap(pinC, pinA); - _swap(offset_ic, offset_ia); - _swap(gain_c, gain_a); - gain_c *= _sign(c.a); - exit_flag = 2; // signal that pins have been switched - }else if(_isset(pinB) && cb_ratio < 0.7f ){ // should be ~0.5 - SIMPLEFOC_DEBUG("CS: Switch C-B"); - _swap(pinC, pinB); - _swap(offset_ic, offset_ib); - _swap(gain_c, gain_b); - gain_b *= _sign(c.b); - exit_flag = 2; // signal that pins have been switched - }else{ - SIMPLEFOC_DEBUG("CS: Err read C"); - // error in current sense - phase either not measured or bad connection - return 0; - } + if( _sign(c_b.c) > 0 ){ // the expected current is -I/2 (if the phase A and B are aligned and C has correct polarity) + SIMPLEFOC_DEBUG("CS: Inv C"); + gain_c *= -1; + phases_inverted = true; // signal that pins have been inverted + } } - // add 2 if pin gains negative - if(gain_a < 0 || gain_b < 0 || gain_c < 0) exit_flag +=2; + + // construct the return flag + // if the phases have been switched return 2 + // if the gains have been inverted return 3 + // if both return 4 + uint8_t exit_flag = 1; + if(phases_switched) exit_flag += 1; + if(phases_inverted) exit_flag += 2; return exit_flag; } @@ -303,7 +394,7 @@ int CurrentSense::alignStepperDriver(float voltage, StepperDriver* stepper_drive _delay(500); PhaseCurrent_s c = readAverageCurrents(); // disable the phases - stepper_driver->setPwm(0, 0); + stepper_driver->setPwm(0, 0); if (fabs(c.a) < 0.1f && fabs(c.b) < 0.1f ){ SIMPLEFOC_DEBUG("CS: Err too low current!"); return 0; // measurement current too low @@ -321,7 +412,7 @@ int CurrentSense::alignStepperDriver(float voltage, StepperDriver* stepper_drive gain_a *= _sign(c.b); exit_flag = 2; // signal that pins have been switched }else if (c.a < 0){ - SIMPLEFOC_DEBUG("CS: Neg A"); + SIMPLEFOC_DEBUG("CS: Inv A"); gain_a *= -1; } } @@ -339,7 +430,7 @@ int CurrentSense::alignStepperDriver(float voltage, StepperDriver* stepper_drive // align phase A // check if measured current a is positive and invert if not if (c.b < 0){ - SIMPLEFOC_DEBUG("CS: Neg B"); + SIMPLEFOC_DEBUG("CS: Inv B"); gain_b *= -1; } } From 780bda930ed51520ece4882a5cfa1babfd60c228 Mon Sep 17 00:00:00 2001 From: askuric Date: Mon, 15 Jul 2024 11:47:34 +0200 Subject: [PATCH 45/57] exit flag currection for steppres --- src/common/base_classes/CurrentSense.cpp | 38 +++++++++++++----------- 1 file changed, 21 insertions(+), 17 deletions(-) diff --git a/src/common/base_classes/CurrentSense.cpp b/src/common/base_classes/CurrentSense.cpp index e354dfaf..1b3ae498 100644 --- a/src/common/base_classes/CurrentSense.cpp +++ b/src/common/base_classes/CurrentSense.cpp @@ -187,19 +187,11 @@ int CurrentSense::alignBLDCDriver(float voltage, BLDCDriver* bldc_driver){ // if yes throw an error and return 0 // either the current sense is not connected or the current is // too low for calibration purposes (one should raise the motor.voltage_sensor_align) - if((_isset(pinA) && fabs(c_a.a) < 0.1f) - || (_isset(pinB) && fabs(c_a.b) < 0.1f) - || (_isset(pinC) && fabs(c_a.c) < 0.1f)){ + if((fabs(c_a.a) < 0.1f) && (fabs(c_a.b) < 0.1f) && (fabs(c_a.c) < 0.1f)){ SIMPLEFOC_DEBUG("CS: Err too low current, rise voltage!"); return 0; // measurement current too low } - // set phase B active and phases A and C down - bldc_driver->setPwm(0, voltage, 0); - _delay(500); - PhaseCurrent_s c_b = readAverageCurrents(); - bldc_driver->setPwm(0, 0, 0); - // now we have to determine // 1) which pin correspond to which phase of the bldc driver // 2) if the currents measured have good polarity @@ -244,7 +236,6 @@ int CurrentSense::alignBLDCDriver(float voltage, BLDCDriver* bldc_driver){ _swap(offset_ia, offset_ib); _swap(gain_a, gain_b); _swap(c_a.b, c_a.b); - _swap(c_b.a, c_b.b); // for the next phase of alignment phases_switched = true; // signal that pins have been switched break; case 2: // phase C is the max current @@ -254,7 +245,6 @@ int CurrentSense::alignBLDCDriver(float voltage, BLDCDriver* bldc_driver){ _swap(offset_ia, offset_ic); _swap(gain_a, gain_c); _swap(c_a.a, c_a.c); - _swap(c_b.a, c_b.c); // for the next phase of alignment phases_switched = true;// signal that pins have been switched break; } @@ -276,7 +266,6 @@ int CurrentSense::alignBLDCDriver(float voltage, BLDCDriver* bldc_driver){ _swap(offset_ia, offset_ib); _swap(gain_a, gain_b); _swap(c_a.b, c_a.b); - _swap(c_b.a, c_b.b); // for the next phase of alignment phases_switched = true; // signal that pins have been switched }else if(_isset(pinA) && !_isset(pinC)){ SIMPLEFOC_DEBUG("CS: Switch A-(C)NC"); @@ -284,7 +273,6 @@ int CurrentSense::alignBLDCDriver(float voltage, BLDCDriver* bldc_driver){ _swap(offset_ia, offset_ic); _swap(gain_a, gain_c); _swap(c_a.b, c_a.c); - _swap(c_b.a, c_b.c); // for the next phase of alignment phases_switched = true; // signal that pins have been switched } } @@ -294,6 +282,13 @@ int CurrentSense::alignBLDCDriver(float voltage, BLDCDriver* bldc_driver){ // // In either case A is done, now we have to check the phase B and C + + // set phase B active and phases A and C down + bldc_driver->setPwm(0, voltage, 0); + _delay(500); + PhaseCurrent_s c_b = readAverageCurrents(); + bldc_driver->setPwm(0, 0, 0); + // check the phase B // find the highest magnitude in c_b // and make sure it's around 2 (1.5 at least) times higher than the other two @@ -386,7 +381,8 @@ int CurrentSense::alignBLDCDriver(float voltage, BLDCDriver* bldc_driver){ // 4 - success but pins reconfigured and gains inverted int CurrentSense::alignStepperDriver(float voltage, StepperDriver* stepper_driver){ - int exit_flag = 1; + bool phases_switched = 0; + bool phases_inverted = 0; if(_isset(pinA)){ // set phase A active to high and B to low @@ -410,10 +406,11 @@ int CurrentSense::alignStepperDriver(float voltage, StepperDriver* stepper_drive _swap(offset_ia, offset_ib); _swap(gain_a, gain_b); gain_a *= _sign(c.b); - exit_flag = 2; // signal that pins have been switched + phases_switched = true; // signal that pins have been switched }else if (c.a < 0){ SIMPLEFOC_DEBUG("CS: Inv A"); gain_a *= -1; + phases_inverted = true; // signal that pins have been inverted } } @@ -432,11 +429,18 @@ int CurrentSense::alignStepperDriver(float voltage, StepperDriver* stepper_drive if (c.b < 0){ SIMPLEFOC_DEBUG("CS: Inv B"); gain_b *= -1; + phases_inverted = true; // signal that pins have been inverted } } - // add 2 if pin gains negative - if(gain_a < 0 || gain_b < 0 ) exit_flag +=2; + // construct the return flag + // if success and nothing changed return 1 + // if the phases have been switched return 2 + // if the gains have been inverted return 3 + // if both return 4 + uint8_t exit_flag = 1; + if(phases_switched) exit_flag += 1; + if(phases_inverted) exit_flag += 2; return exit_flag; } From ad0c4c526bad3c39ec5fc4e099d3ec3912fa648c Mon Sep 17 00:00:00 2001 From: askuric Date: Mon, 15 Jul 2024 13:19:34 +0200 Subject: [PATCH 46/57] feactoring and added ramping for current sense align --- README.md | 3 +- src/common/base_classes/CurrentSense.cpp | 121 ++++++++++++++--------- src/common/foc_utils.h | 2 +- 3 files changed, 76 insertions(+), 50 deletions(-) diff --git a/README.md b/README.md index 9fb24dd1..543e6533 100644 --- a/README.md +++ b/README.md @@ -13,7 +13,8 @@ ![GitHub commits since tagged version](https://img.shields.io/github/commits-since/simplefoc/arduino-foc/latest/dev) ![GitHub commit activity (branch)](https://img.shields.io/github/commit-activity/m/simplefoc/arduino-foc/dev) -[![arduino-library-badge](https://www.ardu-badge.com/badge/Simple%20FOC.svg?)](https://www.ardu-badge.com/badge/Simple%20FOC.svg) +[![arduino-library-badge](https://ardubadge.simplefoc.com?lib=Simple%20FOC)](https://www.ardu-badge.com/badge/Simple%20FOC.svg) +[![PlatformIO Registry](https://badges.registry.platformio.org/packages/askuric/library/Simple%20FOC.svg)](https://registry.platformio.org/libraries/askuric/Simple%20FOC) [![License: MIT](https://img.shields.io/badge/License-MIT-yellow.svg)](https://opensource.org/licenses/MIT) [![status](https://joss.theoj.org/papers/4382445f249e064e9f0a7f6c1bb06b1d/status.svg)](https://joss.theoj.org/papers/4382445f249e064e9f0a7f6c1bb06b1d) diff --git a/src/common/base_classes/CurrentSense.cpp b/src/common/base_classes/CurrentSense.cpp index 1b3ae498..5ef906e0 100644 --- a/src/common/base_classes/CurrentSense.cpp +++ b/src/common/base_classes/CurrentSense.cpp @@ -177,11 +177,17 @@ int CurrentSense::alignBLDCDriver(float voltage, BLDCDriver* bldc_driver){ bool phases_switched = 0; bool phases_inverted = 0; - + float center = driver->voltage_limit/2.0; + // set phase A active and phases B and C down - bldc_driver->setPwm(voltage, 0, 0); + // 300 ms of ramping + for(int i=0; i < 100; i++){ + bldc_driver->setPwm(voltage/100.0*((float)i) , 0, 0); + _delay(3); + } _delay(500); PhaseCurrent_s c_a = readAverageCurrents(); + bldc_driver->setPwm(0, 0, 0); // check if currents are to low (lower than 100mA) // TODO calculate the 100mA threshold from the ADC resolution // if yes throw an error and return 0 @@ -191,6 +197,7 @@ int CurrentSense::alignBLDCDriver(float voltage, BLDCDriver* bldc_driver){ SIMPLEFOC_DEBUG("CS: Err too low current, rise voltage!"); return 0; // measurement current too low } + // now we have to determine // 1) which pin correspond to which phase of the bldc driver @@ -284,7 +291,11 @@ int CurrentSense::alignBLDCDriver(float voltage, BLDCDriver* bldc_driver){ // set phase B active and phases A and C down - bldc_driver->setPwm(0, voltage, 0); + // 300 ms of ramping + for(int i=0; i < 100; i++){ + bldc_driver->setPwm(0, voltage/100.0*((float)i), 0); + _delay(3); + } _delay(500); PhaseCurrent_s c_b = readAverageCurrents(); bldc_driver->setPwm(0, 0, 0); @@ -384,53 +395,67 @@ int CurrentSense::alignStepperDriver(float voltage, StepperDriver* stepper_drive bool phases_switched = 0; bool phases_inverted = 0; - if(_isset(pinA)){ - // set phase A active to high and B to low - stepper_driver->setPwm(voltage, 0); - _delay(500); - PhaseCurrent_s c = readAverageCurrents(); - // disable the phases - stepper_driver->setPwm(0, 0); - if (fabs(c.a) < 0.1f && fabs(c.b) < 0.1f ){ - SIMPLEFOC_DEBUG("CS: Err too low current!"); - return 0; // measurement current too low - } - // align phase A - // check if measured current a is positive and invert if not - // check if current b is around zero and if its not - // check if current a is near zero and if it is invert them - if (fabs(c.a) < fabs(c.b)){ - SIMPLEFOC_DEBUG("CS: Switch A-B"); - // switch phase A and B - _swap(pinA, pinB); - _swap(offset_ia, offset_ib); - _swap(gain_a, gain_b); - gain_a *= _sign(c.b); - phases_switched = true; // signal that pins have been switched - }else if (c.a < 0){ - SIMPLEFOC_DEBUG("CS: Inv A"); - gain_a *= -1; - phases_inverted = true; // signal that pins have been inverted - } + if(!_isset(pinA) || !_isset(pinB)){ + SIMPLEFOC_DEBUG("CS: Pins A & B not specified!"); + return 0; } - if(_isset(pinB)){ - // set phase B active and phases A and C down - stepper_driver->setPwm(voltage, 0); - _delay(500); - PhaseCurrent_s c = readAverageCurrents(); - stepper_driver->setPwm(0, 0); - if (fabs(c.a) < 0.1f && fabs(c.b) < 0.1f ){ - SIMPLEFOC_DEBUG("CS: Err too low current!"); - return 0; // measurement current too low - } - // align phase A - // check if measured current a is positive and invert if not - if (c.b < 0){ - SIMPLEFOC_DEBUG("CS: Inv B"); - gain_b *= -1; - phases_inverted = true; // signal that pins have been inverted - } + // set phase A active and phases B down + // ramp 300ms + for(int i=0; i < 100; i++){ + stepper_driver->setPwm(voltage/100.0*((float)i), 0); + _delay(3); + } + _delay(500); + PhaseCurrent_s c = readAverageCurrents(); + // disable the phases + stepper_driver->setPwm(0, 0); + if (fabs(c.a) < 0.1f && fabs(c.b) < 0.1f ){ + SIMPLEFOC_DEBUG("CS: Err too low current!"); + return 0; // measurement current too low + } + // align phase A + // 1) only one phase can be measured so we first measure which ADC pin corresponds + // to the phase A by comparing the magnitude + if (fabs(c.a) < fabs(c.b)){ + SIMPLEFOC_DEBUG("CS: Switch A-B"); + // switch phase A and B + _swap(pinA, pinB); + _swap(offset_ia, offset_ib); + _swap(gain_a, gain_b); + phases_switched = true; // signal that pins have been switched + } + // 2) check if measured current a is positive and invert if not + if (c.a < 0){ + SIMPLEFOC_DEBUG("CS: Inv A"); + gain_a *= -1; + phases_inverted = true; // signal that pins have been inverted + } + + // at this point the driver's phase A is aligned with the ADC pinA + // and the pin B should be the phase B + + // set phase B active and phases A down + // ramp 300ms + for(int i=0; i < 100; i++){ + stepper_driver->setPwm(0, voltage/100.0*((float)i)); + _delay(3); + } + _delay(500); + c = readAverageCurrents(); + stepper_driver->setPwm(0, 0); + + // phase B should be aligned + // 1) we just need to verify that it has been measured + if (fabs(c.a) < 0.1f && fabs(c.b) < 0.1f ){ + SIMPLEFOC_DEBUG("CS: Err too low current!"); + return 0; // measurement current too low + } + // 2) check if measured current a is positive and invert if not + if (c.b < 0){ + SIMPLEFOC_DEBUG("CS: Inv B"); + gain_b *= -1; + phases_inverted = true; // signal that pins have been inverted } // construct the return flag diff --git a/src/common/foc_utils.h b/src/common/foc_utils.h index 9a5b53ea..2094ab26 100644 --- a/src/common/foc_utils.h +++ b/src/common/foc_utils.h @@ -35,7 +35,7 @@ #define _HIGH_IMPEDANCE 0 #define _HIGH_Z _HIGH_IMPEDANCE #define _ACTIVE 1 -#define _NC (NOT_SET) +#define _NC ((int) NOT_SET) #define MIN_ANGLE_DETECT_MOVEMENT (_2PI/101.0f) From 9aee1aa6a10efe3526b2c3e85273fe372feafc01 Mon Sep 17 00:00:00 2001 From: askuric Date: Mon, 15 Jul 2024 13:21:20 +0200 Subject: [PATCH 47/57] removed accidentally added files --- src/BLDCMotor.cpp.old | 732 ------------------------------------------ src/BLDCMotor.h.old | 121 ------- src/SimpleFOC_empty.h | 119 ------- 3 files changed, 972 deletions(-) delete mode 100644 src/BLDCMotor.cpp.old delete mode 100644 src/BLDCMotor.h.old delete mode 100644 src/SimpleFOC_empty.h diff --git a/src/BLDCMotor.cpp.old b/src/BLDCMotor.cpp.old deleted file mode 100644 index 6b996342..00000000 --- a/src/BLDCMotor.cpp.old +++ /dev/null @@ -1,732 +0,0 @@ -#include "BLDCMotor.h" -#include "./communication/SimpleFOCDebug.h" - -// BLDCMotor( int pp , float R) -// - pp - pole pair number -// - R - motor phase resistance -// - KV - motor kv rating -BLDCMotor::BLDCMotor(int pp, float _R, float _KV) -: FOCMotor() -{ - // save pole pairs number - pole_pairs = pp; - // save phase resistance number - phase_resistance = _R; - // save back emf constant KV = 1/KV - K_bemf = _isset(_KV) ? 1.0/_KV : NOT_SET; - - // torque control type is voltage by default - torque_controller = TorqueControlType::voltage; -} - - -/** - Link the driver which controls the motor -*/ -void BLDCMotor::linkDriver(BLDCDriver* _driver) { - driver = _driver; -} - -// init hardware pins -void BLDCMotor::init() { - if (!driver || !driver->initialized) { - motor_status = FOCMotorStatus::motor_init_failed; - SIMPLEFOC_DEBUG("MOT: Init not possible, driver not initialized"); - return; - } - motor_status = FOCMotorStatus::motor_initializing; - SIMPLEFOC_DEBUG("MOT: Init"); - - // sanity check for the voltage limit configuration - if(voltage_limit > driver->voltage_limit) voltage_limit = driver->voltage_limit; - // constrain voltage for sensor alignment - if(voltage_sensor_align > voltage_limit) voltage_sensor_align = voltage_limit; - - // update the controller limits - if(current_sense){ - // current control loop controls voltage - PID_current_q.limit = voltage_limit; - PID_current_d.limit = voltage_limit; - } - if(_isset(phase_resistance) || torque_controller != TorqueControlType::voltage){ - // velocity control loop controls current - PID_velocity.limit = current_limit; - }else{ - // velocity control loop controls the voltage - PID_velocity.limit = voltage_limit; - } - P_angle.limit = velocity_limit; - - _delay(500); - // enable motor - SIMPLEFOC_DEBUG("MOT: Enable driver."); - enable(); - _delay(500); - motor_status = FOCMotorStatus::motor_uncalibrated; -} - - -// disable motor driver -void BLDCMotor::disable() -{ - // set zero to PWM - driver->setPwm(0, 0, 0); - // disable the driver - driver->disable(); - // motor status update - enabled = 0; -} -// enable motor driver -void BLDCMotor::enable() -{ - // enable the driver - driver->enable(); - // set zero to PWM - driver->setPwm(0, 0, 0); - // motor status update - enabled = 1; -} - -/** - FOC functions -*/ -// FOC initialization function -int BLDCMotor::initFOC( float zero_electric_offset, Direction _sensor_direction) { - int exit_flag = 1; - - motor_status = FOCMotorStatus::motor_calibrating; - - // align motor if necessary - // alignment necessary for encoders! - if(_isset(zero_electric_offset)){ - // abosolute zero offset provided - no need to align - zero_electric_angle = zero_electric_offset; - // set the sensor direction - default CW - sensor_direction = _sensor_direction; - } - - // sensor and motor alignment - can be skipped - // by setting motor.sensor_direction and motor.zero_electric_angle - _delay(500); - if(sensor){ - exit_flag *= alignSensor(); - // added the shaft_angle update - sensor->update(); - shaft_angle = shaftAngle(); - }else - SIMPLEFOC_DEBUG("MOT: No sensor."); - - // aligning the current sensor - can be skipped - // checks if driver phases are the same as current sense phases - // and checks the direction of measuremnt. - _delay(500); - if(exit_flag){ - if(current_sense) exit_flag *= alignCurrentSense(); - else SIMPLEFOC_DEBUG("MOT: No current sense."); - } - - if(exit_flag){ - SIMPLEFOC_DEBUG("MOT: Ready."); - motor_status = FOCMotorStatus::motor_ready; - }else{ - SIMPLEFOC_DEBUG("MOT: Init FOC failed."); - motor_status = FOCMotorStatus::motor_calib_failed; - disable(); - } - - return exit_flag; -} - -// Calibarthe the motor and current sense phases -int BLDCMotor::alignCurrentSense() { - int exit_flag = 1; // success - - SIMPLEFOC_DEBUG("MOT: Align current sense."); - - // align current sense and the driver - exit_flag = current_sense->driverAlign(voltage_sensor_align); - if(!exit_flag){ - // error in current sense - phase either not measured or bad connection - SIMPLEFOC_DEBUG("MOT: Align error!"); - exit_flag = 0; - }else{ - // output the alignment status flag - SIMPLEFOC_DEBUG("MOT: Success: ", exit_flag); - } - - return exit_flag > 0; -} - -// Encoder alignment to electrical 0 angle -int BLDCMotor::alignSensor() { - int exit_flag = 1; //success - SIMPLEFOC_DEBUG("MOT: Align sensor."); - - // check if sensor needs zero search - if(sensor->needsSearch()) exit_flag = absoluteZeroSearch(); - // stop init if not found index - if(!exit_flag) return exit_flag; - - // if unknown natural direction - if(!_isset(sensor_direction)){ - - // find natural direction - // move one electrical revolution forward - for (int i = 0; i <=500; i++ ) { - float angle = _3PI_2 + _2PI * i / 500.0f; - setPhaseVoltage(voltage_sensor_align, 0, angle); - _delay(2); - } - // take and angle in the middle - sensor->update(); - float mid_angle = sensor->getAngle(); - // move one electrical revolution backwards - for (int i = 500; i >=0; i-- ) { - float angle = _3PI_2 + _2PI * i / 500.0f ; - setPhaseVoltage(voltage_sensor_align, 0, angle); - _delay(2); - } - sensor->update(); - float end_angle = sensor->getAngle(); - setPhaseVoltage(0, 0, 0); - _delay(200); - // determine the direction the sensor moved - if (mid_angle == end_angle) { - SIMPLEFOC_DEBUG("MOT: Failed to notice movement"); - return 0; // failed calibration - } else if (mid_angle < end_angle) { - SIMPLEFOC_DEBUG("MOT: sensor_direction==CCW"); - sensor_direction = Direction::CCW; - } else{ - SIMPLEFOC_DEBUG("MOT: sensor_direction==CW"); - sensor_direction = Direction::CW; - } - // check pole pair number - float moved = fabs(mid_angle - end_angle); - if( fabs(moved*pole_pairs - _2PI) > 0.5f ) { // 0.5f is arbitrary number it can be lower or higher! - SIMPLEFOC_DEBUG("MOT: PP check: fail - estimated pp: ", _2PI/moved); - } else - SIMPLEFOC_DEBUG("MOT: PP check: OK!"); - - } else SIMPLEFOC_DEBUG("MOT: Skip dir calib."); - - // zero electric angle not known - if(!_isset(zero_electric_angle)){ - - // // align the electrical phases of the motor and sensor - // // set angle -90(270 = 3PI/2) degrees - // setPhaseVoltage(voltage_sensor_align, 0, _3PI_2); - // _delay(700); - // // read the sensor - // sensor->update(); - // // get the current zero electric angle - // zero_electric_angle = 0; - // zero_electric_angle = electricalAngle(); - // _delay(20); - // if(monitor_port){ - // SIMPLEFOC_DEBUG("MOT: Zero elec. angle: ", zero_electric_angle); - // } - // // stop everything - // setPhaseVoltage(0, 0, 0); - // _delay(200); - - - // align the electrical phases of the motor and sensor - // set angle -90(270 = 3PI/2) degrees - setPhaseVoltage(voltage_sensor_align, 0, _3PI_2); - _delay(700); - // read the sensor - sensor->update(); - - float d_angle = pole_pairs*_2PI/((float) calib_points); - for(int i=0; i< calib_points; i++){ - setPhaseVoltage(voltage_sensor_align, 0, _3PI_2+d_angle*i); - _delay(50); - // read the sensor - sensor->update(); - // get the current zero electric angle - float sensor_angle = (float)(sensor_direction * pole_pairs) * sensor->getMechanicalAngle(); - zero_electric_angle_array[i] = _normalizeAngle( sensor_angle - d_angle*i); - // current_angle_offset_array[i] = 0; - // for(int n=0; n<20;n++){ - // DQCurrent_s c = current_sense->getFOCCurrents(0); // get alpha(d) and beta(q) currents - // current_angle_offset_array[i] += sensor_angle - atan2(c.q, c.d); - // _delay(1); - // } - // current_angle_offset_array[i] = _normalizeAngle(current_angle_offset_array[i]/20.0f ); - // if(i>=1){ - // float d_an = current_angle_offset_array[i] - current_angle_offset_array[i-1]; - // if( fabs(d_an) > 0.05){ - // current_angle_offset_array[i] = _normalizeAngle(current_angle_offset_array[i-1]+_sign(d_an)*0.05); - // } - // } - if(monitor_port){ - SIMPLEFOC_DEBUG("MOT: Zero elec. angle: ", zero_electric_angle_array[i]); - // SimpleFOCDebug::print(_normalizeAngle(zero_electric_angle_array[i]+_PI_2)-_PI_2); - // SimpleFOCDebug::print("\t"); - // SimpleFOCDebug::println(_normalizeAngle(current_angle_offset_array[i]+_PI_2)-_PI_2); - } - } - // stop everything - setPhaseVoltage(0, 0, 0); - // _delay(200); - - }else SIMPLEFOC_DEBUG("MOT: Skip offset calib."); - return exit_flag; -} - -float BLDCMotor::electricalAngle(){ - // if no sensor linked return previous value ( for open loop ) - if(!sensor) return electrical_angle; - float angle = sensor->getMechanicalAngle(); - zero_electric_angle = zero_electric_angle_array[_round(angle/_2PI*calib_points) % calib_points ]; - return _normalizeAngle( (float)(sensor_direction * pole_pairs) * angle - zero_electric_angle ); -} - -// Encoder alignment the absolute zero angle -// - to the index -int BLDCMotor::absoluteZeroSearch() { - // sensor precision: this is all ok, as the search happens near the 0-angle, where the precision - // of float is sufficient. - SIMPLEFOC_DEBUG("MOT: Index search..."); - // search the absolute zero with small velocity - float limit_vel = velocity_limit; - float limit_volt = voltage_limit; - velocity_limit = velocity_index_search; - voltage_limit = voltage_sensor_align; - shaft_angle = 0; - while(sensor->needsSearch() && shaft_angle < _2PI){ - angleOpenloop(1.5f*_2PI); - // call important for some sensors not to loose count - // not needed for the search - sensor->update(); - } - // disable motor - setPhaseVoltage(0, 0, 0); - // reinit the limits - velocity_limit = limit_vel; - voltage_limit = limit_volt; - // check if the zero found - if(monitor_port){ - if(sensor->needsSearch()) SIMPLEFOC_DEBUG("MOT: Error: Not found!"); - else SIMPLEFOC_DEBUG("MOT: Success!"); - } - return !sensor->needsSearch(); -} - -// Iterative function looping FOC algorithm, setting Uq on the Motor -// The faster it can be run the better -void BLDCMotor::loopFOC() { - // update sensor - do this even in open-loop mode, as user may be switching between modes and we could lose track - // of full rotations otherwise. - if (sensor) sensor->update(); - - // if open-loop do nothing - if( controller==MotionControlType::angle_openloop || controller==MotionControlType::velocity_openloop ) return; - - // if disabled do nothing - if(!enabled) return; - - // Needs the update() to be called first - // This function will not have numerical issues because it uses Sensor::getMechanicalAngle() - // which is in range 0-2PI - electrical_angle = electricalAngle(); - float current_electrical_angle = electrical_angle; - // float angle = sensor->getMechanicalAngle(); - // float current_angle_offset = current_angle_offset_array[_round(angle/_2PI*calib_points) % calib_points ]; - // float current_electrical_angle = _normalizeAngle( (float)(sensor_direction * pole_pairs) * angle - current_angle_offset ); - switch (torque_controller) { - - case TorqueControlType::voltage: - // no need to do anything really - break; - case TorqueControlType::dc_current: - if(!current_sense) return; - // read overall current magnitude - current.q = current_sense->getDCCurrent(current_electrical_angle); - // filter the value values - current.q = LPF_current_q(current.q); - // calculate the phase voltage - voltage.q = PID_current_q(current_sp - current.q); - voltage.d = 0; - break; - case TorqueControlType::foc_current: - if(!current_sense) return; - // read dq currents - current = current_sense->getFOCCurrents(current_electrical_angle); - // filter values - current.q = LPF_current_q(current.q); - current.d = LPF_current_d(current.d); - // calculate the phase voltages - voltage.q = PID_current_q(current_sp - current.q); - voltage.d = PID_current_d(-current.d); - break; - default: - // no torque control selected - SIMPLEFOC_DEBUG("MOT: no torque control selected!"); - break; - } - - // set the phase voltage - FOC heart function :) - setPhaseVoltage(voltage.q, voltage.d, electrical_angle); -} - -// Iterative function running outer loop of the FOC algorithm -// Behavior of this function is determined by the motor.controller variable -// It runs either angle, velocity or torque loop -// - needs to be called iteratively it is asynchronous function -// - if target is not set it uses motor.target value -void BLDCMotor::move(float new_target) { - - // downsampling (optional) - if(motion_cnt++ < motion_downsample) return; - motion_cnt = 0; - - // shaft angle/velocity need the update() to be called first - // get shaft angle - // TODO sensor precision: the shaft_angle actually stores the complete position, including full rotations, as a float - // For this reason it is NOT precise when the angles become large. - // Additionally, the way LPF works on angle is a precision issue, and the angle-LPF is a problem - // when switching to a 2-component representation. - if( controller!=MotionControlType::angle_openloop && controller!=MotionControlType::velocity_openloop ) - shaft_angle = shaftAngle(); // read value even if motor is disabled to keep the monitoring updated but not in openloop mode - // get angular velocity - shaft_velocity = shaftVelocity(); // read value even if motor is disabled to keep the monitoring updated - - // if disabled do nothing - if(!enabled) return; - // set internal target variable - if(_isset(new_target)) target = new_target; - - // calculate the back-emf voltage if K_bemf available - if (_isset(K_bemf)) voltage_bemf = K_bemf*shaft_velocity; - // estimate the motor current if phase reistance available and current_sense not available - if(!current_sense && _isset(phase_resistance)) current.q = (voltage.q - voltage_bemf)/phase_resistance; - - // upgrade the current based voltage limit - switch (controller) { - case MotionControlType::torque: - if(torque_controller == TorqueControlType::voltage){ // if voltage torque control - if(!_isset(phase_resistance)) voltage.q = target; - else voltage.q = target*phase_resistance + voltage_bemf; - voltage.q = _constrain(voltage.q, -voltage_limit, voltage_limit); - voltage.d = 0; - }else{ - current_sp = target; // if current/foc_current torque control - } - break; - case MotionControlType::angle: - // TODO sensor precision: this calculation is not numerically precise. The target value cannot express precise positions when - // the angles are large. This results in not being able to command small changes at high position values. - // to solve this, the delta-angle has to be calculated in a numerically precise way. - // angle set point - shaft_angle_sp = target; - // calculate velocity set point - shaft_velocity_sp = P_angle( shaft_angle_sp - shaft_angle ); - // calculate the torque command - sensor precision: this calculation is ok, but based on bad value from previous calculation - current_sp = PID_velocity(shaft_velocity_sp - shaft_velocity); // if voltage torque control - // if torque controlled through voltage - if(torque_controller == TorqueControlType::voltage){ - // use voltage if phase-resistance not provided - if(!_isset(phase_resistance)) voltage.q = current_sp; - else voltage.q = _constrain( current_sp*phase_resistance + voltage_bemf , -voltage_limit, voltage_limit); - voltage.d = 0; - } - break; - case MotionControlType::velocity: - // velocity set point - sensor precision: this calculation is numerically precise. - shaft_velocity_sp = target; - // calculate the torque command - current_sp = PID_velocity(shaft_velocity_sp - shaft_velocity); // if current/foc_current torque control - // if torque controlled through voltage control - if(torque_controller == TorqueControlType::voltage){ - // use voltage if phase-resistance not provided - if(!_isset(phase_resistance)) voltage.q = current_sp; - else voltage.q = _constrain( current_sp*phase_resistance + voltage_bemf , -voltage_limit, voltage_limit); - voltage.d = 0; - } - break; - case MotionControlType::velocity_openloop: - // velocity control in open loop - sensor precision: this calculation is numerically precise. - shaft_velocity_sp = target; - voltage.q = velocityOpenloop(shaft_velocity_sp); // returns the voltage that is set to the motor - voltage.d = 0; - break; - case MotionControlType::angle_openloop: - // angle control in open loop - - // TODO sensor precision: this calculation NOT numerically precise, and subject - // to the same problems in small set-point changes at high angles - // as the closed loop version. - shaft_angle_sp = target; - voltage.q = angleOpenloop(shaft_angle_sp); // returns the voltage that is set to the motor - voltage.d = 0; - break; - } -} - - -// Method using FOC to set Uq and Ud to the motor at the optimal angle -// Function implementing Space Vector PWM and Sine PWM algorithms -// -// Function using sine approximation -// regular sin + cos ~300us (no memory usaage) -// approx _sin + _cos ~110us (400Byte ~ 20% of memory) -void BLDCMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) { - - float center; - int sector; - float _ca,_sa; - - switch (foc_modulation) - { - case FOCModulationType::Trapezoid_120 : - // see https://www.youtube.com/watch?v=InzXA7mWBWE Slide 5 - static int trap_120_map[6][3] = { - {_HIGH_IMPEDANCE,1,-1},{-1,1,_HIGH_IMPEDANCE},{-1,_HIGH_IMPEDANCE,1},{_HIGH_IMPEDANCE,-1,1},{1,-1,_HIGH_IMPEDANCE},{1,_HIGH_IMPEDANCE,-1} // each is 60 degrees with values for 3 phases of 1=positive -1=negative 0=high-z - }; - // static int trap_120_state = 0; - sector = 6 * (_normalizeAngle(angle_el + _PI_6 ) / _2PI); // adding PI/6 to align with other modes - // centering the voltages around either - // modulation_centered == true > driver.volage_limit/2 - // modulation_centered == false > or Adaptable centering, all phases drawn to 0 when Uq=0 - center = modulation_centered ? (driver->voltage_limit)/2 : Uq; - - if(trap_120_map[sector][0] == _HIGH_IMPEDANCE){ - Ua= center; - Ub = trap_120_map[sector][1] * Uq + center; - Uc = trap_120_map[sector][2] * Uq + center; - driver->setPhaseState(_HIGH_IMPEDANCE, _ACTIVE, _ACTIVE); // disable phase if possible - }else if(trap_120_map[sector][1] == _HIGH_IMPEDANCE){ - Ua = trap_120_map[sector][0] * Uq + center; - Ub = center; - Uc = trap_120_map[sector][2] * Uq + center; - driver->setPhaseState(_ACTIVE, _HIGH_IMPEDANCE, _ACTIVE);// disable phase if possible - }else{ - Ua = trap_120_map[sector][0] * Uq + center; - Ub = trap_120_map[sector][1] * Uq + center; - Uc = center; - driver->setPhaseState(_ACTIVE,_ACTIVE, _HIGH_IMPEDANCE);// disable phase if possible - } - - break; - - case FOCModulationType::Trapezoid_150 : - // see https://www.youtube.com/watch?v=InzXA7mWBWE Slide 8 - static int trap_150_map[12][3] = { - {_HIGH_IMPEDANCE,1,-1},{-1,1,-1},{-1,1,_HIGH_IMPEDANCE},{-1,1,1},{-1,_HIGH_IMPEDANCE,1},{-1,-1,1},{_HIGH_IMPEDANCE,-1,1},{1,-1,1},{1,-1,_HIGH_IMPEDANCE},{1,-1,-1},{1,_HIGH_IMPEDANCE,-1},{1,1,-1} // each is 30 degrees with values for 3 phases of 1=positive -1=negative 0=high-z - }; - // static int trap_150_state = 0; - sector = 12 * (_normalizeAngle(angle_el + _PI_6 ) / _2PI); // adding PI/6 to align with other modes - // centering the voltages around either - // modulation_centered == true > driver.volage_limit/2 - // modulation_centered == false > or Adaptable centering, all phases drawn to 0 when Uq=0 - center = modulation_centered ? (driver->voltage_limit)/2 : Uq; - - if(trap_150_map[sector][0] == _HIGH_IMPEDANCE){ - Ua= center; - Ub = trap_150_map[sector][1] * Uq + center; - Uc = trap_150_map[sector][2] * Uq + center; - driver->setPhaseState(_HIGH_IMPEDANCE, _ACTIVE, _ACTIVE); // disable phase if possible - }else if(trap_150_map[sector][1] == _HIGH_IMPEDANCE){ - Ua = trap_150_map[sector][0] * Uq + center; - Ub = center; - Uc = trap_150_map[sector][2] * Uq + center; - driver->setPhaseState(_ACTIVE, _HIGH_IMPEDANCE, _ACTIVE);// disable phase if possible - }else{ - Ua = trap_150_map[sector][0] * Uq + center; - Ub = trap_150_map[sector][1] * Uq + center; - Uc = center; - driver->setPhaseState(_ACTIVE, _ACTIVE, _HIGH_IMPEDANCE);// disable phase if possible - } - - break; - - case FOCModulationType::SinePWM : - // Sinusoidal PWM modulation - // Inverse Park + Clarke transformation - - // angle normalization in between 0 and 2pi - // only necessary if using _sin and _cos - approximation functions - angle_el = _normalizeAngle(angle_el); - _ca = _cos(angle_el); - _sa = _sin(angle_el); - // Inverse park transform - Ualpha = _ca * Ud - _sa * Uq; // -sin(angle) * Uq; - Ubeta = _sa * Ud + _ca * Uq; // cos(angle) * Uq; - - // center = modulation_centered ? (driver->voltage_limit)/2 : Uq; - center = driver->voltage_limit/2; - // Clarke transform - Ua = Ualpha + center; - Ub = -0.5f * Ualpha + _SQRT3_2 * Ubeta + center; - Uc = -0.5f * Ualpha - _SQRT3_2 * Ubeta + center; - - if (!modulation_centered) { - float Umin = min(Ua, min(Ub, Uc)); - Ua -= Umin; - Ub -= Umin; - Uc -= Umin; - } - - break; - - case FOCModulationType::SpaceVectorPWM : - // Nice video explaining the SpaceVectorModulation (SVPWM) algorithm - // https://www.youtube.com/watch?v=QMSWUMEAejg - - // the algorithm goes - // 1) Ualpha, Ubeta - // 2) Uout = sqrt(Ualpha^2 + Ubeta^2) - // 3) angle_el = atan2(Ubeta, Ualpha) - // - // equivalent to 2) because the magnitude does not change is: - // Uout = sqrt(Ud^2 + Uq^2) - // equivalent to 3) is - // angle_el = angle_el + atan2(Uq,Ud) - - float Uout; - // a bit of optitmisation - if(Ud){ // only if Ud and Uq set - // _sqrt is an approx of sqrt (3-4% error) - Uout = _sqrt(Ud*Ud + Uq*Uq) / driver->voltage_limit; - // angle normalisation in between 0 and 2pi - // only necessary if using _sin and _cos - approximation functions - angle_el = _normalizeAngle(angle_el + atan2(Uq, Ud)); - }else{// only Uq available - no need for atan2 and sqrt - Uout = Uq / driver->voltage_limit; - // angle normalisation in between 0 and 2pi - // only necessary if using _sin and _cos - approximation functions - angle_el = _normalizeAngle(angle_el + _PI_2); - } - // find the sector we are in currently - sector = floor(angle_el / _PI_3) + 1; - // calculate the duty cycles - float T1 = _SQRT3*_sin(sector*_PI_3 - angle_el) * Uout; - float T2 = _SQRT3*_sin(angle_el - (sector-1.0f)*_PI_3) * Uout; - // two versions possible - float T0 = 0; // pulled to 0 - better for low power supply voltage - if (modulation_centered) { - T0 = 1 - T1 - T2; // modulation_centered around driver->voltage_limit/2 - } - - // calculate the duty cycles(times) - float Ta,Tb,Tc; - switch(sector){ - case 1: - Ta = T1 + T2 + T0/2; - Tb = T2 + T0/2; - Tc = T0/2; - break; - case 2: - Ta = T1 + T0/2; - Tb = T1 + T2 + T0/2; - Tc = T0/2; - break; - case 3: - Ta = T0/2; - Tb = T1 + T2 + T0/2; - Tc = T2 + T0/2; - break; - case 4: - Ta = T0/2; - Tb = T1+ T0/2; - Tc = T1 + T2 + T0/2; - break; - case 5: - Ta = T2 + T0/2; - Tb = T0/2; - Tc = T1 + T2 + T0/2; - break; - case 6: - Ta = T1 + T2 + T0/2; - Tb = T0/2; - Tc = T1 + T0/2; - break; - default: - // possible error state - Ta = 0; - Tb = 0; - Tc = 0; - } - - // calculate the phase voltages and center - Ua = Ta*driver->voltage_limit; - Ub = Tb*driver->voltage_limit; - Uc = Tc*driver->voltage_limit; - break; - - } - - // set the voltages in driver - driver->setPwm(Ua, Ub, Uc); -} - - - -// Function (iterative) generating open loop movement for target velocity -// - target_velocity - rad/s -// it uses voltage_limit variable -float BLDCMotor::velocityOpenloop(float target_velocity){ - // get current timestamp - unsigned long now_us = _micros(); - // calculate the sample time from last call - float Ts = (now_us - open_loop_timestamp) * 1e-6f; - // quick fix for strange cases (micros overflow + timestamp not defined) - if(Ts <= 0 || Ts > 0.5f) Ts = 1e-3f; - - // calculate the necessary angle to achieve target velocity - shaft_angle = _normalizeAngle(shaft_angle + target_velocity*Ts); - // for display purposes - shaft_velocity = target_velocity; - - // use voltage limit or current limit - float Uq = voltage_limit; - if(_isset(phase_resistance)) - Uq = _constrain(current_limit*phase_resistance + voltage_bemf,-voltage_limit, voltage_limit); - - // set the maximal allowed voltage (voltage_limit) with the necessary angle - setPhaseVoltage(Uq, 0, _electricalAngle(shaft_angle, pole_pairs)); - - // save timestamp for next call - open_loop_timestamp = now_us; - - return Uq; -} - -// Function (iterative) generating open loop movement towards the target angle -// - target_angle - rad -// it uses voltage_limit and velocity_limit variables -float BLDCMotor::angleOpenloop(float target_angle){ - // get current timestamp - unsigned long now_us = _micros(); - // calculate the sample time from last call - float Ts = (now_us - open_loop_timestamp) * 1e-6f; - // quick fix for strange cases (micros overflow + timestamp not defined) - if(Ts <= 0 || Ts > 0.5f) Ts = 1e-3f; - - // calculate the necessary angle to move from current position towards target angle - // with maximal velocity (velocity_limit) - // TODO sensor precision: this calculation is not numerically precise. The angle can grow to the point - // where small position changes are no longer captured by the precision of floats - // when the total position is large. - if(abs( target_angle - shaft_angle ) > abs(velocity_limit*Ts)){ - shaft_angle += _sign(target_angle - shaft_angle) * abs( velocity_limit )*Ts; - shaft_velocity = velocity_limit; - }else{ - shaft_angle = target_angle; - shaft_velocity = 0; - } - - // use voltage limit or current limit - float Uq = voltage_limit; - if(_isset(phase_resistance)) - Uq = _constrain(current_limit*phase_resistance + voltage_bemf,-voltage_limit, voltage_limit); - // set the maximal allowed voltage (voltage_limit) with the necessary angle - // sensor precision: this calculation is OK due to the normalisation - setPhaseVoltage(Uq, 0, _electricalAngle(_normalizeAngle(shaft_angle), pole_pairs)); - - // save timestamp for next call - open_loop_timestamp = now_us; - - return Uq; -} diff --git a/src/BLDCMotor.h.old b/src/BLDCMotor.h.old deleted file mode 100644 index 04c54b92..00000000 --- a/src/BLDCMotor.h.old +++ /dev/null @@ -1,121 +0,0 @@ -#ifndef BLDCMotor_h -#define BLDCMotor_h - -#include "Arduino.h" -#include "common/base_classes/FOCMotor.h" -#include "common/base_classes/Sensor.h" -#include "common/base_classes/BLDCDriver.h" -#include "common/foc_utils.h" -#include "common/time_utils.h" -#include "common/defaults.h" - -/** - BLDC motor class -*/ -class BLDCMotor: public FOCMotor -{ - public: - /** - BLDCMotor class constructor - @param pp pole pairs number - @param R motor phase resistance - @param KV motor KV rating (1/K_bemf) - */ - BLDCMotor(int pp, float R = NOT_SET, float KV = NOT_SET); - - /** - * Function linking a motor and a foc driver - * - * @param driver BLDCDriver class implementing all the hardware specific functions necessary PWM setting - */ - void linkDriver(BLDCDriver* driver); - - /** - * BLDCDriver link: - * - 3PWM - * - 6PWM - */ - BLDCDriver* driver; - - /** Motor hardware init function */ - void init() override; - /** Motor disable function */ - void disable() override; - /** Motor enable function */ - void enable() override; - - /** - * Function initializing FOC algorithm - * and aligning sensor's and motors' zero position - */ - int initFOC( float zero_electric_offset = NOT_SET , Direction sensor_direction = Direction::CW) override; - /** - * Function running FOC algorithm in real-time - * it calculates the gets motor angle and sets the appropriate voltages - * to the phase pwm signals - * - the faster you can run it the better Arduino UNO ~1ms, Bluepill ~ 100us - */ - void loopFOC() override; - - /** - * Function executing the control loops set by the controller parameter of the BLDCMotor. - * - * @param target Either voltage, angle or velocity based on the motor.controller - * If it is not set the motor will use the target set in its variable motor.target - * - * This function doesn't need to be run upon each loop execution - depends of the use case - */ - void move(float target = NOT_SET) override; - - float Ua, Ub, Uc;//!< Current phase voltages Ua,Ub and Uc set to motor - float Ualpha, Ubeta; //!< Phase voltages U alpha and U beta used for inverse Park and Clarke transform - - - - float electricalAngle(); - int calib_points = 1000; - float zero_electric_angle_array[1000]; - float current_angle_offset_array[1000]; - - private: - // FOC methods - /** - * Method using FOC to set Uq to the motor at the optimal angle - * Heart of the FOC algorithm - * - * @param Uq Current voltage in q axis to set to the motor - * @param Ud Current voltage in d axis to set to the motor - * @param angle_el current electrical angle of the motor - */ - void setPhaseVoltage(float Uq, float Ud, float angle_el); - /** Sensor alignment to electrical 0 angle of the motor */ - int alignSensor(); - /** Current sense and motor phase alignment */ - int alignCurrentSense(); - /** Motor and sensor alignment to the sensors absolute 0 angle */ - int absoluteZeroSearch(); - - - // Open loop motion control - /** - * Function (iterative) generating open loop movement for target velocity - * it uses voltage_limit variable - * - * @param target_velocity - rad/s - */ - float velocityOpenloop(float target_velocity); - /** - * Function (iterative) generating open loop movement towards the target angle - * it uses voltage_limit and velocity_limit variables - * - * @param target_angle - rad - */ - float angleOpenloop(float target_angle); - // open loop variables - long open_loop_timestamp; - - -}; - - -#endif diff --git a/src/SimpleFOC_empty.h b/src/SimpleFOC_empty.h deleted file mode 100644 index bf0049dd..00000000 --- a/src/SimpleFOC_empty.h +++ /dev/null @@ -1,119 +0,0 @@ -/*! - * @file SimpleFOC.h - * - * @mainpage Simple Field Oriented Control BLDC motor control library - * - * @section intro_sec Introduction - * - * Proper low-cost and low-power FOC supporting boards are very hard to find these days and even may not exist.
Even harder to find is a stable and simple FOC algorithm code capable of running on Arduino devices. Therefore this is an attempt to: - * - Demystify FOC algorithm and make a robust but simple Arduino library: Arduino SimpleFOC library - * - Develop a modular BLDC driver board: Arduino SimpleFOC shield. - * - * @section features Features - * - Arduino compatible: Arduino library code - * - Easy to setup and configure: - * - Easy hardware configuration - * - Easy tuning the control loops - * - Modular: - * - Supports as many sensors , BLDC motors and driver boards as possible - * - Supports as many application requirements as possible - * - Plug & play: Arduino SimpleFOC shield - * - * @section dependencies Supported Hardware - * - Motors - * - BLDC motors - * - Stepper motors - * - Drivers - * - BLDC drivers - * - Gimbal drivers - * - Stepper drivers - * - Position sensors - * - Encoders - * - Magnetic sensors - * - Hall sensors - * - Open-loop control - * - Microcontrollers - * - Arduino - * - STM32 - * - ESP32 - * - Teensy - * - * @section example_code Example code - * @code -#include - -// BLDCMotor( pole_pairs ) -BLDCMotor motor = BLDCMotor(11); -// BLDCDriver( pin_pwmA, pin_pwmB, pin_pwmC, enable (optional) ) -BLDCDriver3PWM motor = BLDCDriver3PWM(9, 10, 11, 8); -// Encoder(pin_A, pin_B, CPR) -Encoder encoder = Encoder(2, 3, 2048); -// channel A and B callbacks -void doA(){encoder.handleA();} -void doB(){encoder.handleB();} - - -void setup() { - // initialize encoder hardware - encoder.init(); - // hardware interrupt enable - encoder.enableInterrupts(doA, doB); - // link the motor to the sensor - motor.linkSensor(&encoder); - - // power supply voltage [V] - driver.voltage_power_supply = 12; - // initialise driver hardware - driver.init(); - // link driver - motor.linkDriver(&driver); - - // set control loop type to be used - motor.controller = MotionControlType::velocity; - // initialize motor - motor.init(); - - // align encoder and start FOC - motor.initFOC(); -} - -void loop() { - // FOC algorithm function - motor.loopFOC(); - - // velocity control loop function - // setting the target velocity or 2rad/s - motor.move(2); -} - * @endcode - * - * @section license License - * - * MIT license, all text here must be included in any redistribution. - * - */ - -#ifndef SIMPLEFOC_EMPTY_H -#define SIMPLEFOC_EMPTY_H - -// #include "BLDCMotor.h" -// #include "StepperMotor.h" -// #include "sensors/Encoder.h" -// #include "sensors/MagneticSensorSPI.h" -// #include "sensors/MagneticSensorI2C.h" -// #include "sensors/MagneticSensorAnalog.h" -// #include "sensors/MagneticSensorPWM.h" -// #include "sensors/HallSensor.h" -// #include "sensors/GenericSensor.h" -// #include "drivers/BLDCDriver3PWM.h" -// #include "drivers/BLDCDriver6PWM.h" -// #include "drivers/StepperDriver4PWM.h" -// #include "drivers/StepperDriver2PWM.h" -// #include "current_sense/InlineCurrentSense.h" -// #include "current_sense/LowsideCurrentSense.h" -// #include "current_sense/GenericCurrentSense.h" -// #include "communication/Commander.h" -// #include "communication/StepDirListener.h" -// #include "communication/SimpleFOCDebug.h" - -#endif From cf70cce014fc94f290c9df9315a8ee23dc22e543 Mon Sep 17 00:00:00 2001 From: askuric Date: Mon, 15 Jul 2024 14:01:55 +0200 Subject: [PATCH 48/57] added the contering posibility --- src/BLDCMotor.cpp | 2 +- src/StepperMotor.cpp | 2 +- src/common/base_classes/CurrentSense.cpp | 32 +++++++++++++---------- src/common/base_classes/CurrentSense.h | 6 ++--- src/current_sense/GenericCurrentSense.cpp | 2 +- src/current_sense/GenericCurrentSense.h | 2 +- 6 files changed, 25 insertions(+), 21 deletions(-) diff --git a/src/BLDCMotor.cpp b/src/BLDCMotor.cpp index acc5b0fe..792381d5 100644 --- a/src/BLDCMotor.cpp +++ b/src/BLDCMotor.cpp @@ -202,7 +202,7 @@ int BLDCMotor::alignCurrentSense() { SIMPLEFOC_DEBUG("MOT: Align current sense."); // align current sense and the driver - exit_flag = current_sense->driverAlign(voltage_sensor_align); + exit_flag = current_sense->driverAlign(voltage_sensor_align, modulation_centered); if(!exit_flag){ // error in current sense - phase either not measured or bad connection SIMPLEFOC_DEBUG("MOT: Align error!"); diff --git a/src/StepperMotor.cpp b/src/StepperMotor.cpp index 38e8999f..5db9afa2 100644 --- a/src/StepperMotor.cpp +++ b/src/StepperMotor.cpp @@ -159,7 +159,7 @@ int StepperMotor::alignCurrentSense() { SIMPLEFOC_DEBUG("MOT: Align current sense."); // align current sense and the driver - exit_flag = current_sense->driverAlign(voltage_sensor_align); + exit_flag = current_sense->driverAlign(voltage_sensor_align, modulation_centered); if(!exit_flag){ // error in current sense - phase either not measured or bad connection SIMPLEFOC_DEBUG("MOT: Align error!"); diff --git a/src/common/base_classes/CurrentSense.cpp b/src/common/base_classes/CurrentSense.cpp index 5ef906e0..84bc3547 100644 --- a/src/common/base_classes/CurrentSense.cpp +++ b/src/common/base_classes/CurrentSense.cpp @@ -133,7 +133,7 @@ void CurrentSense::disable(){ // 3 - success but gains inverted // 4 - success but pins reconfigured and gains inverted // IMPORTANT, this function can be overriden in the child class -int CurrentSense::driverAlign(float voltage){ +int CurrentSense::driverAlign(float voltage, bool modulation_centered){ int exit_flag = 1; if(skip_align) return exit_flag; @@ -142,9 +142,9 @@ int CurrentSense::driverAlign(float voltage){ // check if stepper or BLDC if(driver_type == DriverType::Stepper) - return alignStepperDriver(voltage, (StepperDriver*)driver); + return alignStepperDriver(voltage, (StepperDriver*)driver, modulation_centered); else - return alignBLDCDriver(voltage, (BLDCDriver*)driver); + return alignBLDCDriver(voltage, (BLDCDriver*)driver, modulation_centered); } @@ -172,22 +172,23 @@ PhaseCurrent_s CurrentSense::readAverageCurrents(int N) { // 2 - success but pins reconfigured // 3 - success but gains inverted // 4 - success but pins reconfigured and gains inverted -int CurrentSense::alignBLDCDriver(float voltage, BLDCDriver* bldc_driver){ +int CurrentSense::alignBLDCDriver(float voltage, BLDCDriver* bldc_driver, bool modulation_centered){ bool phases_switched = 0; bool phases_inverted = 0; - float center = driver->voltage_limit/2.0; + float zero = 0; + if(modulation_centered) zero = driver->voltage_limit/2.0; // set phase A active and phases B and C down // 300 ms of ramping for(int i=0; i < 100; i++){ - bldc_driver->setPwm(voltage/100.0*((float)i) , 0, 0); + bldc_driver->setPwm(voltage/100.0*((float)i)+zero , zero, zero); _delay(3); } _delay(500); PhaseCurrent_s c_a = readAverageCurrents(); - bldc_driver->setPwm(0, 0, 0); + bldc_driver->setPwm(zero, zero, zero); // check if currents are to low (lower than 100mA) // TODO calculate the 100mA threshold from the ADC resolution // if yes throw an error and return 0 @@ -293,12 +294,12 @@ int CurrentSense::alignBLDCDriver(float voltage, BLDCDriver* bldc_driver){ // set phase B active and phases A and C down // 300 ms of ramping for(int i=0; i < 100; i++){ - bldc_driver->setPwm(0, voltage/100.0*((float)i), 0); + bldc_driver->setPwm(zero, voltage/100.0*((float)i)+zero, zero); _delay(3); } _delay(500); PhaseCurrent_s c_b = readAverageCurrents(); - bldc_driver->setPwm(0, 0, 0); + bldc_driver->setPwm(zero, zero, zero); // check the phase B // find the highest magnitude in c_b @@ -390,10 +391,13 @@ int CurrentSense::alignBLDCDriver(float voltage, BLDCDriver* bldc_driver){ // 2 - success but pins reconfigured // 3 - success but gains inverted // 4 - success but pins reconfigured and gains inverted -int CurrentSense::alignStepperDriver(float voltage, StepperDriver* stepper_driver){ +int CurrentSense::alignStepperDriver(float voltage, StepperDriver* stepper_driver, bool modulation_centered){ bool phases_switched = 0; bool phases_inverted = 0; + + float zero = 0; + if(modulation_centered) zero = driver->voltage_limit/2.0; if(!_isset(pinA) || !_isset(pinB)){ SIMPLEFOC_DEBUG("CS: Pins A & B not specified!"); @@ -403,13 +407,13 @@ int CurrentSense::alignStepperDriver(float voltage, StepperDriver* stepper_drive // set phase A active and phases B down // ramp 300ms for(int i=0; i < 100; i++){ - stepper_driver->setPwm(voltage/100.0*((float)i), 0); + stepper_driver->setPwm(voltage/100.0*((float)i), zero); _delay(3); } _delay(500); PhaseCurrent_s c = readAverageCurrents(); // disable the phases - stepper_driver->setPwm(0, 0); + stepper_driver->setPwm(zero, zero); if (fabs(c.a) < 0.1f && fabs(c.b) < 0.1f ){ SIMPLEFOC_DEBUG("CS: Err too low current!"); return 0; // measurement current too low @@ -438,12 +442,12 @@ int CurrentSense::alignStepperDriver(float voltage, StepperDriver* stepper_drive // set phase B active and phases A down // ramp 300ms for(int i=0; i < 100; i++){ - stepper_driver->setPwm(0, voltage/100.0*((float)i)); + stepper_driver->setPwm(zero, voltage/100.0*((float)i)+zero); _delay(3); } _delay(500); c = readAverageCurrents(); - stepper_driver->setPwm(0, 0); + stepper_driver->setPwm(zero, zero); // phase B should be aligned // 1) we just need to verify that it has been measured diff --git a/src/common/base_classes/CurrentSense.h b/src/common/base_classes/CurrentSense.h index c9c79650..d3f7f8ed 100644 --- a/src/common/base_classes/CurrentSense.h +++ b/src/common/base_classes/CurrentSense.h @@ -68,7 +68,7 @@ class CurrentSense{ * * IMPORTANT: Default implementation provided in the CurrentSense class, but can be overriden in the child classes */ - virtual int driverAlign(float align_voltage); + virtual int driverAlign(float align_voltage, bool modulation_centered = false); /** * Function rading the phase currents a, b and c @@ -130,11 +130,11 @@ class CurrentSense{ /** * Function used to align the current sense with the BLDC motor driver */ - int alignBLDCDriver(float align_voltage, BLDCDriver* driver); + int alignBLDCDriver(float align_voltage, BLDCDriver* driver, bool modulation_centered); /** * Function used to align the current sense with the Stepper motor driver */ - int alignStepperDriver(float align_voltage, StepperDriver* driver); + int alignStepperDriver(float align_voltage, StepperDriver* driver, bool modulation_centered); /** * Function used to read the average current values over N samples */ diff --git a/src/current_sense/GenericCurrentSense.cpp b/src/current_sense/GenericCurrentSense.cpp index 9d90f0ca..54c4f12e 100644 --- a/src/current_sense/GenericCurrentSense.cpp +++ b/src/current_sense/GenericCurrentSense.cpp @@ -54,7 +54,7 @@ PhaseCurrent_s GenericCurrentSense::getPhaseCurrents(){ // returns flag // 0 - fail // 1 - success and nothing changed -int GenericCurrentSense::driverAlign(float voltage){ +int GenericCurrentSense::driverAlign(float voltage, bool modulation_centered){ _UNUSED(voltage) ; // remove unused parameter warning int exit_flag = 1; if(skip_align) return exit_flag; diff --git a/src/current_sense/GenericCurrentSense.h b/src/current_sense/GenericCurrentSense.h index a63df49d..02bf8fa9 100644 --- a/src/current_sense/GenericCurrentSense.h +++ b/src/current_sense/GenericCurrentSense.h @@ -20,7 +20,7 @@ class GenericCurrentSense: public CurrentSense{ // CurrentSense interface implementing functions int init() override; PhaseCurrent_s getPhaseCurrents() override; - int driverAlign(float align_voltage) override; + int driverAlign(float align_voltage, bool modulation_centered) override; PhaseCurrent_s (*readCallback)() = nullptr; //!< function pointer to sensor reading From dbc62a1d5e913af6436d8c36eb57cda3cebbb152 Mon Sep 17 00:00:00 2001 From: askuric Date: Mon, 15 Jul 2024 14:03:51 +0200 Subject: [PATCH 49/57] a correction for stepper --- src/common/base_classes/CurrentSense.cpp | 15 +++++++-------- 1 file changed, 7 insertions(+), 8 deletions(-) diff --git a/src/common/base_classes/CurrentSense.cpp b/src/common/base_classes/CurrentSense.cpp index 84bc3547..a3966ad6 100644 --- a/src/common/base_classes/CurrentSense.cpp +++ b/src/common/base_classes/CurrentSense.cpp @@ -392,12 +392,11 @@ int CurrentSense::alignBLDCDriver(float voltage, BLDCDriver* bldc_driver, bool m // 3 - success but gains inverted // 4 - success but pins reconfigured and gains inverted int CurrentSense::alignStepperDriver(float voltage, StepperDriver* stepper_driver, bool modulation_centered){ - + + _UNUSED(modulation_centered); + bool phases_switched = 0; bool phases_inverted = 0; - - float zero = 0; - if(modulation_centered) zero = driver->voltage_limit/2.0; if(!_isset(pinA) || !_isset(pinB)){ SIMPLEFOC_DEBUG("CS: Pins A & B not specified!"); @@ -407,13 +406,13 @@ int CurrentSense::alignStepperDriver(float voltage, StepperDriver* stepper_drive // set phase A active and phases B down // ramp 300ms for(int i=0; i < 100; i++){ - stepper_driver->setPwm(voltage/100.0*((float)i), zero); + stepper_driver->setPwm(voltage/100.0*((float)i), 0); _delay(3); } _delay(500); PhaseCurrent_s c = readAverageCurrents(); // disable the phases - stepper_driver->setPwm(zero, zero); + stepper_driver->setPwm(0, 0); if (fabs(c.a) < 0.1f && fabs(c.b) < 0.1f ){ SIMPLEFOC_DEBUG("CS: Err too low current!"); return 0; // measurement current too low @@ -442,12 +441,12 @@ int CurrentSense::alignStepperDriver(float voltage, StepperDriver* stepper_drive // set phase B active and phases A down // ramp 300ms for(int i=0; i < 100; i++){ - stepper_driver->setPwm(zero, voltage/100.0*((float)i)+zero); + stepper_driver->setPwm(0, voltage/100.0*((float)i)); _delay(3); } _delay(500); c = readAverageCurrents(); - stepper_driver->setPwm(zero, zero); + stepper_driver->setPwm(0, 0); // phase B should be aligned // 1) we just need to verify that it has been measured From bd21d2b1ecd66293e4982e9f4c542f4b3ddc7f6f Mon Sep 17 00:00:00 2001 From: Antun Skuric <36178713+askuric@users.noreply.github.com> Date: Sat, 20 Jul 2024 16:06:26 +0200 Subject: [PATCH 50/57] Update CurrentSense.cpp --- src/common/base_classes/CurrentSense.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/common/base_classes/CurrentSense.cpp b/src/common/base_classes/CurrentSense.cpp index a3966ad6..4813dc3b 100644 --- a/src/common/base_classes/CurrentSense.cpp +++ b/src/common/base_classes/CurrentSense.cpp @@ -450,8 +450,8 @@ int CurrentSense::alignStepperDriver(float voltage, StepperDriver* stepper_drive // phase B should be aligned // 1) we just need to verify that it has been measured - if (fabs(c.a) < 0.1f && fabs(c.b) < 0.1f ){ - SIMPLEFOC_DEBUG("CS: Err too low current!"); + if (fabs(c.b) < 0.1f ){ + SIMPLEFOC_DEBUG("CS: Err too low current on B!"); return 0; // measurement current too low } // 2) check if measured current a is positive and invert if not From d429148f85c775a9fe43c03f8f0394024efc118c Mon Sep 17 00:00:00 2001 From: askuric Date: Sun, 21 Jul 2024 13:08:23 +0200 Subject: [PATCH 51/57] refactored examples --- .../single_full_control_example.ino | 2 +- .../open_loop_velocity_example.ino | 11 ++++- .../inline_current_sense_test.ino | 5 +- .../bldc_driver_3pwm_standalone.ino | 7 ++- .../bldc_driver_6pwm_standalone.ino | 7 ++- .../stepper_driver_2pwm_standalone.ino | 7 ++- .../stepper_driver_4pwm_standalone.ino | 7 ++- .../hall_sensor_example.ino | 9 ---- ...all_sensor_hardware_interrupts_example.ino | 48 +++++++++++++++++++ 9 files changed, 82 insertions(+), 21 deletions(-) create mode 100644 examples/utils/sensor_test/hall_sensors/hall_sensor_hardware_interrupts_example/hall_sensor_hardware_interrupts_example.ino diff --git a/examples/hardware_specific_examples/SimpleFOCShield/version_v3/single_full_control_example/single_full_control_example.ino b/examples/hardware_specific_examples/SimpleFOCShield/version_v3/single_full_control_example/single_full_control_example.ino index 9c80a36f..6359f201 100644 --- a/examples/hardware_specific_examples/SimpleFOCShield/version_v3/single_full_control_example/single_full_control_example.ino +++ b/examples/hardware_specific_examples/SimpleFOCShield/version_v3/single_full_control_example/single_full_control_example.ino @@ -13,7 +13,7 @@ void doB(){encoder.handleB();} // inline current sensor instance // ACS712-05B has the resolution of 0.185mV per Amp -InlineCurrentSense current_sense = InlineCurrentSense(1.0f, 0.185f, A0, A2); +InlineCurrentSense current_sense = InlineCurrentSense(185.0f, A0, A2); // commander communication instance Commander command = Commander(Serial); diff --git a/examples/motion_control/open_loop_motor_control/open_loop_velocity_example/open_loop_velocity_example.ino b/examples/motion_control/open_loop_motor_control/open_loop_velocity_example/open_loop_velocity_example.ino index 075dc9c6..546ac3ea 100644 --- a/examples/motion_control/open_loop_motor_control/open_loop_velocity_example/open_loop_velocity_example.ino +++ b/examples/motion_control/open_loop_motor_control/open_loop_velocity_example/open_loop_velocity_example.ino @@ -36,7 +36,10 @@ void setup() { // as a protection measure for the low-resistance motors // this value is fixed on startup driver.voltage_limit = 6; - driver.init(); + if(!driver.init()){ + Serial.println("Driver init failed!"); + return; + } // link the motor and the driver motor.linkDriver(&driver); @@ -50,7 +53,11 @@ void setup() { motor.controller = MotionControlType::velocity_openloop; // init motor hardware - motor.init(); + if(!motor.init()){ + Serial.println("Motor init failed!"); + return; + } + // add target command T command.add('T', doTarget, "target velocity"); diff --git a/examples/utils/current_sense_test/inline_current_sense_test/inline_current_sense_test.ino b/examples/utils/current_sense_test/inline_current_sense_test/inline_current_sense_test.ino index 195021eb..1198cdcd 100644 --- a/examples/utils/current_sense_test/inline_current_sense_test/inline_current_sense_test.ino +++ b/examples/utils/current_sense_test/inline_current_sense_test/inline_current_sense_test.ino @@ -19,7 +19,10 @@ void setup() { SimpleFOCDebug::enable(&Serial); // initialise the current sensing - current_sense.init(); + if(!current_sense.init()){ + Serial.println("Current sense init failed."); + return; + } // for SimpleFOCShield v2.01/v2.0.2 current_sense.gain_b *= -1; diff --git a/examples/utils/driver_standalone_test/bldc_driver_3pwm_standalone/bldc_driver_3pwm_standalone.ino b/examples/utils/driver_standalone_test/bldc_driver_3pwm_standalone/bldc_driver_3pwm_standalone.ino index 82716353..eef793d7 100644 --- a/examples/utils/driver_standalone_test/bldc_driver_3pwm_standalone/bldc_driver_3pwm_standalone.ino +++ b/examples/utils/driver_standalone_test/bldc_driver_3pwm_standalone/bldc_driver_3pwm_standalone.ino @@ -23,11 +23,14 @@ void setup() { driver.voltage_limit = 12; // driver init - driver.init(); + if (!driver.init()){ + Serial.println("Driver init failed!"); + return; + } // enable driver driver.enable(); - + Serial.println("Driver ready!"); _delay(1000); } diff --git a/examples/utils/driver_standalone_test/bldc_driver_6pwm_standalone/bldc_driver_6pwm_standalone.ino b/examples/utils/driver_standalone_test/bldc_driver_6pwm_standalone/bldc_driver_6pwm_standalone.ino index 1ed37441..56a1afbe 100644 --- a/examples/utils/driver_standalone_test/bldc_driver_6pwm_standalone/bldc_driver_6pwm_standalone.ino +++ b/examples/utils/driver_standalone_test/bldc_driver_6pwm_standalone/bldc_driver_6pwm_standalone.ino @@ -24,11 +24,14 @@ void setup() { driver.dead_zone = 0.05f; // driver init - driver.init(); + if (!driver.init()){ + Serial.println("Driver init failed!"); + return; + } // enable driver driver.enable(); - + Serial.println("Driver ready!"); _delay(1000); } diff --git a/examples/utils/driver_standalone_test/stepper_driver_2pwm_standalone/stepper_driver_2pwm_standalone.ino b/examples/utils/driver_standalone_test/stepper_driver_2pwm_standalone/stepper_driver_2pwm_standalone.ino index 889143e2..59343a14 100644 --- a/examples/utils/driver_standalone_test/stepper_driver_2pwm_standalone/stepper_driver_2pwm_standalone.ino +++ b/examples/utils/driver_standalone_test/stepper_driver_2pwm_standalone/stepper_driver_2pwm_standalone.ino @@ -29,11 +29,14 @@ void setup() { driver.voltage_limit = 12; // driver init - driver.init(); + if (!driver.init()){ + Serial.println("Driver init failed!"); + return; + } // enable driver driver.enable(); - + Serial.println("Driver ready!"); _delay(1000); } diff --git a/examples/utils/driver_standalone_test/stepper_driver_4pwm_standalone/stepper_driver_4pwm_standalone.ino b/examples/utils/driver_standalone_test/stepper_driver_4pwm_standalone/stepper_driver_4pwm_standalone.ino index 95dcc589..a58d7940 100644 --- a/examples/utils/driver_standalone_test/stepper_driver_4pwm_standalone/stepper_driver_4pwm_standalone.ino +++ b/examples/utils/driver_standalone_test/stepper_driver_4pwm_standalone/stepper_driver_4pwm_standalone.ino @@ -24,11 +24,14 @@ void setup() { driver.voltage_limit = 12; // driver init - driver.init(); + if (!driver.init()){ + Serial.println("Driver init failed!"); + return; + } // enable driver driver.enable(); - + Serial.println("Driver ready!"); _delay(1000); } diff --git a/examples/utils/sensor_test/hall_sensors/hall_sensor_example/hall_sensor_example.ino b/examples/utils/sensor_test/hall_sensors/hall_sensor_example/hall_sensor_example.ino index c4777baa..cc8dfdb8 100644 --- a/examples/utils/sensor_test/hall_sensors/hall_sensor_example/hall_sensor_example.ino +++ b/examples/utils/sensor_test/hall_sensors/hall_sensor_example/hall_sensor_example.ino @@ -13,13 +13,6 @@ // - pp - pole pairs HallSensor sensor = HallSensor(2, 3, 4, 14); -// Interrupt routine intialisation -// channel A and B callbacks -void doA(){sensor.handleA();} -void doB(){sensor.handleB();} -void doC(){sensor.handleC();} - - void setup() { // monitoring port Serial.begin(115200); @@ -29,8 +22,6 @@ void setup() { // initialise encoder hardware sensor.init(); - // hardware interrupt enable - sensor.enableInterrupts(doA, doB, doC); Serial.println("Sensor ready"); _delay(1000); diff --git a/examples/utils/sensor_test/hall_sensors/hall_sensor_hardware_interrupts_example/hall_sensor_hardware_interrupts_example.ino b/examples/utils/sensor_test/hall_sensors/hall_sensor_hardware_interrupts_example/hall_sensor_hardware_interrupts_example.ino new file mode 100644 index 00000000..c4777baa --- /dev/null +++ b/examples/utils/sensor_test/hall_sensors/hall_sensor_hardware_interrupts_example/hall_sensor_hardware_interrupts_example.ino @@ -0,0 +1,48 @@ +/** + * Hall sensor example code + * + * This is a code intended to test the hall sensors connections and to demonstrate the hall sensor setup. + * + */ + +#include + +// Hall sensor instance +// HallSensor(int hallA, int hallB , int cpr, int index) +// - hallA, hallB, hallC - HallSensor A, B and C pins +// - pp - pole pairs +HallSensor sensor = HallSensor(2, 3, 4, 14); + +// Interrupt routine intialisation +// channel A and B callbacks +void doA(){sensor.handleA();} +void doB(){sensor.handleB();} +void doC(){sensor.handleC();} + + +void setup() { + // monitoring port + Serial.begin(115200); + + // check if you need internal pullups + sensor.pullup = Pullup::USE_EXTERN; + + // initialise encoder hardware + sensor.init(); + // hardware interrupt enable + sensor.enableInterrupts(doA, doB, doC); + + Serial.println("Sensor ready"); + _delay(1000); +} + +void loop() { + // iterative function updating the sensor internal variables + // it is usually called in motor.loopFOC() + sensor.update(); + // display the angle and the angular velocity to the terminal + Serial.print(sensor.getAngle()); + Serial.print("\t"); + Serial.println(sensor.getVelocity()); + delay(100); +} From b2647a63cfd33b864602f31ffead17fab9954171 Mon Sep 17 00:00:00 2001 From: askuric Date: Sun, 21 Jul 2024 13:09:32 +0200 Subject: [PATCH 52/57] error in timer align (forgotten ifdef --- .../hardware_specific/stm32/stm32_mcu.cpp | 53 +++++++++++++++++-- 1 file changed, 48 insertions(+), 5 deletions(-) diff --git a/src/drivers/hardware_specific/stm32/stm32_mcu.cpp b/src/drivers/hardware_specific/stm32/stm32_mcu.cpp index 0fdec2f0..4009281e 100644 --- a/src/drivers/hardware_specific/stm32/stm32_mcu.cpp +++ b/src/drivers/hardware_specific/stm32/stm32_mcu.cpp @@ -249,7 +249,15 @@ int _getInternalSourceTrigger(HardwareTimer* master, HardwareTimer* slave) { TIM_TypeDef *TIM_slave = slave->getHandle()->Instance; #if defined(TIM1) && defined(LL_TIM_TS_ITR0) if (TIM_master == TIM1){ - if(TIM_slave == TIM2 || TIM_slave == TIM3 || TIM_slave == TIM4) return LL_TIM_TS_ITR0; + #if defined(TIM2) + if(TIM_slave == TIM2) return LL_TIM_TS_ITR0; + #endif + #if defined(TIM3) + else if(TIM_slave == TIM3) return LL_TIM_TS_ITR0; + #endif + #if defined(TIM4) + else if(TIM_slave == TIM4) return LL_TIM_TS_ITR0; + #endif #if defined(TIM8) else if(TIM_slave == TIM8) return LL_TIM_TS_ITR0; #endif @@ -257,7 +265,15 @@ int _getInternalSourceTrigger(HardwareTimer* master, HardwareTimer* slave) { #endif #if defined(TIM2) && defined(LL_TIM_TS_ITR1) else if (TIM_master == TIM2){ - if(TIM_slave == TIM1 || TIM_slave == TIM3 || TIM_slave == TIM4) return LL_TIM_TS_ITR1; + #if defined(TIM1) + if(TIM_slave == TIM1) return LL_TIM_TS_ITR1; + #endif + #if defined(TIM3) + else if(TIM_slave == TIM3) return LL_TIM_TS_ITR1; + #endif + #if defined(TIM4) + else if(TIM_slave == TIM4) return LL_TIM_TS_ITR1; + #endif #if defined(TIM8) else if(TIM_slave == TIM8) return LL_TIM_TS_ITR1; #endif @@ -268,7 +284,15 @@ int _getInternalSourceTrigger(HardwareTimer* master, HardwareTimer* slave) { #endif #if defined(TIM3) && defined(LL_TIM_TS_ITR2) else if (TIM_master == TIM3){ - if(TIM_slave== TIM1 || TIM_slave == TIM2 || TIM_slave == TIM4) return LL_TIM_TS_ITR2; + #if defined(TIM1) + if(TIM_slave == TIM1) return LL_TIM_TS_ITR2; + #endif + #if defined(TIM2) + else if(TIM_slave == TIM2) return LL_TIM_TS_ITR2; + #endif + #if defined(TIM4) + else if(TIM_slave == TIM4) return LL_TIM_TS_ITR2; + #endif #if defined(TIM5) else if(TIM_slave == TIM5) return LL_TIM_TS_ITR1; #endif @@ -276,7 +300,15 @@ int _getInternalSourceTrigger(HardwareTimer* master, HardwareTimer* slave) { #endif #if defined(TIM4) && defined(LL_TIM_TS_ITR3) else if (TIM_master == TIM4){ - if(TIM_slave == TIM1 || TIM_slave == TIM2 || TIM_slave == TIM3) return LL_TIM_TS_ITR3; + #if defined(TIM1) + if(TIM_slave == TIM1) return LL_TIM_TS_ITR3; + #endif + #if defined(TIM2) + else if(TIM_slave == TIM2) return LL_TIM_TS_ITR3; + #endif + #if defined(TIM3) + else if(TIM_slave == TIM3) return LL_TIM_TS_ITR3; + #endif #if defined(TIM8) else if(TIM_slave == TIM8) return LL_TIM_TS_ITR2; #endif @@ -288,9 +320,13 @@ int _getInternalSourceTrigger(HardwareTimer* master, HardwareTimer* slave) { #if defined(TIM5) else if (TIM_master == TIM5){ #if !defined(STM32L4xx) // only difference between F4,F1 and L4 + #if defined(TIM1) if(TIM_slave == TIM1) return LL_TIM_TS_ITR0; + #endif + #if defined(TIM3) else if(TIM_slave == TIM3) return LL_TIM_TS_ITR2; #endif + #endif #if defined(TIM8) if(TIM_slave == TIM8) return LL_TIM_TS_ITR3; #endif @@ -298,8 +334,15 @@ int _getInternalSourceTrigger(HardwareTimer* master, HardwareTimer* slave) { #endif #if defined(TIM8) else if (TIM_master == TIM8){ + #if defined(TIM2) if(TIM_slave==TIM2) return LL_TIM_TS_ITR1; - else if(TIM_slave ==TIM4 || TIM_slave ==TIM5) return LL_TIM_TS_ITR3; + #endif + #if defined(TIM4) + else if(TIM_slave == TIM4) return LL_TIM_TS_ITR3; + #endif + #if defined(TIM5) + else if(TIM_slave == TIM5) return LL_TIM_TS_ITR3; + #endif } #endif return -1; // combination not supported From 150a40b284369ce207f1d8838a3ad6198b604459 Mon Sep 17 00:00:00 2001 From: askuric Date: Sun, 21 Jul 2024 13:14:35 +0200 Subject: [PATCH 53/57] error in exampels --- .../open_loop_velocity_example.ino | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/examples/motion_control/open_loop_motor_control/open_loop_velocity_example/open_loop_velocity_example.ino b/examples/motion_control/open_loop_motor_control/open_loop_velocity_example/open_loop_velocity_example.ino index 546ac3ea..c439bf70 100644 --- a/examples/motion_control/open_loop_motor_control/open_loop_velocity_example/open_loop_velocity_example.ino +++ b/examples/motion_control/open_loop_motor_control/open_loop_velocity_example/open_loop_velocity_example.ino @@ -53,11 +53,7 @@ void setup() { motor.controller = MotionControlType::velocity_openloop; // init motor hardware - if(!motor.init()){ - Serial.println("Motor init failed!"); - return; - } - + motor.init(); // add target command T command.add('T', doTarget, "target velocity"); From 05672d28187320c10f74f8fa382e92b09cfadce0 Mon Sep 17 00:00:00 2001 From: askuric Date: Sun, 21 Jul 2024 13:20:16 +0200 Subject: [PATCH 54/57] motor init returns success --- .../open_loop_position_example.ino | 10 ++++++++-- .../open_loop_velocity_example.ino | 5 ++++- src/BLDCMotor.cpp | 3 ++- src/BLDCMotor.h | 2 +- src/StepperMotor.cpp | 3 ++- src/StepperMotor.h | 2 +- src/common/base_classes/FOCMotor.h | 2 +- 7 files changed, 19 insertions(+), 8 deletions(-) diff --git a/examples/motion_control/open_loop_motor_control/open_loop_position_example/open_loop_position_example.ino b/examples/motion_control/open_loop_motor_control/open_loop_position_example/open_loop_position_example.ino index 6bcfdff9..c1ff7bbc 100644 --- a/examples/motion_control/open_loop_motor_control/open_loop_position_example/open_loop_position_example.ino +++ b/examples/motion_control/open_loop_motor_control/open_loop_position_example/open_loop_position_example.ino @@ -36,7 +36,10 @@ void setup() { // as a protection measure for the low-resistance motors // this value is fixed on startup driver.voltage_limit = 6; - driver.init(); + if(!driver.init()){ + Serial.println("Driver init failed!"); + return; + } // link the motor and the driver motor.linkDriver(&driver); @@ -52,7 +55,10 @@ void setup() { motor.controller = MotionControlType::angle_openloop; // init motor hardware - motor.init(); + if(!motor.init()){ + Serial.println("Motor init failed!"); + return; + } // add target command T command.add('T', doTarget, "target angle"); diff --git a/examples/motion_control/open_loop_motor_control/open_loop_velocity_example/open_loop_velocity_example.ino b/examples/motion_control/open_loop_motor_control/open_loop_velocity_example/open_loop_velocity_example.ino index c439bf70..b1bc2760 100644 --- a/examples/motion_control/open_loop_motor_control/open_loop_velocity_example/open_loop_velocity_example.ino +++ b/examples/motion_control/open_loop_motor_control/open_loop_velocity_example/open_loop_velocity_example.ino @@ -53,7 +53,10 @@ void setup() { motor.controller = MotionControlType::velocity_openloop; // init motor hardware - motor.init(); + if(!motor.init()){ + Serial.println("Motor init failed!"); + return; + } // add target command T command.add('T', doTarget, "target velocity"); diff --git a/src/BLDCMotor.cpp b/src/BLDCMotor.cpp index 792381d5..c127949e 100644 --- a/src/BLDCMotor.cpp +++ b/src/BLDCMotor.cpp @@ -67,7 +67,7 @@ void BLDCMotor::init() { if (!driver || !driver->initialized) { motor_status = FOCMotorStatus::motor_init_failed; SIMPLEFOC_DEBUG("MOT: Init not possible, driver not initialized"); - return; + return 0; } motor_status = FOCMotorStatus::motor_initializing; SIMPLEFOC_DEBUG("MOT: Init"); @@ -105,6 +105,7 @@ void BLDCMotor::init() { enable(); _delay(500); motor_status = FOCMotorStatus::motor_uncalibrated; + return 1; } diff --git a/src/BLDCMotor.h b/src/BLDCMotor.h index 41a5a1c0..c261e405 100644 --- a/src/BLDCMotor.h +++ b/src/BLDCMotor.h @@ -40,7 +40,7 @@ class BLDCMotor: public FOCMotor BLDCDriver* driver; /** Motor hardware init function */ - void init() override; + int init() override; /** Motor disable function */ void disable() override; /** Motor enable function */ diff --git a/src/StepperMotor.cpp b/src/StepperMotor.cpp index 5db9afa2..7b4a5820 100644 --- a/src/StepperMotor.cpp +++ b/src/StepperMotor.cpp @@ -37,7 +37,7 @@ void StepperMotor::init() { if (!driver || !driver->initialized) { motor_status = FOCMotorStatus::motor_init_failed; SIMPLEFOC_DEBUG("MOT: Init not possible, driver not initialized"); - return; + return 0; } motor_status = FOCMotorStatus::motor_initializing; SIMPLEFOC_DEBUG("MOT: Init"); @@ -70,6 +70,7 @@ void StepperMotor::init() { _delay(500); motor_status = FOCMotorStatus::motor_uncalibrated; + return 1; } diff --git a/src/StepperMotor.h b/src/StepperMotor.h index f76e7bcf..7e7810d8 100644 --- a/src/StepperMotor.h +++ b/src/StepperMotor.h @@ -43,7 +43,7 @@ class StepperMotor: public FOCMotor StepperDriver* driver; /** Motor hardware init function */ - void init() override; + int init() override; /** Motor disable function */ void disable() override; /** Motor enable function */ diff --git a/src/common/base_classes/FOCMotor.h b/src/common/base_classes/FOCMotor.h index b5ba2e96..8ae0e8af 100644 --- a/src/common/base_classes/FOCMotor.h +++ b/src/common/base_classes/FOCMotor.h @@ -78,7 +78,7 @@ class FOCMotor FOCMotor(); /** Motor hardware init function */ - virtual void init()=0; + virtual int init()=0; /** Motor disable function */ virtual void disable()=0; /** Motor enable function */ From ea8eda83d265f3589a2d0ae6669373641daedf7b Mon Sep 17 00:00:00 2001 From: askuric Date: Sun, 21 Jul 2024 13:25:33 +0200 Subject: [PATCH 55/57] typo in transition from void to int --- src/BLDCMotor.cpp | 2 +- src/StepperMotor.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/BLDCMotor.cpp b/src/BLDCMotor.cpp index c127949e..501a8bc9 100644 --- a/src/BLDCMotor.cpp +++ b/src/BLDCMotor.cpp @@ -63,7 +63,7 @@ void BLDCMotor::linkDriver(BLDCDriver* _driver) { } // init hardware pins -void BLDCMotor::init() { +int BLDCMotor::init() { if (!driver || !driver->initialized) { motor_status = FOCMotorStatus::motor_init_failed; SIMPLEFOC_DEBUG("MOT: Init not possible, driver not initialized"); diff --git a/src/StepperMotor.cpp b/src/StepperMotor.cpp index 7b4a5820..1d8f3147 100644 --- a/src/StepperMotor.cpp +++ b/src/StepperMotor.cpp @@ -33,7 +33,7 @@ void StepperMotor::linkDriver(StepperDriver* _driver) { } // init hardware pins -void StepperMotor::init() { +int StepperMotor::init() { if (!driver || !driver->initialized) { motor_status = FOCMotorStatus::motor_init_failed; SIMPLEFOC_DEBUG("MOT: Init not possible, driver not initialized"); From 5afea2b88e0569adb3cc2834c79a4c35280b4a35 Mon Sep 17 00:00:00 2001 From: askuric Date: Sun, 21 Jul 2024 13:26:18 +0200 Subject: [PATCH 56/57] preparations for v2.3.4 --- library.properties | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/library.properties b/library.properties index 0a2606b2..5daa49d9 100644 --- a/library.properties +++ b/library.properties @@ -1,5 +1,5 @@ name=Simple FOC -version=2.3.3 +version=2.3.4 author=Simplefoc maintainer=Simplefoc sentence=A library demistifying FOC for BLDC motors From 37ca90d673a997b72c58247f0aa1f4ca10f21c94 Mon Sep 17 00:00:00 2001 From: askuric Date: Sun, 21 Jul 2024 14:05:27 +0200 Subject: [PATCH 57/57] readme for v2.3.4 --- README.md | 24 ++++++++++++++++++++---- 1 file changed, 20 insertions(+), 4 deletions(-) diff --git a/README.md b/README.md index 543e6533..4fbb7517 100644 --- a/README.md +++ b/README.md @@ -30,10 +30,26 @@ Therefore this is an attempt to: - Many many more boards developed by the community members, see [SimpleFOCCommunity](https://community.simplefoc.com/) > NEXT RELEASE 📢 : SimpleFOClibrary v2.3.4 -> - Current sensing support for Stepper motors (lowside and inline) - - -## Arduino *SimpleFOClibrary* v2.3.3 +> - ESP32 MCUs extended support [#414](https://github.com/simplefoc/Arduino-FOC/pull/414) +> - Transition to the arduino-esp32 version v3.x (ESP-IDF v5.x) [#387](https://github.com/espressif/arduino-esp32/releases) +> - New support for MCPWM driver +> - New support for LEDC drivers - center-aligned PWM and 6PWM available +> - Rewritten and simplified the fast ADC driver code (`adcRead`) - for low-side and inline current sensing. +> - Stepper motors current sensing support [#421](https://github.com/simplefoc/Arduino-FOC/pull/421) +> - Support for current sensing (low-side and inline) - [see in docs](https://docs.simplefoc.com/current_sense) +> - Support for true FOC control - `foc_current` torque control - [see in docs](https://docs.simplefoc.com/motion_control) +> - New current sense alignment procedure [#422](https://github.com/simplefoc/Arduino-FOC/pull/422) - [see in docs](https://docs.simplefoc.com/current_sense_align) +> - Support for steppers +> - Much more robust and reliable +> - More verbose and informative +> - Support for HallSensors without interrupts [#424](https://docs.simplefoc.com/https://github.com/simplefoc/Arduino-FOC/pull/424) - [see in docs](hall_sensors) +> - Docs +> - A short guide to debugging of common issues +> - A short guide to the units in the library - [see in docs](https://docs.simplefoc.com/library_units) +> - See the complete list of bugfixes and new features of v2.3.4 [fixes and PRs](https://github.com/simplefoc/Arduino-FOC/milestone/11) + + +## Arduino *SimpleFOClibrary* v2.3.4