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/26] 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/26] 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/26] 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/26] 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/26] 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 3b7fa90b1e39d033138cc5bcd5a2140456957c5d Mon Sep 17 00:00:00 2001 From: askuric Date: Mon, 20 May 2024 18:01:31 +0200 Subject: [PATCH 06/26] 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 07/26] 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 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 08/26] 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 09/26] 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 463eb31f354dd80c29f0ae4f16d0c1733f208a0a Mon Sep 17 00:00:00 2001 From: Antun Skuric <36178713+askuric@users.noreply.github.com> Date: Thu, 11 Jul 2024 09:22:59 +0200 Subject: [PATCH 10/26] Update README.md --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 86551f79..c456b1d4 100644 --- a/README.md +++ b/README.md @@ -13,7 +13,7 @@ ![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://ardubadge.simplefoc.com?lib=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) From d2a409338f9d796c4ff86ecad0f42b576d05e522 Mon Sep 17 00:00:00 2001 From: askuric Date: Sat, 13 Jul 2024 14:40:43 +0200 Subject: [PATCH 11/26] 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 12/26] 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 13/26] 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 14/26] 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 15/26] 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 16/26] 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 17/26] 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 18/26] 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 19/26] 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 20/26] 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 21/26] 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 22/26] 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 23/26] 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 24/26] 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 25/26] 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 26/26] 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