diff --git a/README.md b/README.md
index 5d31c84d..aca53ac5 100644
--- a/README.md
+++ b/README.md
@@ -30,7 +30,7 @@ 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/)
-> NEXT RELEASE 📢 : SimpleFOClibrary v2.3.6
+> NEXT RELEASE 📢 : SimpleFOClibrary v2.4.0
>
> - STM32 bugfix
> - BG341 low-side current sense sync was lost in v2.3.5 - fixed [#482](https://github.com/simplefoc/Arduino-FOC/pull/482)
diff --git a/library.properties b/library.properties
index c50cf1e3..49660c4b 100644
--- a/library.properties
+++ b/library.properties
@@ -1,5 +1,5 @@
name=Simple FOC
-version=2.3.6
+version=2.4.0
author=Simplefoc
maintainer=Simplefoc
sentence=A library demistifying FOC for BLDC motors
diff --git a/src/BLDCMotor.cpp b/src/BLDCMotor.cpp
index a30dc1c5..0b339a2e 100644
--- a/src/BLDCMotor.cpp
+++ b/src/BLDCMotor.cpp
@@ -35,7 +35,7 @@ int trap_150_map[12][3] = {
// - R - motor phase resistance
// - KV - motor kv rating (rmp/v)
// - L - motor phase inductance
-BLDCMotor::BLDCMotor(int pp, float _R, float _KV, float _inductance)
+BLDCMotor::BLDCMotor(int pp, float _R, float _KV, float _Lq, float _Ld)
: FOCMotor()
{
// save pole pairs number
@@ -46,7 +46,8 @@ BLDCMotor::BLDCMotor(int pp, float _R, float _KV, float _inductance)
// 1/sqrt(3) - rms value
KV_rating = _KV;
// save phase inductance
- phase_inductance = _inductance;
+ phase_inductance_dq = {_Ld, _Lq};
+ phase_inductance = _Lq; // FOR BACKWARDS COMPATIBILITY
// torque control type is voltage by default
torque_controller = TorqueControlType::voltage;
@@ -64,11 +65,11 @@ void BLDCMotor::linkDriver(BLDCDriver* _driver) {
int BLDCMotor::init() {
if (!driver || !driver->initialized) {
motor_status = FOCMotorStatus::motor_init_failed;
- SIMPLEFOC_DEBUG("MOT: Init not possible, driver not initialized");
+ SIMPLEFOC_MOTOR_ERROR("Init not possible, driver not init");
return 0;
}
motor_status = FOCMotorStatus::motor_initializing;
- SIMPLEFOC_DEBUG("MOT: Init");
+ SIMPLEFOC_MOTOR_DEBUG("Init");
// sanity check for the voltage limit configuration
if(voltage_limit > driver->voltage_limit) voltage_limit = driver->voltage_limit;
@@ -80,6 +81,11 @@ int BLDCMotor::init() {
updateVoltageLimit(voltage_limit);
updateVelocityLimit(velocity_limit);
+ if(_isset(phase_inductance) && !(_isset(phase_inductance_dq.q))) {
+ // if only single inductance value is set, use it for both d and q axis
+ phase_inductance_dq = {phase_inductance, phase_inductance};
+ }
+
// if using open loop control, set a CW as the default direction if not already set
// only if no sensor is used
if(!sensor){
@@ -92,7 +98,7 @@ int BLDCMotor::init() {
_delay(500);
// enable motor
- SIMPLEFOC_DEBUG("MOT: Enable driver.");
+ SIMPLEFOC_MOTOR_DEBUG("Enable driver.");
enable();
_delay(500);
motor_status = FOCMotorStatus::motor_uncalibrated;
@@ -133,362 +139,16 @@ void BLDCMotor::enable()
/**
FOC functions
*/
-// FOC initialization function
-int BLDCMotor::initFOC() {
- int exit_flag = 1;
-
- motor_status = FOCMotorStatus::motor_calibrating;
-
- // align motor if necessary
- // alignment necessary for encoders!
- // sensor and motor alignment - can be skipped
- // by setting motor.sensor_direction and motor.zero_electric_angle
- if(sensor){
- exit_flag *= alignSensor();
- // added the shaft_angle update
- sensor->update();
- 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)){
- exit_flag = 1;
- SIMPLEFOC_DEBUG("MOT: Openloop only!");
- }else{
- exit_flag = 0; // no FOC without sensor
- }
- }
-
- 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, modulation_centered);
- 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;
-
- // v2.3.3 fix for R_AVR_7_PCREL against symbol" bug for AVR boards
- // TODO figure out why this works
- float voltage_align = voltage_sensor_align;
-
- // if unknown natural direction
- if(sensor_direction == Direction::UNKNOWN){
-
- // 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_align, 0, angle);
- sensor->update();
- _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_align, 0, angle);
- sensor->update();
- _delay(2);
- }
- sensor->update();
- float end_angle = sensor->getAngle();
- // setPhaseVoltage(0, 0, 0);
- _delay(200);
- // determine the direction the sensor moved
- float moved = fabs(mid_angle - end_angle);
- if (moved 0.5f); // 0.5f is arbitrary number it can be lower or higher!
- if( pp_check_result==false ) {
- 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_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);
- SIMPLEFOC_DEBUG("MOT: Zero elec. angle: ", zero_electric_angle);
- // stop everything
- setPhaseVoltage(0, 0, 0);
- _delay(200);
- } else { SIMPLEFOC_DEBUG("MOT: Skip offset calib."); }
- return exit_flag;
-}
-
-// 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 loop time measurement
- updateLoopTime();
-
- // 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 disabled do nothing
- if(!enabled) return;
- // if open-loop do nothing
- if( controller==MotionControlType::angle_openloop || controller==MotionControlType::velocity_openloop )
- // calculate the open loop electirical angle
- electrical_angle = _electricalAngle((shaft_angle), pole_pairs);
- else
- // 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:
- voltage.q = _constrain(current_sp, -voltage_limit, voltage_limit) + feed_forward_voltage.q;
- voltage.d = feed_forward_voltage.d;
- break;
- case TorqueControlType::estimated_current:
- if(! _isset(phase_resistance)) return;
- // constrain current setpoint
- current_sp = _constrain(current_sp, -current_limit, current_limit) + feed_forward_current.q; // desired current is the setpoint
- // calculate the back-emf voltage if KV_rating available U_bemf = vel*(1/KV)
- if (_isset(KV_rating)) voltage_bemf = shaft_velocity/(KV_rating*_SQRT3)/_RPM_TO_RADS;
- // filter the value values
- current.q = LPF_current_q(current_sp);
- // calculate the phase voltage
- voltage.q = current.q * phase_resistance + voltage_bemf;
- // constrain voltage within limits
- voltage.q = _constrain(voltage.q, -voltage_limit, voltage_limit) + feed_forward_voltage.q;
- // d voltage - lag compensation
- if(_isset(phase_inductance)) voltage.d = _constrain( -current_sp*shaft_velocity*pole_pairs*phase_inductance, -voltage_limit, voltage_limit) + feed_forward_voltage.d;
- else voltage.d = feed_forward_voltage.d;
- break;
- case TorqueControlType::dc_current:
- if(!current_sense) return;
- // constrain current setpoint
- current_sp = _constrain(current_sp, -current_limit, current_limit) + feed_forward_current.q;
- // 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) + feed_forward_voltage.q;
- // d voltage - lag compensation
- if(_isset(phase_inductance)) voltage.d = _constrain( -current_sp*shaft_velocity*pole_pairs*phase_inductance, -voltage_limit, voltage_limit) + feed_forward_voltage.d;
- else voltage.d = feed_forward_voltage.d;
- break;
- case TorqueControlType::foc_current:
- if(!current_sense) return;
- // constrain current setpoint
- current_sp = _constrain(current_sp, -current_limit, current_limit) + feed_forward_current.q;
- // 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) + feed_forward_voltage.q;
- voltage.d = PID_current_d(feed_forward_current.d - current.d) + feed_forward_voltage.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);
-}
-
-// 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) {
-
- // set internal target variable
- if(_isset(new_target)) target = new_target;
-
- // downsampling (optional)
- if(motion_cnt++ < motion_downsample) return;
- motion_cnt = 0;
-
- // read value even if motor is disabled to keep the monitoring updated
- // except for the open loop modes where the values are updated within angle/velocityOpenLoop functions
-
- // 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 ){
- // read the values only if the motor is not in open loop
- // because in open loop the shaft angle/velocity is updated within angle/velocityOpenLoop functions
- shaft_angle = shaftAngle();
- shaft_velocity = shaftVelocity();
- }
-
- // if disabled do nothing
- if(!enabled) return;
-
-
- // upgrade the current based voltage limit
- switch (controller) {
- case MotionControlType::torque:
- current_sp = target;
- 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 - 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
- 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
- break;
- case MotionControlType::velocity_openloop:
- // velocity control in open loop - sensor precision: this calculation is numerically precise.
- shaft_velocity_sp = target;
- // this function updates the shaft_angle and shaft_velocity
- // returns the voltage or current that is to be set to the motor (depending on torque control mode)
- // returned values correspond to the voltage_limit and current_limit
- current_sp = velocityOpenloop(shaft_velocity_sp);
- 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;
- // this function updates the shaft_angle and shaft_velocity
- // returns the voltage or current that is to be set to the motor (depending on torque control mode)
- // returned values correspond to the voltage_limit and current_limit
- current_sp = angleOpenloop(shaft_angle_sp);
- break;
- }
+float BLDCMotor::estimateBEMF(float vel){
+ // bemf constant is approximately 1/KV rating
+ // V_bemf = K_bemf * velocity
+ return vel/(KV_rating*_SQRT3)/_RPM_TO_RADS;
}
// 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 usage)
-// approx _sin + _cos ~110us (400Byte ~ 20% of memory)
+// Function implementing Space Vector PWM, Sine PWM and Trapezoidal commutation algorithms
void BLDCMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) {
float center;
@@ -560,22 +220,28 @@ void BLDCMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) {
case FOCModulationType::SinePWM :
case FOCModulationType::SpaceVectorPWM :
- // Sinusoidal PWM modulation
- // Inverse Park + Clarke transformation
- _sincos(angle_el, &_sa, &_ca);
-
- // Inverse park transform
- Ualpha = _ca * Ud - _sa * Uq; // -sin(angle) * Uq;
- Ubeta = _sa * Ud + _ca * Uq; // cos(angle) * Uq;
- // Clarke transform
- Ua = Ualpha;
- Ub = -0.5f * Ualpha + _SQRT3_2 * Ubeta;
- Uc = -0.5f * Ualpha - _SQRT3_2 * Ubeta;
-
- // centering the voltages around either
- // modulation_centered == true > driver.voltage_limit/2
- // modulation_centered == false > or Adaptable centering, all phases drawn to 0 when Uq=0
+ // Sinusoidal PWM modulation
+ // Inverse Park + Clarke transformation
+ _sincos(angle_el, &_sa, &_ca);
+
+ // Inverse park transform
+ Ualpha = _ca * Ud - _sa * Uq; // -sin(angle) * Uq;
+ Ubeta = _sa * Ud + _ca * Uq; // cos(angle) * Uq;
+
+ // Clarke transform
+ Ua = Ualpha;
+ Ub = -0.5f * Ualpha + _SQRT3_2 * Ubeta;
+ Uc = -0.5f * Ualpha - _SQRT3_2 * Ubeta;
+
+ // centering the voltages around either
+ // - centered modulation: around driver.voltage_limit/2
+ // - non-centered modulation: pulls the lowest voltage to 0
+ // - Can be useful for low-side current sensing
+ // in cases where the ADC had long sample time
+ // - The part of the duty cycle in which all phases are
+ // off is longer than in centered modulation
+ // - Both SinePWM and SpaceVectorPWM have the same form for non-centered modulation
if (modulation_centered) {
center = driver->voltage_limit/2;
if (foc_modulation == FOCModulationType::SpaceVectorPWM){
diff --git a/src/BLDCMotor.h b/src/BLDCMotor.h
index 4d506321..aa217d8e 100644
--- a/src/BLDCMotor.h
+++ b/src/BLDCMotor.h
@@ -18,12 +18,13 @@ class BLDCMotor: public FOCMotor
public:
/**
BLDCMotor class constructor
- @param pp pole pairs number
- @param R motor phase resistance - [Ohm]
+ @param pp pole pairs number
+ @param R motor phase resistance - [Ohm]
@param KV motor KV rating (1/K_bemf) - rpm/V
- @param L motor phase inductance - [H]
+ @param L_q motor phase inductance - [H]
+ @param L_d motor direct axis inductance - [H]
*/
- BLDCMotor(int pp, float R = NOT_SET, float KV = NOT_SET, float L = NOT_SET);
+ BLDCMotor(int pp, float R = NOT_SET, float KV = NOT_SET, float L_q = NOT_SET, float L_d = NOT_SET);
/**
* Function linking a motor and a foc driver
@@ -32,46 +33,20 @@ class BLDCMotor: public FOCMotor
*/
virtual void linkDriver(BLDCDriver* driver);
- /**
- * BLDCDriver link:
- * - 3PWM
- * - 6PWM
- */
- BLDCDriver* driver;
+
+ float Ua, Ub, Uc;//!< Current phase voltages Ua,Ub and Uc set to motor
+
+ BLDCDriver* driver; //!< BLDCDriver instance
+ // Methods implementing the FOCMotor interface
/** Motor hardware init function */
- int init() override;
+ int 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() 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
-
- /**
* Method using FOC to set Uq to the motor at the optimal angle
* Heart of the FOC algorithm
*
@@ -81,6 +56,17 @@ class BLDCMotor: public FOCMotor
*/
void setPhaseVoltage(float Uq, float Ud, float angle_el) override;
+ /**
+ * Method estimating the Back EMF voltage based
+ * based on the current velocity and KV rating
+ *
+ * @param velocity Current motor velocity
+ */
+ float estimateBEMF(float velocity) override;
+
+
+ // Methods overriding the FOCMotor default behavior
+
/**
* Measure resistance and inductance of a BLDCMotor and print results to debug.
* If a sensor is available, an estimate of zero electric angle will be reported too.
@@ -91,15 +77,7 @@ class BLDCMotor: public FOCMotor
return FOCMotor::characteriseMotor(voltage, 1.5f);
}
- private:
- // FOC methods
- /** 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();
};
diff --git a/src/HybridStepperMotor.cpp b/src/HybridStepperMotor.cpp
index e6f781fe..501aad49 100644
--- a/src/HybridStepperMotor.cpp
+++ b/src/HybridStepperMotor.cpp
@@ -6,7 +6,7 @@
// - R - motor phase resistance
// - KV - motor kv rating (rmp/v)
// - L - motor phase inductance [H]
-HybridStepperMotor::HybridStepperMotor(int pp, float _R, float _KV, float _inductance)
+HybridStepperMotor::HybridStepperMotor(int pp, float _R, float _KV, float _Lq, float _Ld)
: FOCMotor()
{
// number od pole pairs
@@ -17,7 +17,9 @@ HybridStepperMotor::HybridStepperMotor(int pp, float _R, float _KV, float _induc
// usually used rms
KV_rating = _KV;
// save phase inductance
- phase_inductance = _inductance;
+ phase_inductance_dq = {_Ld, _Lq};
+ phase_inductance = _Lq; // FOR BACKWARDS COMPATIBILITY
+
// torque control type is voltage by default
// current and foc_current not supported yet
@@ -30,7 +32,14 @@ HybridStepperMotor::HybridStepperMotor(int pp, float _R, float _KV, float _induc
void HybridStepperMotor::linkDriver(BLDCDriver *_driver)
{
driver = _driver;
- SIMPLEFOC_DEBUG("MOT: BLDCDriver linked, using pin C as the mid-phase");
+ SIMPLEFOC_MOTOR_DEBUG("BLDCDriver linked, using pin C as the mid-phase");
+}
+
+// override of the FOCMotor's current sense linking
+// setting the driver type directly
+void HybridStepperMotor::linkCurrentSense(CurrentSense* _cs){
+ FOCMotor::linkCurrentSense(_cs);
+ current_sense->driver_type = DriverType::Hybrid;
}
// init hardware pins
@@ -39,12 +48,11 @@ int HybridStepperMotor::init()
if (!driver || !driver->initialized)
{
motor_status = FOCMotorStatus::motor_init_failed;
- SIMPLEFOC_DEBUG("MOT: Init not possible, driver not initialized");
+ SIMPLEFOC_MOTOR_ERROR("Init not possible, driver not init");
return 0;
}
motor_status = FOCMotorStatus::motor_initializing;
- SIMPLEFOC_DEBUG("MOT: Init");
-
+ SIMPLEFOC_MOTOR_DEBUG("Init");
// sanity check for the voltage limit configuration
if (voltage_limit > driver->voltage_limit)
voltage_limit = driver->voltage_limit;
@@ -57,6 +65,11 @@ int HybridStepperMotor::init()
updateVoltageLimit(voltage_limit);
updateVelocityLimit(velocity_limit);
+ if(_isset(phase_inductance) && !(_isset(phase_inductance_dq.q))) {
+ // if only single inductance value is set, use it for both d and q axis
+ phase_inductance_dq = {phase_inductance, phase_inductance};
+ }
+
// if using open loop control, set a CW as the default direction if not already set
// only if no sensor is used
if(!sensor){
@@ -69,7 +82,7 @@ int HybridStepperMotor::init()
_delay(500);
// enable motor
- SIMPLEFOC_DEBUG("MOT: Enable driver.");
+ SIMPLEFOC_MOTOR_DEBUG("Enable driver.");
enable();
_delay(500);
@@ -80,6 +93,8 @@ int HybridStepperMotor::init()
// disable motor driver
void HybridStepperMotor::disable()
{
+ // disable the current sense
+ if(current_sense) current_sense->disable();
// set zero to PWM
driver->setPwm(0, 0, 0);
// disable driver
@@ -87,6 +102,7 @@ void HybridStepperMotor::disable()
// motor status update
enabled = 0;
}
+
// enable motor driver
void HybridStepperMotor::enable()
{
@@ -94,382 +110,33 @@ void HybridStepperMotor::enable()
driver->enable();
// set zero to PWM
driver->setPwm(0, 0, 0);
+ // enable the current sense
+ if(current_sense) current_sense->enable();
+ // reset the pids
+ PID_velocity.reset();
+ P_angle.reset();
+ PID_current_q.reset();
+ PID_current_d.reset();
// motor status update
enabled = 1;
}
-/**
- * FOC functions
- */
-int HybridStepperMotor::initFOC() {
- int exit_flag = 1;
-
- motor_status = FOCMotorStatus::motor_calibrating;
-
- // align motor if necessary
- // alignment necessary for encoders!
- // sensor and motor alignment - can be skipped
- // by setting motor.sensor_direction and motor.zero_electric_angle
- if(sensor){
- exit_flag *= alignSensor();
- // added the shaft_angle update
- sensor->update();
- 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){
- current_sense->driver_type = DriverType::Hybrid;
- 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)){
- exit_flag = 1;
- SIMPLEFOC_DEBUG("MOT: Openloop only!");
- }else{
- exit_flag = 0; // no FOC without sensor
- }
- }
-
- 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;
-}
-
-// Calibrate the motor and current sense phases
-int HybridStepperMotor::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, modulation_centered);
- 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 HybridStepperMotor::alignSensor()
-{
- int exit_flag = 1; // success
- SIMPLEFOC_DEBUG("MOT: Align sensor.");
-
- // if unknown natural direction
- if(sensor_direction==Direction::UNKNOWN) {
- // check if sensor needs zero search
- if (sensor->needsSearch())
- exit_flag = absoluteZeroSearch();
- // stop init if not found index
- if (!exit_flag)
- return exit_flag;
-
- // 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);
- sensor->update();
- _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);
- sensor->update();
- _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);
- }
- else
- SIMPLEFOC_DEBUG("MOT: Skip offset calib.");
- return exit_flag;
-}
-
-// Encoder alignment the absolute zero angle
-// - to the index
-int HybridStepperMotor::absoluteZeroSearch()
-{
-
- 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 HybridStepperMotor::loopFOC() {
- // update loop time measurement
- updateLoopTime();
-
- // 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 disabled do nothing
- if(!enabled) return;
-
- // if open-loop do nothing
- if( controller==MotionControlType::angle_openloop || controller==MotionControlType::velocity_openloop )
- // calculate the open loop electirical angle
- electrical_angle = _electricalAngle((shaft_angle), pole_pairs);
- else
- // 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:
- voltage.q = _constrain(current_sp, -voltage_limit, voltage_limit) + feed_forward_voltage.q;
- voltage.d = feed_forward_voltage.d;
- break;
- case TorqueControlType::estimated_current:
- if(! _isset(phase_resistance)) return;
- // constrain current setpoint
- current_sp = _constrain(current_sp, -current_limit, current_limit) + feed_forward_current.q; // desired current is the setpoint
- // calculate the back-emf voltage if KV_rating available U_bemf = vel*(1/KV)
- if (_isset(KV_rating)) voltage_bemf = shaft_velocity/(KV_rating*_SQRT2)/_RPM_TO_RADS;
- // filter the value values
- current.q = LPF_current_q(current_sp);
- // calculate the phase voltage
- voltage.q = current.q * phase_resistance + voltage_bemf;
- // constrain voltage within limits
- voltage.q = _constrain(voltage.q, -voltage_limit, voltage_limit) + feed_forward_voltage.q;
- // d voltage - lag compensation
- if(_isset(phase_inductance)) voltage.d = _constrain( -current_sp*shaft_velocity*pole_pairs*phase_inductance, -voltage_limit, voltage_limit) + feed_forward_voltage.d;
- else voltage.d = feed_forward_voltage.d;
- break;
- case TorqueControlType::dc_current:
- if(!current_sense) return;
- // constrain current setpoint
- current_sp = _constrain(current_sp, -current_limit, current_limit) + feed_forward_current.q;
- // 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) + feed_forward_voltage.q;
- // d voltage - lag compensation
- if(_isset(phase_inductance)) voltage.d = _constrain( -current_sp*shaft_velocity*pole_pairs*phase_inductance, -voltage_limit, voltage_limit) + feed_forward_voltage.d;
- else voltage.d = feed_forward_voltage.d;
- break;
- case TorqueControlType::foc_current:
- if(!current_sense) return;
- // constrain current setpoint
- current_sp = _constrain(current_sp, -current_limit, current_limit) + feed_forward_current.q;
- // 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) + feed_forward_voltage.q;
- voltage.d = PID_current_d(feed_forward_current.d - current.d) + feed_forward_voltage.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);
+float HybridStepperMotor::estimateBEMF(float vel){
+ // bemf constant is approximately 1/KV rating
+ // V_bemf = K_bemf * velocity
+ return vel/(KV_rating*_SQRT2)/_RPM_TO_RADS;
}
-// 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 voltage loop
-// - needs to be called iteratively it is asynchronous function
-// - if target is not set it uses motor.target value
-void HybridStepperMotor::move(float new_target) {
-
- // set internal target variable
- if(_isset(new_target) ) target = new_target;
-
- // downsampling (optional)
- if(motion_cnt++ < motion_downsample) return;
- motion_cnt = 0;
-
- // read value even if motor is disabled to keep the monitoring updated
- // except for the open loop modes where the values are updated within angle/velocityOpenLoop functions
-
- // 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 ){
- // read the values only if the motor is not in open loop
- // because in open loop the shaft angle/velocity is updated within angle/velocityOpenLoop functions
- shaft_angle = shaftAngle();
- shaft_velocity = shaftVelocity();
- }
-
- // if disabled do nothing
- if(!enabled) return;
-
-
- // upgrade the current based voltage limit
- switch (controller) {
- case MotionControlType::torque:
- current_sp = target;
- 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 - 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
- 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
- break;
- case MotionControlType::velocity_openloop:
- // velocity control in open loop - sensor precision: this calculation is numerically precise.
- shaft_velocity_sp = target;
- // this function updates the shaft_angle and shaft_velocity
- // returns the voltage or current that is to be set to the motor (depending on torque control mode)
- // returned values correspond to the voltage_limit and current_limit
- current_sp = velocityOpenloop(shaft_velocity_sp);
- 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;
- // this function updates the shaft_angle and shaft_velocity
- // returns the voltage or current that is to be set to the motor (depending on torque control mode)
- // returned values correspond to the voltage_limit and current_limit
- current_sp = angleOpenloop(shaft_angle_sp);
- break;
- }
-}
+// Method using FOC to set Uq and Ud to the motor at the optimal angle
+// Function implementing Sine PWM and SVPWM algorithms
void HybridStepperMotor::setPhaseVoltage(float Uq, float Ud, float angle_el)
{
- angle_el = _normalizeAngle(angle_el);
- float _ca = _cos(angle_el);
- float _sa = _sin(angle_el);
float center;
+ float _sa, _ca;
+
+ _sincos(angle_el, &_sa, &_ca);
switch (foc_modulation) {
case FOCModulationType::Trapezoid_120:
diff --git a/src/HybridStepperMotor.h b/src/HybridStepperMotor.h
index 2280af83..40bc2eab 100644
--- a/src/HybridStepperMotor.h
+++ b/src/HybridStepperMotor.h
@@ -21,13 +21,13 @@ class HybridStepperMotor : public FOCMotor
{
public:
/**
- HybridStepperMotor class constructor
- @param pp pole pair number
- @param R motor phase resistance - [Ohm]
- @param KV motor KV rating (1/K_bemf) - rpm/V
- @param L motor phase inductance - [H]
+ * HybridStepperMotor class constructor
+ * @param pp pole pair number
+ * @param R motor phase resistance - [Ohm]
+ * @param KV motor KV rating (1/K_bemf) - rpm/V
+ * @param L motor phase inductance - [H]
*/
- HybridStepperMotor(int pp, float R = NOT_SET, float KV = NOT_SET, float L = NOT_SET);
+ HybridStepperMotor(int pp, float R = NOT_SET, float KV = NOT_SET, float L_q = NOT_SET, float L_d = NOT_SET);
/**
* Function linking a motor and a foc driver
@@ -36,10 +36,11 @@ class HybridStepperMotor : public FOCMotor
*/
void linkDriver(BLDCDriver *driver);
- /**
- * BLDCDriver link:
- */
- BLDCDriver *driver;
+ BLDCDriver* driver; //!< BLDCDriver instance
+
+ float Ua, Ub, Uc; //!< Phase voltages used for inverse Park and Clarke transform
+
+ // Methods implementing the FOCMotor interface
/** Motor hardware init function */
int init() override;
@@ -48,32 +49,6 @@ class HybridStepperMotor : public FOCMotor
/** Motor enable function */
void enable() override;
- /**
- * Function initializing FOC algorithm
- * and aligning sensor's and motors' zero position
- */
- int initFOC() 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 HybridStepperMotor.
- *
- * @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; //!< Phase voltages used for inverse Park and Clarke transform
-
/**
* Method using FOC to set Uq to the motor at the optimal angle
* Heart of the FOC algorithm
@@ -85,7 +60,18 @@ class HybridStepperMotor : public FOCMotor
void setPhaseVoltage(float Uq, float Ud, float angle_el) override;
/**
- * Measure resistance and inductance of a StepperMotor and print results to debug.
+ * Method estimating the Back EMF voltage based
+ * based on the current velocity and KV rating
+ *
+ * @param velocity Current motor velocity
+ */
+ float estimateBEMF(float velocity) override;
+
+
+ // Methods overriding the FOCMotor default behavior
+
+ /**
+ * Measure resistance and inductance of a HybridStepperMotor and print results to debug.
* If a sensor is available, an estimate of zero electric angle will be reported too.
* TODO: determine the correction factor
* @param voltage The voltage applied to the motor
@@ -94,13 +80,13 @@ class HybridStepperMotor : public FOCMotor
int characteriseMotor(float voltage){
return FOCMotor::characteriseMotor(voltage, 1.0f);
}
+
+ /**
+ * Link the currentsense
+ */
+ void linkCurrentSense(CurrentSense* current_sense);
+
-private:
- int alignCurrentSense();
- /** Sensor alignment to electrical 0 angle of the motor */
- int alignSensor();
- /** Motor and sensor alignment to the sensors absolute 0 angle */
- int absoluteZeroSearch();
};
#endif
diff --git a/src/StepperMotor.cpp b/src/StepperMotor.cpp
index 36dcaf5a..4b74fbf7 100644
--- a/src/StepperMotor.cpp
+++ b/src/StepperMotor.cpp
@@ -7,7 +7,7 @@
// - R - motor phase resistance
// - KV - motor kv rating (rmp/v)
// - L - motor phase inductance [H]
-StepperMotor::StepperMotor(int pp, float _R, float _KV, float _inductance)
+StepperMotor::StepperMotor(int pp, float _R, float _KV, float _Lq, float _Ld)
: FOCMotor()
{
// number od pole pairs
@@ -18,7 +18,8 @@ StepperMotor::StepperMotor(int pp, float _R, float _KV, float _inductance)
// usually used rms
KV_rating = _KV;
// save phase inductance
- phase_inductance = _inductance;
+ phase_inductance_dq = {_Ld, _Lq};
+ phase_inductance = _Lq; // FOR BACKWARDS COMPATIBILITY
// torque control type is voltage by default
// current and foc_current not supported yet
@@ -36,23 +37,28 @@ void StepperMotor::linkDriver(StepperDriver* _driver) {
int StepperMotor::init() {
if (!driver || !driver->initialized) {
motor_status = FOCMotorStatus::motor_init_failed;
- SIMPLEFOC_DEBUG("MOT: Init not possible, driver not initialized");
+ SIMPLEFOC_MOTOR_ERROR("Init not possible, driver not init");
return 0;
}
motor_status = FOCMotorStatus::motor_initializing;
- SIMPLEFOC_DEBUG("MOT: Init");
+ SIMPLEFOC_MOTOR_DEBUG("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 limits in the motor controllers
updateCurrentLimit(current_limit);
updateVoltageLimit(voltage_limit);
updateVelocityLimit(velocity_limit);
+ if(_isset(phase_inductance) && !(_isset(phase_inductance_dq.q))) {
+ // if only single inductance value is set, use it for both d and q axis
+ phase_inductance_dq = {phase_inductance, phase_inductance};
+ }
+
+
// if using open loop control, set a CW as the default direction if not already set
// only if no sensor is used
if(!sensor){
@@ -65,7 +71,7 @@ int StepperMotor::init() {
_delay(500);
// enable motor
- SIMPLEFOC_DEBUG("MOT: Enable driver.");
+ SIMPLEFOC_MOTOR_DEBUG("Enable driver.");
enable();
_delay(500);
motor_status = FOCMotorStatus::motor_uncalibrated;
@@ -104,364 +110,14 @@ void StepperMotor::enable()
}
-/**
- FOC functions
-*/
-// FOC initialization function
-int StepperMotor::initFOC() {
- int exit_flag = 1;
-
- motor_status = FOCMotorStatus::motor_calibrating;
-
- // align motor if necessary
- // alignment necessary for encoders!
- // sensor and motor alignment - can be skipped
- // by setting motor.sensor_direction and motor.zero_electric_angle
- if(sensor){
- exit_flag *= alignSensor();
- // added the shaft_angle update
- sensor->update();
- 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)){
- exit_flag = 1;
- SIMPLEFOC_DEBUG("MOT: Openloop only!");
- }else{
- exit_flag = 0; // no FOC without sensor
- }
- }
-
- 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;
+float StepperMotor::estimateBEMF(float vel){
+ // bemf constant is approximately 1/KV rating
+ // V_bemf = K_bemf * velocity
+ return vel/(KV_rating*_SQRT2)/_RPM_TO_RADS;
}
-// 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, modulation_centered);
- 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
- 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;
-
- // v2.3.3 fix for R_AVR_7_PCREL against symbol" bug for AVR boards
- // TODO figure out why this works
- float voltage_align = voltage_sensor_align;
-
- // if unknown natural direction
- if(sensor_direction == Direction::UNKNOWN){
-
- // 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_align, 0, angle);
- sensor->update();
- _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_align, 0, angle);
- sensor->update();
- _delay(2);
- }
- sensor->update();
- float end_angle = sensor->getAngle();
- setPhaseVoltage(0, 0, 0);
- _delay(200);
- // determine the direction the sensor moved
- float moved = fabs(mid_angle - end_angle);
- if (moved 0.5f); // 0.5f is arbitrary number it can be lower or higher!
- if( pp_check_result==false ) {
- 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_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);
- SIMPLEFOC_DEBUG("MOT: Zero elec. angle: ", zero_electric_angle);
- // stop everything
- setPhaseVoltage(0, 0, 0);
- _delay(200);
- } else { SIMPLEFOC_DEBUG("MOT: Skip offset calib."); }
- return exit_flag;
-}
-
-// Encoder alignment the absolute zero angle
-// - to the index
-int StepperMotor::absoluteZeroSearch() {
-
- 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 StepperMotor::loopFOC() {
- // update loop time measurement
- updateLoopTime();
-
- // 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 disabled do nothing
- if(!enabled) return;
-
- // if open-loop do nothing
- if( controller==MotionControlType::angle_openloop || controller==MotionControlType::velocity_openloop )
- // calculate the open loop electirical angle
- electrical_angle = _electricalAngle((shaft_angle), pole_pairs);
- else
- // 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:
- voltage.q = _constrain(current_sp, -voltage_limit, voltage_limit) + feed_forward_voltage.q;
- voltage.d = feed_forward_voltage.d;
- break;
- case TorqueControlType::estimated_current:
- if(! _isset(phase_resistance)) return;
- // constrain current setpoint
- current_sp = _constrain(current_sp, -current_limit, current_limit) + feed_forward_current.q; // desired current is the setpoint
- // calculate the back-emf voltage if KV_rating available U_bemf = vel*(1/KV)
- if (_isset(KV_rating)) voltage_bemf = shaft_velocity/(KV_rating*_SQRT2)/_RPM_TO_RADS;
- // filter the value values
- current.q = LPF_current_q(current_sp);
- // calculate the phase voltage
- voltage.q = current.q * phase_resistance + voltage_bemf;
- // constrain voltage within limits
- voltage.q = _constrain(voltage.q, -voltage_limit, voltage_limit) + feed_forward_voltage.q;
- // d voltage - lag compensation
- if(_isset(phase_inductance)) voltage.d = _constrain( -current_sp*shaft_velocity*pole_pairs*phase_inductance, -voltage_limit, voltage_limit) + feed_forward_voltage.d;
- else voltage.d = feed_forward_voltage.d;
- break;
- case TorqueControlType::dc_current:
- if(!current_sense) return;
- // constrain current setpoint
- current_sp = _constrain(current_sp, -current_limit, current_limit) + feed_forward_current.q;
- // 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) + feed_forward_voltage.q;
- // d voltage - lag compensation
- if(_isset(phase_inductance)) voltage.d = _constrain( -current_sp*shaft_velocity*pole_pairs*phase_inductance, -voltage_limit, voltage_limit) + feed_forward_voltage.d;
- else voltage.d = feed_forward_voltage.d;
- break;
- case TorqueControlType::foc_current:
- if(!current_sense) return;
- // constrain current setpoint
- current_sp = _constrain(current_sp, -current_limit, current_limit) + feed_forward_current.q;
- // 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) + feed_forward_voltage.q;
- voltage.d = PID_current_d(feed_forward_current.d - current.d) + feed_forward_voltage.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);
-}
-
-// 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 StepperMotor::move(float new_target) {
-
- // set internal target variable
- if(_isset(new_target)) target = new_target;
-
- // downsampling (optional)
- if(motion_cnt++ < motion_downsample) return;
- motion_cnt = 0;
-
- // read value even if motor is disabled to keep the monitoring updated
- // except for the open loop modes where the values are updated within angle/velocityOpenLoop functions
-
- // 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 ){
- // read the values only if the motor is not in open loop
- // because in open loop the shaft angle/velocity is updated within angle/velocityOpenLoop functions
- shaft_angle = shaftAngle();
- shaft_velocity = shaftVelocity();
- }
-
- // if disabled do nothing
- if(!enabled) return;
-
-
- // upgrade the current based voltage limit
- switch (controller) {
- case MotionControlType::torque:
- current_sp = target;
- 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 - 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
- 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
- break;
- case MotionControlType::velocity_openloop:
- // velocity control in open loop - sensor precision: this calculation is numerically precise.
- shaft_velocity_sp = target;
- // this function updates the shaft_angle and shaft_velocity
- // returns the voltage or current that is to be set to the motor (depending on torque control mode)
- // returned values correspond to the voltage_limit and current_limit
- current_sp = velocityOpenloop(shaft_velocity_sp);
- 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;
- // this function updates the shaft_angle and shaft_velocity
- // returns the voltage or current that is to be set to the motor (depending on torque control mode)
- // returned values correspond to the voltage_limit and current_limit
- current_sp = angleOpenloop(shaft_angle_sp);
- break;
- }
-}
-
-
// Method using FOC to set Uq and Ud to the motor at the optimal angle
// Function implementing Sine PWM algorithms
-// - space vector not implemented yet
-//
-// Function using sine approximation
-// regular sin + cos ~300us (no memory usaage)
-// approx _sin + _cos ~110us (400Byte ~ 20% of memory)
void StepperMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) {
// Sinusoidal PWM modulation
// Inverse Park transformation
@@ -469,8 +125,8 @@ void StepperMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) {
_sincos(angle_el, &_sa, &_ca);
// Inverse park transform
- Ualpha = _ca * Ud - _sa * Uq; // -sin(angle) * Uq;
- Ubeta = _sa * Ud + _ca * Uq; // cos(angle) * Uq;
+ Ualpha = _ca * Ud - _sa * Uq;
+ Ubeta = _sa * Ud + _ca * Uq;
// set the voltages in hardware
driver->setPwm(Ualpha, Ubeta);
diff --git a/src/StepperMotor.h b/src/StepperMotor.h
index 22d1ccb2..48472737 100644
--- a/src/StepperMotor.h
+++ b/src/StepperMotor.h
@@ -27,7 +27,7 @@ class StepperMotor: public FOCMotor
@param KV motor KV rating (1/K_bemf) - rpm/V
@param L motor phase inductance - [H]
*/
- StepperMotor(int pp, float R = NOT_SET, float KV = NOT_SET, float L = NOT_SET);
+ StepperMotor(int pp, float R = NOT_SET, float KV = NOT_SET, float L_q = NOT_SET, float L_d = NOT_SET);
/**
* Function linking a motor and a foc driver
@@ -42,38 +42,17 @@ class StepperMotor: public FOCMotor
*/
StepperDriver* driver;
+
+ // Methods implementing the FOCMotor interface
+
/** Motor hardware init function */
- int init() override;
+ int 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
- *
- * - If zero_electric_offset parameter is set the alignment procedure is skipped
- */
- int initFOC() 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 StepperMotor.
- *
- * @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;
- /**
+ /**
* Method using FOC to set Uq to the motor at the optimal angle
* Heart of the FOC algorithm
*
@@ -83,6 +62,16 @@ class StepperMotor: public FOCMotor
*/
void setPhaseVoltage(float Uq, float Ud, float angle_el) override;
+ /**
+ * Method estimating the Back EMF voltage based
+ * based on the current velocity and KV rating
+ *
+ * @param velocity Current motor velocity
+ */
+ float estimateBEMF(float velocity) override;
+
+ // Methods overriding the FOCMotor default behavior
+
/**
* Measure resistance and inductance of a StepperMotor and print results to debug.
* If a sensor is available, an estimate of zero electric angle will be reported too.
@@ -94,14 +83,6 @@ class StepperMotor: public FOCMotor
return FOCMotor::characteriseMotor(voltage, 1.0f);
}
- private:
-
- /** Sensor alignment to electrical 0 angle of the motor */
- int alignSensor();
- /** Motor and sensor alignment to the sensors absolute 0 angle */
- int absoluteZeroSearch();
- /** Current sense and motor phase alignment */
- int alignCurrentSense();
};
diff --git a/src/common/base_classes/FOCMotor.cpp b/src/common/base_classes/FOCMotor.cpp
index 3f7fb0a1..ecaff6c7 100644
--- a/src/common/base_classes/FOCMotor.cpp
+++ b/src/common/base_classes/FOCMotor.cpp
@@ -88,7 +88,7 @@ void FOCMotor::useMonitoring(Print &print){
monitor_port = &print; //operate on the address of print
#ifndef SIMPLEFOC_DISABLE_DEBUG
SimpleFOCDebug::enable(&print);
- SIMPLEFOC_DEBUG("MOT: Monitor enabled!");
+ SIMPLEFOC_MOTOR_DEBUG("Monitor enabled!");
#endif
}
@@ -96,17 +96,17 @@ void FOCMotor::useMonitoring(Print &print){
int FOCMotor::characteriseMotor(float voltage, float correction_factor=1.0f){
if (!this->current_sense || !this->current_sense->initialized)
{
- SIMPLEFOC_DEBUG("ERR: MOT: Cannot characterise motor: CS unconfigured or not initialized");
+ SIMPLEFOC_MOTOR_ERROR("Fail. CS not init.");
return 1;
}
if (voltage <= 0.0f){
- SIMPLEFOC_DEBUG("ERR: MOT: Cannot characterise motor: Voltage is negative or less than zero");
+ SIMPLEFOC_MOTOR_ERROR("Fail. Volt. <= 0");
return 2;
}
voltage = _constrain(voltage, 0.0f, voltage_limit);
- SIMPLEFOC_DEBUG("MOT: Measuring phase to phase resistance, keep motor still...");
+ SIMPLEFOC_MOTOR_DEBUG("Meas R..");
float current_electric_angle = electricalAngle();
@@ -135,23 +135,23 @@ int FOCMotor::characteriseMotor(float voltage, float correction_factor=1.0f){
if (fabsf(r_currents.d - zerocurrent.d) < 0.2f)
{
- SIMPLEFOC_DEBUG("ERR: MOT: Motor characterisation failed: measured current too low");
+ SIMPLEFOC_MOTOR_ERROR("Fail. current too low");
return 3;
}
float resistance = voltage / (correction_factor * (r_currents.d - zerocurrent.d));
if (resistance <= 0.0f)
{
- SIMPLEFOC_DEBUG("ERR: MOT: Motor characterisation failed: Calculated resistance <= 0");
+ SIMPLEFOC_MOTOR_ERROR("Fail. Est. R<= 0");
return 4;
}
- SIMPLEFOC_DEBUG("MOT: Estimated phase to phase resistance: ", 2.0f * resistance);
+ SIMPLEFOC_MOTOR_DEBUG("Est. R: ", 2.0f * resistance);
_delay(100);
// Start inductance measurement
- SIMPLEFOC_DEBUG("MOT: Measuring inductance, keep motor still...");
+ SIMPLEFOC_MOTOR_DEBUG("Meas L...");
unsigned long t0 = 0;
unsigned long t1 = 0;
@@ -220,7 +220,7 @@ int FOCMotor::characteriseMotor(float voltage, float correction_factor=1.0f){
Ltemp = i < 2 ? inductanced : Ltemp * 0.6 + inductanced * 0.4;
float timeconstant = fabsf(Ltemp / resistance); // Timeconstant of an RL circuit (L/R)
- // SIMPLEFOC_DEBUG("MOT: Estimated time constant in us: ", 1000000.0f * timeconstant);
+ // SIMPLEFOC_MOTOR_DEBUG("Estimated time constant in us: ", 1000000.0f * timeconstant);
// Wait as long as possible (due to limited timing accuracy & sample rate), but as short as needed (while the current still changes)
risetime_us = _constrain(risetime_us * 0.6f + 0.4f * 1000000 * 0.6f * timeconstant, 100, 10000);
@@ -292,23 +292,26 @@ int FOCMotor::characteriseMotor(float voltage, float correction_factor=1.0f){
estimated_zero_electric_angle = estimated_zero_electric_angle_B;
}
- SIMPLEFOC_DEBUG("MOT: Newly estimated electrical zero: ", estimated_zero_electric_angle);
- SIMPLEFOC_DEBUG("MOT: Current electrical zero: ", zero_electric_angle);
+ SIMPLEFOC_MOTOR_DEBUG("New el. zero: ", estimated_zero_electric_angle);
+ SIMPLEFOC_MOTOR_DEBUG("Curr. el. zero: ", zero_electric_angle);
}
- SIMPLEFOC_DEBUG("MOT: Inductance measurement complete!");
- SIMPLEFOC_DEBUG("MOT: Measured D-inductance in mH: ", Ld * 1000.0f);
- SIMPLEFOC_DEBUG("MOT: Measured Q-inductance in mH: ", Lq * 1000.0f);
+ SIMPLEFOC_MOTOR_DEBUG("Ld [mH]: ", Ld * 1000.0f);
+ SIMPLEFOC_MOTOR_DEBUG("Lq [mH]: ", Lq * 1000.0f);
if (Ld > Lq)
{
- SIMPLEFOC_DEBUG("WARN: MOT: Measured inductance is larger in D than in Q axis. This is normally a sign of a measurement error.");
+ SIMPLEFOC_MOTOR_WARN("Ld>Lq. Likely error.");
}
if (Ld * 2.0f < Lq)
{
- SIMPLEFOC_DEBUG("WARN: MOT: Measured Q inductance is more than twice the D inductance. This is probably wrong. From experience, the lower value is probably close to reality.");
+ SIMPLEFOC_MOTOR_WARN("Lq > 2*Ld. Likely error.");
}
+ // store the measured values
+ phase_resistance = 2.0f * resistance;
+ phase_inductance_dq = {Ld, Lq};
+ phase_inductance = (Ld + Lq) / 2.0f; // FOR BACKWARDS COMPATIBILITY
return 0;
}
@@ -439,16 +442,6 @@ float FOCMotor::angleOpenloop(float target_angle){
return current_limit;
}
-// Function udating loop time measurement
-// time between two loopFOC executions in microseconds
-// It filters the value using low pass filtering alpha = 0.1
-void FOCMotor::updateLoopTime() {
- uint32_t now = _micros();
- last_loop_time_us = now - last_loop_timestamp_us;
- loop_time_us = 0.9f * loop_time_us + 0.1f * last_loop_time_us;
- last_loop_timestamp_us = now;
-}
-
// Update limit values in controllers when changed
void FOCMotor::updateVelocityLimit(float new_velocity_limit) {
velocity_limit = new_velocity_limit;
@@ -492,32 +485,427 @@ void FOCMotor::updateTorqueControlType(TorqueControlType new_torque_controller)
// - if changing to torque control set target to zero
void FOCMotor::updateMotionControlType(MotionControlType new_motion_controller) {
if (controller == new_motion_controller) return; // no change
- switch(new_motion_controller)
- {
- case MotionControlType::angle:
- if(controller != MotionControlType::angle_openloop) break;
- case MotionControlType::angle_openloop:
- if(controller != MotionControlType::angle) break;
- // if the previous controller was not angle control
- // set target to current angle
- target = shaft_angle;
- break;
- case MotionControlType::velocity:
- if(controller != MotionControlType::velocity_openloop) break;
- case MotionControlType::velocity_openloop:
- if(controller != MotionControlType::velocity) break;
- // if the previous controller was not velocity control
- // stop the motor
- target = 0;
- break;
- case MotionControlType::torque:
- // if torque control set target to zero
- target = 0;
- break;
- default:
- break;
+
+ switch(new_motion_controller){
+ case MotionControlType::angle:
+ if(controller == MotionControlType::angle_openloop) break;
+ case MotionControlType::angle_openloop:
+ if(controller == MotionControlType::angle) break;
+ // if the previous controller was not angle control
+ // set target to current angle
+ target = shaft_angle;
+ break;
+ case MotionControlType::velocity:
+ if(controller == MotionControlType::velocity_openloop) break;
+ case MotionControlType::velocity_openloop:
+ if(controller == MotionControlType::velocity) break;
+ // if the previous controller was not velocity control
+ // stop the motor
+ target = 0;
+ break;
+ case MotionControlType::torque:
+ // if torque control set target to zero
+ target = 0;
+ break;
+ default:
+ break;
}
// finally set the new controller
controller = new_motion_controller;
-}
\ No newline at end of file
+}
+
+
+int FOCMotor::tuneCurrentController(float bandwidth) {
+ if (bandwidth <= 0.0f) {
+ // check bandwidth is positive
+ SIMPLEFOC_MOTOR_ERROR("Fail. BW <= 0");
+ return 1;
+ }
+ if (loopfoc_time_us && bandwidth > 0.5f * (1e6f / loopfoc_time_us)) {
+ // check bandwidth is not too high for the control loop frequency
+ SIMPLEFOC_MOTOR_ERROR("Fail. BW too high, current loop freq:" , (1e6f / loopfoc_time_us));
+ return 2;
+ }
+ if (!_isset(phase_resistance) || (!_isset(phase_inductance) && !_isset(phase_inductance_dq.q))) {
+ // need motor parameters to tune the controller
+ SIMPLEFOC_MOTOR_WARN("Motor params missing!");
+ if(characteriseMotor( voltage_sensor_align )) {
+ return 3;
+ }
+ }else if (_isset(phase_inductance) && !(_isset(phase_inductance_dq.q))) {
+ // if only single inductance value is set, use it for both d and q axis
+ phase_inductance_dq = {phase_inductance, phase_inductance};
+ }
+
+ PID_current_q.P = phase_inductance_dq.q * (_2PI * bandwidth);
+ PID_current_q.I = phase_resistance * (_2PI * bandwidth);
+ PID_current_d.P = phase_inductance_dq.d * (_2PI * bandwidth);
+ PID_current_d.I = phase_resistance * (_2PI * bandwidth);
+ LPF_current_d.Tf = 1.0f / (_2PI * bandwidth * 5.0f); // filter cutoff at 5x bandwidth
+ LPF_current_q.Tf = 1.0f / (_2PI * bandwidth * 5.0f); // filter cutoff at 5x bandwidth
+
+ SIMPLEFOC_MOTOR_DEBUG("Tunned PI params for BW [Hz]: ", bandwidth);
+ SIMPLEFOC_MOTOR_DEBUG("Pq: ", PID_current_q.P);
+ SIMPLEFOC_MOTOR_DEBUG("Iq: ", PID_current_q.I);
+ SIMPLEFOC_MOTOR_DEBUG("Pd: ", PID_current_d.P);
+ SIMPLEFOC_MOTOR_DEBUG("Id: ", PID_current_d.I);
+
+ return 0;
+}
+
+
+
+// Iterative function looping FOC algorithm, setting Uq on the Motor
+// The faster it can be run the better
+void FOCMotor::loopFOC() {
+ // update loop time measurement
+ updateLoopFOCTime();
+
+ // 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 disabled do nothing
+ if(!enabled) return;
+
+ // if open-loop do nothing
+ if( controller==MotionControlType::angle_openloop || controller==MotionControlType::velocity_openloop )
+ // calculate the open loop electirical angle
+ electrical_angle = _electricalAngle((shaft_angle), pole_pairs);
+ else
+ // 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:
+ voltage.q = _constrain(current_sp, -voltage_limit, voltage_limit) + feed_forward_voltage.q;
+ voltage.d = feed_forward_voltage.d;
+ break;
+ case TorqueControlType::estimated_current:
+ if(! _isset(phase_resistance)) return;
+ // constrain current setpoint
+ current_sp = _constrain(current_sp, -current_limit, current_limit) + feed_forward_current.q; // desired current is the setpoint
+ // calculate the back-emf voltage if KV_rating available U_bemf = vel*(1/KV)
+ if (_isset(KV_rating)) voltage_bemf = estimateBEMF(shaft_velocity);
+ // filter the value values
+ current.q = LPF_current_q(current_sp);
+ // calculate the phase voltage
+ voltage.q = current.q * phase_resistance + voltage_bemf;
+ // constrain voltage within limits
+ voltage.q = _constrain(voltage.q, -voltage_limit, voltage_limit) + feed_forward_voltage.q;
+ // d voltage - lag compensation
+ if(_isset(phase_inductance_dq.d)) voltage.d = _constrain( -current_sp*shaft_velocity*pole_pairs*phase_inductance_dq.d, -voltage_limit, voltage_limit) + feed_forward_voltage.d;
+ else voltage.d = feed_forward_voltage.d;
+ break;
+ case TorqueControlType::dc_current:
+ if(!current_sense) return;
+ // constrain current setpoint
+ current_sp = _constrain(current_sp, -current_limit, current_limit) + feed_forward_current.q;
+ // 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) + feed_forward_voltage.q;
+ // d voltage - lag compensation
+ if(_isset(phase_inductance_dq.d)) voltage.d = _constrain( -current_sp*shaft_velocity*pole_pairs*phase_inductance_dq.d, -voltage_limit, voltage_limit) + feed_forward_voltage.d;
+ else voltage.d = feed_forward_voltage.d;
+ break;
+ case TorqueControlType::foc_current:
+ if(!current_sense) return;
+ // constrain current setpoint
+ current_sp = _constrain(current_sp, -current_limit, current_limit) + feed_forward_current.q;
+ // 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) + feed_forward_voltage.q;
+ voltage.d = PID_current_d(feed_forward_current.d - current.d) + feed_forward_voltage.d;
+ // d voltage - lag compensation - TODO verify
+ // if(_isset(phase_inductance_dq.d)) voltage.d = _constrain( voltage.d - current_sp*shaft_velocity*pole_pairs*phase_inductance_dq.d, -voltage_limit, voltage_limit);
+ break;
+ default:
+ // no torque control selected
+ SIMPLEFOC_MOTOR_ERROR("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 voltage loop
+// - needs to be called iteratively it is asynchronous function
+// - if target is not set it uses motor.target value
+void FOCMotor::move(float new_target) {
+
+ // set internal target variable
+ if(_isset(new_target) ) target = new_target;
+
+ // downsampling (optional)
+ if(motion_cnt++ < motion_downsample) return;
+ motion_cnt = 0;
+
+ // calculate the elapsed time between the calls
+ // TODO replace downsample by runnind the code at
+ // a specific frequency (or almost)
+ updateMotionControlTime();
+
+ // read value even if motor is disabled to keep the monitoring updated
+ // except for the open loop modes where the values are updated within angle/velocityOpenLoop functions
+
+ // 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 ){
+ // read the values only if the motor is not in open loop
+ // because in open loop the shaft angle/velocity is updated within angle/velocityOpenLoop functions
+ shaft_angle = shaftAngle();
+ shaft_velocity = shaftVelocity();
+ }
+
+ // if disabled do nothing
+ if(!enabled) return;
+
+
+ // upgrade the current based voltage limit
+ switch (controller) {
+ case MotionControlType::torque:
+ current_sp = target;
+ 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 - 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
+ 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
+ break;
+ case MotionControlType::velocity_openloop:
+ // velocity control in open loop - sensor precision: this calculation is numerically precise.
+ shaft_velocity_sp = target;
+ // this function updates the shaft_angle and shaft_velocity
+ // returns the voltage or current that is to be set to the motor (depending on torque control mode)
+ // returned values correspond to the voltage_limit and current_limit
+ current_sp = velocityOpenloop(shaft_velocity_sp);
+ 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;
+ // this function updates the shaft_angle and shaft_velocity
+ // returns the voltage or current that is to be set to the motor (depending on torque control mode)
+ // returned values correspond to the voltage_limit and current_limit
+ current_sp = angleOpenloop(shaft_angle_sp);
+ break;
+ }
+}
+
+
+// FOC initialization function
+int FOCMotor::initFOC() {
+ int exit_flag = 1;
+
+ motor_status = FOCMotorStatus::motor_calibrating;
+
+ // align motor if necessary
+ // alignment necessary for encoders!
+ // sensor and motor alignment - can be skipped
+ // by setting motor.sensor_direction and motor.zero_electric_angle
+ if(sensor){
+ exit_flag *= alignSensor();
+ // added the shaft_angle update
+ sensor->update();
+ 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_MOTOR_ERROR("Init FOC error, current sense not init");
+ exit_flag = 0;
+ }else{
+ exit_flag *= alignCurrentSense();
+ }
+ }
+ else { SIMPLEFOC_MOTOR_ERROR("No current sense"); }
+ }
+
+ } else {
+ SIMPLEFOC_MOTOR_DEBUG("No sensor.");
+ if ((controller == MotionControlType::angle_openloop || controller == MotionControlType::velocity_openloop)){
+ exit_flag = 1;
+ SIMPLEFOC_MOTOR_ERROR("Openloop only!");
+ }else{
+ exit_flag = 0; // no FOC without sensor
+ }
+ }
+
+ if(exit_flag){
+ SIMPLEFOC_MOTOR_DEBUG("Ready.");
+ motor_status = FOCMotorStatus::motor_ready;
+ }else{
+ SIMPLEFOC_MOTOR_ERROR("Init FOC fail");
+ motor_status = FOCMotorStatus::motor_calib_failed;
+ disable();
+ }
+
+ return exit_flag;
+}
+
+// Calibarthe the motor and current sense phases
+int FOCMotor::alignCurrentSense() {
+ int exit_flag = 1; // success
+
+ SIMPLEFOC_MOTOR_DEBUG("Align current sense.");
+
+ // align current sense and the driver
+ 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_MOTOR_ERROR("Align error!");
+ exit_flag = 0;
+ }else{
+ // output the alignment status flag
+ SIMPLEFOC_MOTOR_DEBUG("Success: ", exit_flag);
+ }
+
+ return exit_flag > 0;
+}
+
+// Encoder alignment to electrical 0 angle
+int FOCMotor::alignSensor() {
+ int exit_flag = 1; // success
+ SIMPLEFOC_MOTOR_DEBUG("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;
+
+ // v2.3.3 fix for R_AVR_7_PCREL against symbol" bug for AVR boards
+ // TODO figure out why this works
+ float voltage_align = voltage_sensor_align;
+
+ // if unknown natural direction
+ if(sensor_direction == Direction::UNKNOWN){
+
+ // 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_align, 0, angle);
+ sensor->update();
+ _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_align, 0, angle);
+ sensor->update();
+ _delay(2);
+ }
+ sensor->update();
+ float end_angle = sensor->getAngle();
+ // setPhaseVoltage(0, 0, 0);
+ _delay(200);
+ // determine the direction the sensor moved
+ float moved = fabs(mid_angle - end_angle);
+ if (moved 0.5f); // 0.5f is arbitrary number it can be lower or higher!
+ if( pp_check_result==false ) {
+ SIMPLEFOC_MOTOR_WARN("PP check: fail - est. pp: ", _2PI/moved);
+ } else {
+ SIMPLEFOC_MOTOR_DEBUG("PP check: OK!");
+ }
+
+ } else SIMPLEFOC_MOTOR_DEBUG("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_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);
+ SIMPLEFOC_MOTOR_DEBUG("Zero elec. angle: ", zero_electric_angle);
+ // stop everything
+ setPhaseVoltage(0, 0, 0);
+ _delay(200);
+ } else { SIMPLEFOC_MOTOR_DEBUG("Skip offset calib."); }
+ return exit_flag;
+}
+
+
+// Encoder alignment the absolute zero angle
+// - to the index
+int FOCMotor::absoluteZeroSearch() {
+ // sensor precision: this is all ok, as the search happens near the 0-angle, where the precision
+ // of float is sufficient.
+ SIMPLEFOC_MOTOR_DEBUG("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_MOTOR_ERROR("Not found!"); }
+ else { SIMPLEFOC_MOTOR_DEBUG("Success!"); }
+ }
+ return !sensor->needsSearch();
+}
diff --git a/src/common/base_classes/FOCMotor.h b/src/common/base_classes/FOCMotor.h
index 80fa4728..5414b8d5 100644
--- a/src/common/base_classes/FOCMotor.h
+++ b/src/common/base_classes/FOCMotor.h
@@ -11,6 +11,24 @@
#include "../pid.h"
#include "../lowpass_filter.h"
+#ifndef SIMPLEFOC_DISABLE_DEBUG
+#define SIMPLEFOC_MOTOR_WARN(msg, ...) \
+ SimpleFOCDebug::print("WARN-MOT: "); \
+ SIMPLEFOC_DEBUG(msg, ##__VA_ARGS__)
+
+#define SIMPLEFOC_MOTOR_ERROR(msg, ...) \
+ SimpleFOCDebug::print("ERR-MOT: "); \
+ SIMPLEFOC_DEBUG(msg, ##__VA_ARGS__)
+
+#define SIMPLEFOC_MOTOR_DEBUG(msg, ...) \
+ SimpleFOCDebug::print("MOT: "); \
+ SIMPLEFOC_DEBUG(msg, ##__VA_ARGS__)
+
+#else
+#define SIMPLEFOC_MOTOR_DEBUG(msg, ...)
+#define SIMPLEFOC_MOTOR_ERROR(msg, ...)
+#define SIMPLEFOC_MOTOR_WARN(msg, ...)
+#endif
// monitoring bitmap
#define _MON_TARGET 0b1000000 // monitor target value
@@ -78,27 +96,34 @@ class FOCMotor
*/
FOCMotor();
+
+ // Methods that need to be implemented, defining the FOCMotor interface
+
/** Motor hardware init function */
- virtual int init()=0;
+ virtual int init() = 0;
/** Motor disable function */
virtual void disable()=0;
/** Motor enable function */
virtual void enable()=0;
/**
- * Function linking a motor and a sensor
- *
- * @param sensor Sensor class wrapper for the FOC algorihtm to read the motor angle and velocity
- */
- void linkSensor(Sensor* sensor);
-
+ * 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
+ */
+ virtual void setPhaseVoltage(float Uq, float Ud, float angle_el)=0;
+
/**
- * Function linking a motor and current sensing
+ * Estimation of the Back EMF voltage
*
- * @param current_sense CurrentSense class wrapper for the FOC algorihtm to read the motor current measurements
+ * @param velocity - current shaft velocity
*/
- void linkCurrentSense(CurrentSense* current_sense);
+ virtual float estimateBEMF(float velocity){return 0.0f;};
+ // Methods that have a default behavior but can be overriden if needed
/**
* Function initializing FOC algorithm
@@ -106,34 +131,41 @@ class FOCMotor
*
* - If zero_electric_offset parameter is set the alignment procedure is skipped
*/
- virtual int initFOC()=0;
+ virtual int initFOC();
+
/**
* 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
*/
- virtual void loopFOC()=0;
+ virtual void loopFOC();
+
/**
- * Function executing the control loops set by the controller parameter of the BLDCMotor.
+ * Function executing the control loops set by the controller.
*
* @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
*/
- virtual void move(float target = NOT_SET)=0;
+ virtual void move(float target = NOT_SET);
/**
- * 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
- */
- virtual void setPhaseVoltage(float Uq, float Ud, float angle_el)=0;
-
+ * Function linking a motor and a sensor
+ *
+ * @param sensor Sensor class wrapper for the FOC algorihtm to read the motor angle and velocity
+ */
+ void linkSensor(Sensor* sensor);
+
+ /**
+ * Function linking a motor and current sensing
+ *
+ * @param current_sense CurrentSense class wrapper for the FOC algorihtm to read the motor current measurements
+ */
+ void linkCurrentSense(CurrentSense* current_sense);
+
+
// State calculation methods
/** Shaft angle calculation in radians [rad] */
float shaftAngle();
@@ -143,13 +175,12 @@ class FOCMotor
*/
float shaftVelocity();
-
-
/**
* Electrical angle calculation
*/
float electricalAngle();
+
/**
* Measure resistance and inductance of a motor and print results to debug.
* If a sensor is available, an estimate of zero electric angle will be reported too.
@@ -159,6 +190,16 @@ class FOCMotor
*/
int characteriseMotor(float voltage, float correction_factor);
+ /**
+ * Auto-tune the current controller PID parameters based on desired bandwidth.
+ * Uses a simple method that assumes a first order system and requires knowledge of
+ * the motor phase resistance and inductance (if not set, the characteriseMotor function can be used).
+ *
+ * @param bandwidth Desired closed-loop bandwidth in Hz.
+ * @returns returns 0 for success, >0 for failure
+ */
+ int tuneCurrentController(float bandwidth);
+
// state variables
float target; //!< current target value - depends of the controller
float feed_forward_velocity = 0.0f; //!< current feed forward velocity
@@ -184,7 +225,8 @@ class FOCMotor
float phase_resistance; //!< motor phase resistance
int pole_pairs;//!< motor pole pairs number
float KV_rating; //!< motor KV rating
- float phase_inductance; //!< motor phase inductance
+ float phase_inductance; //!< motor phase inductance q axis - FOR BACKWARDS COMPATIBILITY
+ DQ_s phase_inductance_dq{NOT_SET, NOT_SET}; //!< motor direct axis phase inductance
// limiting variables
float voltage_limit; //!< Voltage limiting variable - global limit
@@ -250,24 +292,15 @@ class FOCMotor
* - HallSensor
*/
Sensor* sensor;
- /**
- * CurrentSense link
- */
+ //!< CurrentSense link
CurrentSense* current_sense;
// monitoring functions
Print* monitor_port; //!< Serial terminal variable if provided
//!< time between two loopFOC executions in microseconds
- uint32_t loop_time_us = 0; //!< filtered loop times
-
- /**
- * Function udating loop time measurement
- * time between two loopFOC executions in microseconds
- * It filters the value using low pass filtering alpha = 0.1
- * @note - using _micros() function - be aware of its overflow every ~70 minutes
- */
- void updateLoopTime();
+ uint32_t loopfoc_time_us = 0; //!< filtered loop times
+ uint32_t move_time_us = 0; // filtered motion control times
/**
* Update limit values in controllers when changed
@@ -332,15 +365,49 @@ class FOCMotor
* @param target_angle - rad
*/
float angleOpenloop(float target_angle);
- // open loop variables
- uint32_t open_loop_timestamp;
+
+ protected:
+
+ /**
+ * Function udating loop time measurement
+ * time between two loopFOC executions in microseconds
+ * It filters the value using low pass filtering alpha = 0.1
+ * @note - using _micros() function - be aware of its overflow every ~70 minutes
+ */
+ void updateLoopFOCTime(){
+ updateTime(loopfoc_time_us, last_loopfoc_time_us, last_loopfoc_timestamp_us);
+ }
+
+ void updateMotionControlTime(){
+ updateTime(move_time_us, last_move_time_us, last_move_timestamp_us);
+ }
+
+ /** 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();
+ uint32_t last_loopfoc_timestamp_us = 0; //!< timestamp of the last loopFOC execution in microseconds
+ uint32_t last_loopfoc_time_us = 0; //!< last elapsed time of loopFOC in microseconds
+ uint32_t last_move_timestamp_us = 0; //!< timestamp of the last move execution in microseconds
+ uint32_t last_move_time_us = 0; //!< last elapsed time of move in microseconds
private:
// monitor counting variable
unsigned int monitor_cnt = 0 ; //!< counting variable
- uint32_t last_loop_timestamp_us = 0; //!< timestamp of the last loopFOC execution in microseconds
- uint32_t last_loop_time_us = 0; //!< time between two loopFOC executions in microseconds
+ // time measuring function
+ // It filters the value using low pass filtering alpha = 0.1
+ void updateTime(uint32_t& elapsed_time_filetered, uint32_t& elapsed_time, uint32_t& last_timestamp_us, float alpha = 0.1f){
+ uint32_t now = _micros();
+ elapsed_time = now - last_timestamp_us;
+ elapsed_time_filetered = (1-alpha) * elapsed_time_filetered + alpha * elapsed_time;
+ last_timestamp_us = now;
+ }
+
+ // open loop variables
+ uint32_t open_loop_timestamp;
};
diff --git a/src/common/defaults.h b/src/common/defaults.h
index ac57daa1..c4368e5f 100644
--- a/src/common/defaults.h
+++ b/src/common/defaults.h
@@ -18,6 +18,7 @@
#define DEF_PID_CURR_RAMP 1000.0f //!< default PID controller voltage ramp value
#define DEF_PID_CURR_LIMIT (DEF_POWER_SUPPLY) //!< default PID controller voltage limit
#define DEF_CURR_FILTER_Tf 0.01f //!< default velocity filter time constant
+#define DEF_CURR_BANDWIDTH 100.0f //!< current bandwidth
#else
// for stm32, due, teensy, esp32 and similar
#define DEF_PID_CURR_P 3 //!< default PID controller P value
@@ -25,7 +26,8 @@
#define DEF_PID_CURR_D 0.0f //!< default PID controller D value
#define DEF_PID_CURR_RAMP 0 //!< default PID controller voltage ramp value
#define DEF_PID_CURR_LIMIT (DEF_POWER_SUPPLY) //!< default PID controller voltage limit
-#define DEF_CURR_FILTER_Tf 0.005f //!< default currnet filter time constant
+#define DEF_CURR_BANDWIDTH 300.0f //!< current bandwidth
+#define DEF_CURR_FILTER_Tf 1/(_2PI*DEF_CURR_BANDWIDTH) //!< default currnet filter time constant
#endif
// default current limit values
#define DEF_CURRENT_LIM 2.0f //!< 2Amps current limit by default
diff --git a/src/common/foc_utils.h b/src/common/foc_utils.h
index 2094ab26..9e2ab456 100644
--- a/src/common/foc_utils.h
+++ b/src/common/foc_utils.h
@@ -39,31 +39,36 @@
#define MIN_ANGLE_DETECT_MOVEMENT (_2PI/101.0f)
-// dq current structure
-struct DQCurrent_s
+// dq variables
+struct DQ_s
{
float d;
float q;
};
-// phase current structure
-struct PhaseCurrent_s
-{
- float a;
- float b;
- float c;
-};
+
// dq voltage structs
-struct DQVoltage_s
-{
- float d;
- float q;
-};
-// alpha beta current structure
-struct ABCurrent_s
+typedef DQ_s DQVoltage_s;
+// dq current structs
+typedef DQ_s DQCurrent_s;
+
+// alpha-beta variables
+struct AB_s
{
float alpha;
float beta;
};
+typedef AB_s ABVoltage_s; // NOT USED
+typedef AB_s ABCurrent_s;
+
+// phase structs
+struct Phase_s
+{
+ float a;
+ float b;
+ float c;
+};
+typedef Phase_s PhaseVoltage_s; // NOT USED
+typedef Phase_s PhaseCurrent_s;
/**
diff --git a/src/common/lowpass_filter.cpp b/src/common/lowpass_filter.cpp
index ffb15cfc..aabb7790 100644
--- a/src/common/lowpass_filter.cpp
+++ b/src/common/lowpass_filter.cpp
@@ -1,7 +1,8 @@
#include "lowpass_filter.h"
-LowPassFilter::LowPassFilter(float time_constant)
+LowPassFilter::LowPassFilter(float time_constant, float sampling_time)
: Tf(time_constant)
+ , Ts(sampling_time)
, y_prev(0.0f)
{
timestamp_prev = _micros();
@@ -10,19 +11,28 @@ LowPassFilter::LowPassFilter(float time_constant)
float LowPassFilter::operator() (float x)
{
- unsigned long timestamp = _micros();
- float dt = (timestamp - timestamp_prev)*1e-6f;
+ // initalise the elapsed time with the fixed sampling tims Ts
+ float dt = Ts;
+ // if Ts is not set, use adaptive sampling time
+ // calculate the ellapsed time dt
+ if(!_isset(dt)){
+ unsigned long timestamp = _micros();
+ dt = (timestamp - timestamp_prev)*1e-6f;
- if (dt < 0.0f ) dt = 1e-3f;
- else if(dt > 0.3f) {
- y_prev = x;
+ if (dt < 0.0f ) dt = 1e-3f;
+ else if(dt > 0.3f) {
+ y_prev = x;
+ timestamp_prev = timestamp;
+ return x;
+ }
timestamp_prev = timestamp;
- return x;
}
+ // calculate the first order filer
float alpha = Tf/(Tf + dt);
float y = alpha*y_prev + (1.0f - alpha)*x;
+
+ // save the variables for the future steps
y_prev = y;
- timestamp_prev = timestamp;
return y;
}
diff --git a/src/common/lowpass_filter.h b/src/common/lowpass_filter.h
index 1f24e80d..a5964f1e 100644
--- a/src/common/lowpass_filter.h
+++ b/src/common/lowpass_filter.h
@@ -13,12 +13,19 @@ class LowPassFilter
public:
/**
* @param Tf - Low pass filter time constant
+ * @param Ts - Filter sampling time
+ *
+ * @note If sampling time Ts is not set the filter will measure the
+ * elapsed time between each call.
+ * @note Ts can be changed dynamically as well by modifying the
+ * variable in runtime.
*/
- LowPassFilter(float Tf);
+ LowPassFilter(float Tf, float Ts = NOT_SET);
~LowPassFilter() = default;
float operator() (float x);
float Tf; //!< Low pass filter time constant
+ float Ts; //!< Fixed sampling time (optional default NOT_SET)
protected:
unsigned long timestamp_prev; //!< Last execution timestamp
diff --git a/src/common/pid.cpp b/src/common/pid.cpp
index da1bee13..7489554d 100644
--- a/src/common/pid.cpp
+++ b/src/common/pid.cpp
@@ -1,11 +1,12 @@
#include "pid.h"
-PIDController::PIDController(float P, float I, float D, float ramp, float limit)
+PIDController::PIDController(float P, float I, float D, float ramp, float limit, float sampling_time)
: P(P)
, I(I)
, D(D)
- , output_ramp(ramp) // output derivative limit [volts/second]
- , limit(limit) // output supply limit [volts]
+ , output_ramp(ramp) // output derivative limit [ex. volts/second]
+ , limit(limit) // output supply limit [ex. volts]
+ , Ts(sampling_time) // sampling time [S]
, error_prev(0.0f)
, output_prev(0.0f)
, integral_prev(0.0f)
@@ -15,11 +16,17 @@ PIDController::PIDController(float P, float I, float D, float ramp, float limit)
// PID controller function
float PIDController::operator() (float error){
- // calculate the time from the last call
- unsigned long timestamp_now = _micros();
- float Ts = (timestamp_now - timestamp_prev) * 1e-6f;
- // quick fix for strange cases (micros overflow)
- if(Ts <= 0 || Ts > 0.5f) Ts = 1e-3f;
+ // initalise the elapsed time with the fixed sampling tims Ts
+ float dt = Ts;
+ // if Ts is not set, use adaptive sampling time
+ // calculate the ellapsed time dt
+ if(!_isset(dt)){
+ unsigned long timestamp_now = _micros();
+ dt = (timestamp_now - timestamp_prev) * 1e-6f;
+ // quick fix for strange cases (micros overflow)
+ if(dt <= 0 || dt > 0.5f) dt = 1e-3f;
+ timestamp_prev = timestamp_now;
+ }
// u(s) = (P + I/s + Ds)e(s)
// Discrete implementations
@@ -28,12 +35,12 @@ float PIDController::operator() (float error){
float proportional = P * error;
// Tustin transform of the integral part
// u_ik = u_ik_1 + I*Ts/2*(ek + ek_1)
- float integral = integral_prev + I*Ts*0.5f*(error + error_prev);
+ float integral = integral_prev + I*dt*0.5f*(error + error_prev);
// antiwindup - limit the output
integral = _constrain(integral, -limit, limit);
// Discrete derivation
// u_dk = D(ek - ek_1)/Ts
- float derivative = D*(error - error_prev)/Ts;
+ float derivative = D*(error - error_prev)/dt;
// sum all the components
float output = proportional + integral + derivative;
@@ -43,17 +50,16 @@ float PIDController::operator() (float error){
// if output ramp defined
if(output_ramp > 0){
// limit the acceleration by ramping the output
- float output_rate = (output - output_prev)/Ts;
+ float output_rate = (output - output_prev)/dt;
if (output_rate > output_ramp)
- output = output_prev + output_ramp*Ts;
+ output = output_prev + output_ramp*dt;
else if (output_rate < -output_ramp)
- output = output_prev - output_ramp*Ts;
+ output = output_prev - output_ramp*dt;
}
// saving for the next pass
integral_prev = integral;
output_prev = output;
error_prev = error;
- timestamp_prev = timestamp_now;
return output;
}
diff --git a/src/common/pid.h b/src/common/pid.h
index acd68d6f..c171e30d 100644
--- a/src/common/pid.h
+++ b/src/common/pid.h
@@ -18,8 +18,14 @@ class PIDController
* @param D - Derivative gain
* @param ramp - Maximum speed of change of the output value
* @param limit - Maximum output value
+ * @param sampling_time - sampling time
+ *
+ * @note If sampling time is not set the filter will measure the
+ * elapsed time between each call. If yes it will consider it fixed.
+ * @note Sampling time can be changed dynamically as well by modifying the
+ * variable Ts in runtime.
*/
- PIDController(float P, float I, float D, float ramp, float limit);
+ PIDController(float P, float I, float D, float ramp, float limit, float sampling_time = NOT_SET);
~PIDController() = default;
float operator() (float error);
@@ -30,6 +36,7 @@ class PIDController
float D; //!< Derivative gain
float output_ramp; //!< Maximum speed of change of the output value
float limit; //!< Maximum output value
+ float Ts; //!< Fixed sampling time (optional default NOT_SET)
protected:
float error_prev; //!< last tracking error value
diff --git a/src/communication/Commander.cpp b/src/communication/Commander.cpp
index de288abf..8f07b07d 100644
--- a/src/communication/Commander.cpp
+++ b/src/communication/Commander.cpp
@@ -224,11 +224,34 @@ void Commander::motor(FOCMotor* motor, char* user_command) {
break;
case CMD_INDUCTANCE:
printVerbose(F("L phase: "));
- if(!GET){
- motor->phase_inductance = value;
+ switch (sub_cmd){
+ case SCMD_INDUCT_D:
+ printVerbose(F("d: "));
+ if(!GET){
+ motor->phase_inductance_dq.d = value;
+ motor->phase_inductance = value;
+ }
+ if(_isset(motor->phase_inductance_dq.d)) println(motor->phase_inductance_dq.d);
+ else println(0);
+ break;
+ case SCMD_INDUCT_Q:
+ printVerbose(F("q: "));
+ if(!GET){
+ motor->phase_inductance_dq.q = value;
+ }
+ if(_isset(motor->phase_inductance_dq.q)) println(motor->phase_inductance_dq.q);
+ else println(0);
+ break;
+ default:
+ if(!GET){
+ motor->phase_inductance = value;
+ motor->phase_inductance_dq.d = value;
+ motor->phase_inductance_dq.q = value;
+ }
+ if(_isset(motor->phase_inductance)) println(motor->phase_inductance);
+ else println(0);
+ break;
}
- if(_isset(motor->phase_inductance)) println(motor->phase_inductance);
- else println(0);
break;
case CMD_KV_RATING:
printVerbose(F("Motor KV: "));
@@ -240,8 +263,8 @@ void Commander::motor(FOCMotor* motor, char* user_command) {
break;
case CMD_SENSOR:
// Sensor zero offset
- printVerbose(F("Sensor | "));
- switch (sub_cmd){
+ printVerbose(F("Sensor | "));
+ switch (sub_cmd){
case SCMD_SENS_MECH_OFFSET: // zero offset
printVerbose(F("offset: "));
if(!GET) motor->sensor_offset = value;
@@ -255,20 +278,36 @@ void Commander::motor(FOCMotor* motor, char* user_command) {
default:
printError();
break;
- }
+ }
break;
case CMD_FOC_PARAMS:
printVerbose(F("FOC | "));
switch (sub_cmd){
case SCMD_LOOPFOC_TIME: // loopFOC execution time
printVerbose(F("loop time: "));
- println((int)motor->loop_time_us);
+ println((int)motor->loopfoc_time_us);
+ break;
+ case SCMD_MOVE_TIME: // loopFOC execution time
+ printVerbose(F("move time: "));
+ println((int)motor->move_time_us);
break;
case SCMD_REINIT_FOC:
printVerbose(F("Reinit!"));
motor->initFOC();
println(F("done"));
break;
+ case SCMD_TUNE_CURR:
+ printVerbose(F("PI tune curr.| "));
+ {
+ int res = motor->tuneCurrentController(value);
+ if(res == 0){
+ println(F("done"));
+ } else {
+ printVerbose(F("failed, err code: "));
+ println(res);
+ }
+ }
+ break;
default:
printError();
break;
diff --git a/src/communication/commands.h b/src/communication/commands.h
index b925c73b..9d97a891 100644
--- a/src/communication/commands.h
+++ b/src/communication/commands.h
@@ -49,7 +49,12 @@
#define SCMD_PWMMOD_TYPE 'T' //!<< Pwm modulation type
#define SCMD_PWMMOD_CENTER 'C' //!<< Pwm modulation center flag
- #define SCMD_LOOPFOC_TIME 'L' //!< loopFOC execution time
+ #define SCMD_LOOPFOC_TIME 'L' //!< loopFOC time between executions (filtered)
+ #define SCMD_MOVE_TIME 'M' //!< move time between executions (filtered)
#define SCMD_REINIT_FOC 'R' //!< reinitialize FOC
+ #define SCMD_TUNE_CURR 'C' //!< tune current controller
+
+ #define SCMD_INDUCT_D 'D' //!< inductance d axis
+ #define SCMD_INDUCT_Q 'Q' //!< inductance q axis
#endif
\ No newline at end of file