From 3b7fa90b1e39d033138cc5bcd5a2140456957c5d Mon Sep 17 00:00:00 2001 From: askuric Date: Mon, 20 May 2024 18:01:31 +0200 Subject: [PATCH 1/9] 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 2/9] 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 d2a409338f9d796c4ff86ecad0f42b576d05e522 Mon Sep 17 00:00:00 2001 From: askuric Date: Sat, 13 Jul 2024 14:40:43 +0200 Subject: [PATCH 3/9] 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 842b3615eec1b74cdb4729c03a2c64d21d7135d8 Mon Sep 17 00:00:00 2001 From: askuric Date: Sun, 14 Jul 2024 13:15:36 +0200 Subject: [PATCH 4/9] 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 5/9] 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 6/9] 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 7/9] 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 8/9] 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 9/9] 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