diff --git a/README.md b/README.md
index 86551f79..543e6533 100644
--- a/README.md
+++ b/README.md
@@ -13,7 +13,8 @@
-[](https://www.ardu-badge.com/badge/Simple%20FOC.svg)
+[](https://www.ardu-badge.com/badge/Simple%20FOC.svg)
+[](https://registry.platformio.org/libraries/askuric/Simple%20FOC)
[](https://opensource.org/licenses/MIT)
[](https://joss.theoj.org/papers/4382445f249e064e9f0a7f6c1bb06b1d)
@@ -28,18 +29,8 @@ Therefore this is an attempt to:
- For official driver boards see [SimpleFOCBoards](https://docs.simplefoc.com/boards)
- Many many more boards developed by the community members, see [SimpleFOCCommunity](https://community.simplefoc.com/)
-> NEW RELEASE 📢 : SimpleFOClibrary v2.3.3
-> - Teensy4
-> - support for low-side current sensing [#392](https://github.com/simplefoc/Arduino-FOC/pull/392)
-> - support for center aligned 6pwm and 3pwm (optional) [#392](https://github.com/simplefoc/Arduino-FOC/pull/392)
-> - stm32
-> - support for center aligned pwm (even across multiple timers and motors/drivers) [#374](https://github.com/simplefoc/Arduino-FOC/pull/374), [#388](https://github.com/simplefoc/Arduino-FOC/pull/388)
-> - support for DMA based low-side current sensing: [#383](https://github.com/simplefoc/Arduino-FOC/pull/383),[#378](https://github.com/simplefoc/Arduino-FOC/pull/378)
-> - support for f7 architecture [#388](https://github.com/simplefoc/Arduino-FOC/pull/388),[#394](https://github.com/simplefoc/Arduino-FOC/pull/394)
-> - KV rating calculation fix [#347](https://github.com/simplefoc/Arduino-FOC/pull/347)
-> - Much more performant Space Vector PWM calculation [#340](https://github.com/simplefoc/Arduino-FOC/pull/340)
-> - And much more:
-> - See the complete list of bugfixes and new features of v2.3.3 [fixes and PRs](https://github.com/simplefoc/Arduino-FOC/milestone/10?closed=1)
+> NEXT RELEASE 📢 : SimpleFOClibrary v2.3.4
+> - Current sensing support for Stepper motors (lowside and inline)
## Arduino *SimpleFOClibrary* v2.3.3
diff --git a/examples/hardware_specific_examples/SimpleFOCMini/open_loop/open_loop.ino b/examples/hardware_specific_examples/SimpleFOCMini/open_loop/open_loop.ino
new file mode 100644
index 00000000..c458718b
--- /dev/null
+++ b/examples/hardware_specific_examples/SimpleFOCMini/open_loop/open_loop.ino
@@ -0,0 +1,93 @@
+/**
+ *
+ * SimpleFOCMini motor control example
+ *
+ * For Arduino UNO or the other boards with the UNO headers
+ * the most convenient way to use the board is to stack it to the pins:
+ * - 12 - ENABLE
+ * - 11 - IN1
+ * - 10 - IN2
+ * - 9 - IN3
+ *
+ */
+#include
+
+
+// BLDC motor & driver instance
+BLDCMotor motor = BLDCMotor(11);
+// BLDCDriver3PWM driver = BLDCDriver3PWM(11, 10, 9, 8); // mini v1.0
+BLDCDriver3PWM driver = BLDCDriver3PWM(9, 10, 11, 12); // mini v1.1
+
+// instantiate the commander
+Commander command = Commander(Serial);
+void doMotor(char* cmd) { command.motor(&motor, cmd); }
+
+void setup() {
+ // use monitoring with serial
+ Serial.begin(115200);
+ // enable more verbose output for debugging
+ // comment out if not needed
+ SimpleFOCDebug::enable(&Serial);
+
+ // if SimpleFOCMini is stacked in arduino headers
+ // on pins 12,11,10,9,8
+ // pin 12 is used as ground
+ pinMode(12,OUTPUT);
+ pinMode(12,LOW);
+
+ // driver config
+ // power supply voltage [V]
+ driver.voltage_power_supply = 12;
+ driver.init();
+ // link the motor and the driver
+ motor.linkDriver(&driver);
+
+ // aligning voltage [V]
+ motor.voltage_sensor_align = 3;
+
+ // set motion control loop to be used
+ motor.controller = MotionControlType::velocity_openloop;
+
+ // default voltage_power_supply
+ motor.voltage_limit = 2; // Volts
+
+ // comment out if not needed
+ motor.useMonitoring(Serial);
+
+ // initialize motor
+ motor.init();
+ // align encoder and start FOC
+ motor.initFOC();
+
+ // add target command M
+ command.add('M', doMotor, "motor");
+
+ Serial.println(F("Motor ready."));
+ Serial.println(F("Set the target velocity using serial terminal:"));
+
+ motor.target = 1; //initial target velocity 1 rad/s
+ Serial.println("Target velocity: 1 rad/s");
+ Serial.println("Voltage limit 2V");
+ _delay(1000);
+}
+
+void loop() {
+ // main FOC algorithm function
+ // the faster you run this function the better
+ // Arduino UNO loop ~1kHz
+ // Bluepill loop ~10kHz
+ motor.loopFOC();
+
+ // Motion control function
+ // velocity, position or voltage (defined in motor.controller)
+ // this function can be run at much lower frequency than loopFOC() function
+ // You can also use motor.move() and set the motor.target in the code
+ motor.move();
+
+ // function intended to be used with serial plotter to monitor motor variables
+ // significantly slowing the execution down!!!!
+ // motor.monitor();
+
+ // user communication
+ command.run();
+}
\ No newline at end of file
diff --git a/src/BLDCMotor.cpp b/src/BLDCMotor.cpp
index acc5b0fe..792381d5 100644
--- a/src/BLDCMotor.cpp
+++ b/src/BLDCMotor.cpp
@@ -202,7 +202,7 @@ int BLDCMotor::alignCurrentSense() {
SIMPLEFOC_DEBUG("MOT: Align current sense.");
// align current sense and the driver
- exit_flag = current_sense->driverAlign(voltage_sensor_align);
+ exit_flag = current_sense->driverAlign(voltage_sensor_align, modulation_centered);
if(!exit_flag){
// error in current sense - phase either not measured or bad connection
SIMPLEFOC_DEBUG("MOT: Align error!");
diff --git a/src/BLDCMotor.h b/src/BLDCMotor.h
index a7155c1f..41a5a1c0 100644
--- a/src/BLDCMotor.h
+++ b/src/BLDCMotor.h
@@ -4,6 +4,7 @@
#include "Arduino.h"
#include "common/base_classes/FOCMotor.h"
#include "common/base_classes/Sensor.h"
+#include "common/base_classes/FOCDriver.h"
#include "common/base_classes/BLDCDriver.h"
#include "common/foc_utils.h"
#include "common/time_utils.h"
diff --git a/src/StepperMotor.cpp b/src/StepperMotor.cpp
index 19c96cd7..5db9afa2 100644
--- a/src/StepperMotor.cpp
+++ b/src/StepperMotor.cpp
@@ -108,12 +108,28 @@ int StepperMotor::initFOC() {
// alignment necessary for encoders!
// sensor and motor alignment - can be skipped
// by setting motor.sensor_direction and motor.zero_electric_angle
- _delay(500);
if(sensor){
exit_flag *= alignSensor();
// added the shaft_angle update
sensor->update();
- shaft_angle = sensor->getAngle();
+ shaft_angle = shaftAngle();
+
+ // aligning the current sensor - can be skipped
+ // checks if driver phases are the same as current sense phases
+ // and checks the direction of measuremnt.
+ if(exit_flag){
+ if(current_sense){
+ if (!current_sense->initialized) {
+ motor_status = FOCMotorStatus::motor_calib_failed;
+ SIMPLEFOC_DEBUG("MOT: Init FOC error, current sense not initialized");
+ exit_flag = 0;
+ }else{
+ exit_flag *= alignCurrentSense();
+ }
+ }
+ else { SIMPLEFOC_DEBUG("MOT: No current sense."); }
+ }
+
} else {
SIMPLEFOC_DEBUG("MOT: No sensor.");
if ((controller == MotionControlType::angle_openloop || controller == MotionControlType::velocity_openloop)){
@@ -136,6 +152,26 @@ int StepperMotor::initFOC() {
return exit_flag;
}
+// Calibrate the motor and current sense phases
+int StepperMotor::alignCurrentSense() {
+ int exit_flag = 1; // success
+
+ SIMPLEFOC_DEBUG("MOT: Align current sense.");
+
+ // align current sense and the driver
+ exit_flag = current_sense->driverAlign(voltage_sensor_align, 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
@@ -261,8 +297,6 @@ void StepperMotor::loopFOC() {
// if open-loop do nothing
if( controller==MotionControlType::angle_openloop || controller==MotionControlType::velocity_openloop ) return;
- // shaft angle
- shaft_angle = shaftAngle();
// if disabled do nothing
if(!enabled) return;
@@ -271,7 +305,40 @@ void StepperMotor::loopFOC() {
// This function will not have numerical issues because it uses Sensor::getMechanicalAngle()
// which is in range 0-2PI
electrical_angle = electricalAngle();
-
+ switch (torque_controller) {
+ case TorqueControlType::voltage:
+ // no need to do anything really
+ break;
+ case TorqueControlType::dc_current:
+ if(!current_sense) return;
+ // read overall current magnitude
+ current.q = current_sense->getDCCurrent(electrical_angle);
+ // filter the value values
+ current.q = LPF_current_q(current.q);
+ // calculate the phase voltage
+ voltage.q = PID_current_q(current_sp - current.q);
+ // d voltage - lag compensation
+ if(_isset(phase_inductance)) voltage.d = _constrain( -current_sp*shaft_velocity*pole_pairs*phase_inductance, -voltage_limit, voltage_limit);
+ else voltage.d = 0;
+ break;
+ case TorqueControlType::foc_current:
+ if(!current_sense) return;
+ // read dq currents
+ current = current_sense->getFOCCurrents(electrical_angle);
+ // filter values
+ current.q = LPF_current_q(current.q);
+ current.d = LPF_current_d(current.d);
+ // calculate the phase voltages
+ voltage.q = PID_current_q(current_sp - current.q);
+ voltage.d = PID_current_d(-current.d);
+ // d voltage - lag compensation - TODO verify
+ // if(_isset(phase_inductance)) voltage.d = _constrain( voltage.d - current_sp*shaft_velocity*pole_pairs*phase_inductance, -voltage_limit, voltage_limit);
+ break;
+ default:
+ // no torque control selected
+ SIMPLEFOC_DEBUG("MOT: no torque control selected!");
+ break;
+ }
// set the phase voltage - FOC heart function :)
setPhaseVoltage(voltage.q, voltage.d, electrical_angle);
}
@@ -310,56 +377,70 @@ void StepperMotor::move(float new_target) {
// estimate the motor current if phase reistance available and current_sense not available
if(!current_sense && _isset(phase_resistance)) current.q = (voltage.q - voltage_bemf)/phase_resistance;
- // choose control loop
+ // upgrade the current based voltage limit
switch (controller) {
case MotionControlType::torque:
- if(!_isset(phase_resistance)) voltage.q = target; // if voltage torque control
- else voltage.q = target*phase_resistance + voltage_bemf;
- voltage.q = _constrain(voltage.q, -voltage_limit, voltage_limit);
- // set d-component (lag compensation if known inductance)
- if(!_isset(phase_inductance)) voltage.d = 0;
- else voltage.d = _constrain( -target*shaft_velocity*pole_pairs*phase_inductance, -voltage_limit, voltage_limit);
+ if(torque_controller == TorqueControlType::voltage){ // if voltage torque control
+ if(!_isset(phase_resistance)) voltage.q = target;
+ else voltage.q = target*phase_resistance + voltage_bemf;
+ voltage.q = _constrain(voltage.q, -voltage_limit, voltage_limit);
+ // set d-component (lag compensation if known inductance)
+ if(!_isset(phase_inductance)) voltage.d = 0;
+ else voltage.d = _constrain( -target*shaft_velocity*pole_pairs*phase_inductance, -voltage_limit, voltage_limit);
+ }else{
+ current_sp = target; // if current/foc_current torque control
+ }
break;
case MotionControlType::angle:
+ // TODO sensor precision: this calculation is not numerically precise. The target value cannot express precise positions when
+ // the angles are large. This results in not being able to command small changes at high position values.
+ // to solve this, the delta-angle has to be calculated in a numerically precise way.
// angle set point
shaft_angle_sp = target;
// calculate velocity set point
shaft_velocity_sp = feed_forward_velocity + P_angle( shaft_angle_sp - shaft_angle );
- shaft_velocity_sp = _constrain(shaft_velocity_sp, -velocity_limit, velocity_limit);
- // calculate the torque command
+ shaft_velocity_sp = _constrain(shaft_velocity_sp,-velocity_limit, velocity_limit);
+ // calculate the torque command - sensor precision: this calculation is ok, but based on bad value from previous calculation
current_sp = PID_velocity(shaft_velocity_sp - shaft_velocity); // if voltage torque control
// if torque controlled through voltage
- // use voltage if phase-resistance not provided
- if(!_isset(phase_resistance)) voltage.q = current_sp;
- else voltage.q = _constrain( current_sp*phase_resistance + voltage_bemf , -voltage_limit, voltage_limit);
- // set d-component (lag compensation if known inductance)
- if(!_isset(phase_inductance)) voltage.d = 0;
- else voltage.d = _constrain( -current_sp*shaft_velocity*pole_pairs*phase_inductance, -voltage_limit, voltage_limit);
+ if(torque_controller == TorqueControlType::voltage){
+ // use voltage if phase-resistance not provided
+ if(!_isset(phase_resistance)) voltage.q = current_sp;
+ else voltage.q = _constrain( current_sp*phase_resistance + voltage_bemf , -voltage_limit, voltage_limit);
+ // set d-component (lag compensation if known inductance)
+ if(!_isset(phase_inductance)) voltage.d = 0;
+ else voltage.d = _constrain( -current_sp*shaft_velocity*pole_pairs*phase_inductance, -voltage_limit, voltage_limit);
+ }
break;
case MotionControlType::velocity:
- // velocity set point
+ // velocity set point - sensor precision: this calculation is numerically precise.
shaft_velocity_sp = target;
// calculate the torque command
current_sp = PID_velocity(shaft_velocity_sp - shaft_velocity); // if current/foc_current torque control
// if torque controlled through voltage control
- // use voltage if phase-resistance not provided
- if(!_isset(phase_resistance)) voltage.q = current_sp;
- else voltage.q = _constrain( current_sp*phase_resistance + voltage_bemf , -voltage_limit, voltage_limit);
- // set d-component (lag compensation if known inductance)
- if(!_isset(phase_inductance)) voltage.d = 0;
- else voltage.d = _constrain( -current_sp*shaft_velocity*pole_pairs*phase_inductance, -voltage_limit, voltage_limit);
+ if(torque_controller == TorqueControlType::voltage){
+ // use voltage if phase-resistance not provided
+ if(!_isset(phase_resistance)) voltage.q = current_sp;
+ else voltage.q = _constrain( current_sp*phase_resistance + voltage_bemf , -voltage_limit, voltage_limit);
+ // set d-component (lag compensation if known inductance)
+ if(!_isset(phase_inductance)) voltage.d = 0;
+ else voltage.d = _constrain( -current_sp*shaft_velocity*pole_pairs*phase_inductance, -voltage_limit, voltage_limit);
+ }
break;
case MotionControlType::velocity_openloop:
- // velocity control in open loop
+ // velocity control in open loop - sensor precision: this calculation is numerically precise.
shaft_velocity_sp = target;
voltage.q = velocityOpenloop(shaft_velocity_sp); // returns the voltage that is set to the motor
- voltage.d = 0; // TODO d-component lag-compensation
+ voltage.d = 0;
break;
case MotionControlType::angle_openloop:
- // angle control in open loop
+ // angle control in open loop -
+ // TODO sensor precision: this calculation NOT numerically precise, and subject
+ // to the same problems in small set-point changes at high angles
+ // as the closed loop version.
shaft_angle_sp = target;
voltage.q = angleOpenloop(shaft_angle_sp); // returns the voltage that is set to the motor
- voltage.d = 0; // TODO d-component lag-compensation
+ voltage.d = 0;
break;
}
}
diff --git a/src/StepperMotor.h b/src/StepperMotor.h
index 45d20c63..f76e7bcf 100644
--- a/src/StepperMotor.h
+++ b/src/StepperMotor.h
@@ -89,6 +89,8 @@ class StepperMotor: public FOCMotor
int alignSensor();
/** Motor and sensor alignment to the sensors absolute 0 angle */
int absoluteZeroSearch();
+ /** Current sense and motor phase alignment */
+ int alignCurrentSense();
// Open loop motion control
/**
diff --git a/src/common/base_classes/BLDCDriver.h b/src/common/base_classes/BLDCDriver.h
index f405d785..6ae8186f 100644
--- a/src/common/base_classes/BLDCDriver.h
+++ b/src/common/base_classes/BLDCDriver.h
@@ -2,40 +2,17 @@
#define BLDCDRIVER_H
#include "Arduino.h"
+#include "FOCDriver.h"
-
-enum PhaseState : uint8_t {
- PHASE_OFF = 0, // both sides of the phase are off
- PHASE_ON = 1, // both sides of the phase are driven with PWM, dead time is applied in 6-PWM mode
- PHASE_HI = 2, // only the high side of the phase is driven with PWM (6-PWM mode only)
- PHASE_LO = 3, // only the low side of the phase is driven with PWM (6-PWM mode only)
-};
-
-
-class BLDCDriver{
+class BLDCDriver: public FOCDriver{
public:
- /** Initialise hardware */
- virtual int init() = 0;
- /** Enable hardware */
- virtual void enable() = 0;
- /** Disable hardware */
- virtual void disable() = 0;
-
- long pwm_frequency; //!< pwm frequency value in hertz
- float voltage_power_supply; //!< power supply voltage
- float voltage_limit; //!< limiting voltage set to the motor
-
-
float dc_a; //!< currently set duty cycle on phaseA
float dc_b; //!< currently set duty cycle on phaseB
float dc_c; //!< currently set duty cycle on phaseC
- bool initialized = false; // true if driver was successfully initialized
- void* params = 0; // pointer to hardware specific parameters of driver
-
/**
- * Set phase voltages to the harware
+ * Set phase voltages to the hardware
*
* @param Ua - phase A voltage
* @param Ub - phase B voltage
@@ -51,6 +28,9 @@ class BLDCDriver{
* @param sa - phase C state : active / disabled ( high impedance )
*/
virtual void setPhaseState(PhaseState sa, PhaseState sb, PhaseState sc) = 0;
+
+ /** driver type getter function */
+ virtual DriverType type() override { return DriverType::BLDC; };
};
#endif
diff --git a/src/common/base_classes/CurrentSense.cpp b/src/common/base_classes/CurrentSense.cpp
index 03ea19ea..a3966ad6 100644
--- a/src/common/base_classes/CurrentSense.cpp
+++ b/src/common/base_classes/CurrentSense.cpp
@@ -1,4 +1,5 @@
#include "CurrentSense.h"
+#include "../../communication/SimpleFOCDebug.h"
// get current magnitude
@@ -27,7 +28,7 @@ float CurrentSense::getDCCurrent(float motor_electrical_angle){
return sign*_sqrt(ABcurrent.alpha*ABcurrent.alpha + ABcurrent.beta*ABcurrent.beta);
}
-// function used with the foc algorihtm
+// function used with the foc algorithm
// calculating DQ currents from phase currents
// - function calculating park and clarke transform of the phase currents
// - using getPhaseCurrents and getABCurrents internally
@@ -44,11 +45,21 @@ DQCurrent_s CurrentSense::getFOCCurrents(float angle_el){
return return_current;
}
-// function used with the foc algorihtm
+// function used with the foc algorithm
// calculating Alpha Beta currents from phase currents
// - function calculating Clarke transform of the phase currents
ABCurrent_s CurrentSense::getABCurrents(PhaseCurrent_s current){
+ // check if driver is an instance of StepperDriver
+ // if so there is no need to Clarke transform
+ if (driver_type == DriverType::Stepper){
+ ABCurrent_s return_ABcurrent;
+ return_ABcurrent.alpha = current.a;
+ return_ABcurrent.beta = current.b;
+ return return_ABcurrent;
+ }
+
+ // otherwise it's a BLDC motor and
// calculate clarke transform
float i_alpha, i_beta;
if(!current.c){
@@ -80,7 +91,7 @@ ABCurrent_s CurrentSense::getABCurrents(PhaseCurrent_s current){
return return_ABcurrent;
}
-// function used with the foc algorihtm
+// function used with the foc algorithm
// calculating D and Q currents from Alpha Beta currents and electrical angle
// - function calculating Clarke transform of the phase currents
DQCurrent_s CurrentSense::getDQCurrents(ABCurrent_s current, float angle_el){
@@ -97,8 +108,10 @@ DQCurrent_s CurrentSense::getDQCurrents(ABCurrent_s current, float angle_el){
/**
Driver linking to the current sense
*/
-void CurrentSense::linkDriver(BLDCDriver* _driver) {
- driver = _driver;
+void CurrentSense::linkDriver(FOCDriver* _driver) {
+ driver = _driver;
+ // save the driver type for easier access
+ driver_type = driver->type();
}
@@ -108,4 +121,355 @@ void CurrentSense::enable(){
void CurrentSense::disable(){
// nothing is done here, but you can override this function
-};
\ No newline at end of file
+};
+
+
+// Function aligning the current sense with motor driver
+// if all pins are connected well none of this is really necessary! - can be avoided
+// returns flag
+// 0 - fail
+// 1 - success and nothing changed
+// 2 - success but pins reconfigured
+// 3 - success but gains inverted
+// 4 - success but pins reconfigured and gains inverted
+// IMPORTANT, this function can be overriden in the child class
+int CurrentSense::driverAlign(float voltage, bool modulation_centered){
+
+ int exit_flag = 1;
+ if(skip_align) return exit_flag;
+
+ if (!initialized) return 0;
+
+ // check if stepper or BLDC
+ if(driver_type == DriverType::Stepper)
+ return alignStepperDriver(voltage, (StepperDriver*)driver, modulation_centered);
+ else
+ return alignBLDCDriver(voltage, (BLDCDriver*)driver, modulation_centered);
+}
+
+
+
+// Helper function to read and average phase currents
+PhaseCurrent_s CurrentSense::readAverageCurrents(int N) {
+ PhaseCurrent_s c = getPhaseCurrents();
+ for (int i = 0; i < N; i++) {
+ PhaseCurrent_s c1 = getPhaseCurrents();
+ c.a = c.a * 0.6f + 0.4f * c1.a;
+ c.b = c.b * 0.6f + 0.4f * c1.b;
+ c.c = c.c * 0.6f + 0.4f * c1.c;
+ _delay(3);
+ }
+ return c;
+};
+
+
+
+// Function aligning the current sense with motor driver
+// if all pins are connected well none of this is really necessary! - can be avoided
+// returns flag
+// 0 - fail
+// 1 - success and nothing changed
+// 2 - success but pins reconfigured
+// 3 - success but gains inverted
+// 4 - success but pins reconfigured and gains inverted
+int CurrentSense::alignBLDCDriver(float voltage, BLDCDriver* bldc_driver, bool modulation_centered){
+
+ bool phases_switched = 0;
+ bool phases_inverted = 0;
+
+ float zero = 0;
+ if(modulation_centered) zero = driver->voltage_limit/2.0;
+
+ // set phase A active and phases B and C down
+ // 300 ms of ramping
+ for(int i=0; i < 100; i++){
+ bldc_driver->setPwm(voltage/100.0*((float)i)+zero , zero, zero);
+ _delay(3);
+ }
+ _delay(500);
+ PhaseCurrent_s c_a = readAverageCurrents();
+ bldc_driver->setPwm(zero, zero, zero);
+ // check if currents are to low (lower than 100mA)
+ // TODO calculate the 100mA threshold from the ADC resolution
+ // if yes throw an error and return 0
+ // either the current sense is not connected or the current is
+ // too low for calibration purposes (one should raise the motor.voltage_sensor_align)
+ if((fabs(c_a.a) < 0.1f) && (fabs(c_a.b) < 0.1f) && (fabs(c_a.c) < 0.1f)){
+ SIMPLEFOC_DEBUG("CS: Err too low current, rise voltage!");
+ return 0; // measurement current too low
+ }
+
+
+ // now we have to determine
+ // 1) which pin correspond to which phase of the bldc driver
+ // 2) if the currents measured have good polarity
+ //
+ // > when we apply a voltage to a phase A of the driver what we expect to measure is the current I on the phase A
+ // and -I/2 on the phase B and I/2 on the phase C
+
+ // find the highest magnitude in c_a
+ // and make sure it's around 2 (1.5 at least) times higher than the other two
+ float ca[3] = {fabs(c_a.a), fabs(c_a.b), fabs(c_a.c)};
+ uint8_t max_i = -1; // max index
+ float max_c = 0; // max current
+ float max_c_ratio = 0; // max current ratio
+ for(int i = 0; i < 3; i++){
+ if(!ca[i]) continue; // current not measured
+ if(ca[i] > max_c){
+ max_c = ca[i];
+ max_i = i;
+ for(int j = 0; j < 3; j++){
+ if(i == j) continue;
+ if(!ca[j]) continue; // current not measured
+ float ratio = max_c / ca[j];
+ if(ratio > max_c_ratio) max_c_ratio = ratio;
+ }
+ }
+ }
+
+ // check the current magnitude ratios
+ // 1) if there is one current that is approximately 2 times higher than the other two
+ // this is the A current
+ // 2) if the max current is not at least 1.5 times higher than the other two
+ // we have two cases:
+ // - either we only measure two currents and the third one is not measured - then phase A is not measured
+ // - or the current sense is not connected properly
+
+ if(max_c_ratio >=1.5f){
+ switch (max_i){
+ case 1: // phase B is the max current
+ SIMPLEFOC_DEBUG("CS: Switch A-B");
+ // switch phase A and B
+ _swap(pinA, pinB);
+ _swap(offset_ia, offset_ib);
+ _swap(gain_a, gain_b);
+ _swap(c_a.b, c_a.b);
+ phases_switched = true; // signal that pins have been switched
+ break;
+ case 2: // phase C is the max current
+ SIMPLEFOC_DEBUG("CS: Switch A-C");
+ // switch phase A and C
+ _swap(pinA, pinC);
+ _swap(offset_ia, offset_ic);
+ _swap(gain_a, gain_c);
+ _swap(c_a.a, c_a.c);
+ phases_switched = true;// signal that pins have been switched
+ break;
+ }
+ // check if the current is negative and invert the gain if so
+ if( _sign(c_a.a) < 0 ){
+ SIMPLEFOC_DEBUG("CS: Inv A");
+ gain_a *= -1;
+ phases_inverted = true; // signal that pins have been inverted
+ }
+ }else if(_isset(pinA) && _isset(pinB) && _isset(pinC)){
+ // if all three currents are measured and none of them is significantly higher
+ // we have a problem with the current sense
+ SIMPLEFOC_DEBUG("CS: Err A - all currents same magnitude!");
+ return 0;
+ }else{ //phase A is not measured so put the _NC to the phase A
+ if(_isset(pinA) && !_isset(pinB)){
+ SIMPLEFOC_DEBUG("CS: Switch A-(B)NC");
+ _swap(pinA, pinB);
+ _swap(offset_ia, offset_ib);
+ _swap(gain_a, gain_b);
+ _swap(c_a.b, c_a.b);
+ phases_switched = true; // signal that pins have been switched
+ }else if(_isset(pinA) && !_isset(pinC)){
+ SIMPLEFOC_DEBUG("CS: Switch A-(C)NC");
+ _swap(pinA, pinC);
+ _swap(offset_ia, offset_ic);
+ _swap(gain_a, gain_c);
+ _swap(c_a.b, c_a.c);
+ phases_switched = true; // signal that pins have been switched
+ }
+ }
+ // at this point the current sensing on phase A can be either:
+ // - aligned with the driver phase A
+ // - or the phase A is not measured and the _NC is connected to the phase A
+ //
+ // In either case A is done, now we have to check the phase B and C
+
+
+ // set phase B active and phases A and C down
+ // 300 ms of ramping
+ for(int i=0; i < 100; i++){
+ bldc_driver->setPwm(zero, voltage/100.0*((float)i)+zero, zero);
+ _delay(3);
+ }
+ _delay(500);
+ PhaseCurrent_s c_b = readAverageCurrents();
+ bldc_driver->setPwm(zero, zero, zero);
+
+ // check the phase B
+ // find the highest magnitude in c_b
+ // and make sure it's around 2 (1.5 at least) times higher than the other two
+ float cb[3] = {fabs(c_b.a), fabs(c_b.b), fabs(c_b.c)};
+ max_i = -1; // max index
+ max_c = 0; // max current
+ max_c_ratio = 0; // max current ratio
+ for(int i = 0; i < 3; i++){
+ if(!cb[i]) continue; // current not measured
+ if(cb[i] > max_c){
+ max_c = cb[i];
+ max_i = i;
+ for(int j = 0; j < 3; j++){
+ if(i == j) continue;
+ if(!cb[j]) continue; // current not measured
+ float ratio = max_c / cb[j];
+ if(ratio > max_c_ratio) max_c_ratio = ratio;
+ }
+ }
+ }
+ if(max_c_ratio >= 1.5f){
+ switch (max_i){
+ case 0: // phase A is the max current
+ // this is an error as phase A is already aligned
+ SIMPLEFOC_DEBUG("CS: Err align B");
+ return 0;
+ case 2: // phase C is the max current
+ SIMPLEFOC_DEBUG("CS: Switch B-C");
+ _swap(pinB, pinC);
+ _swap(offset_ib, offset_ic);
+ _swap(gain_b, gain_c);
+ _swap(c_b.b, c_b.c);
+ phases_switched = true; // signal that pins have been switched
+ break;
+ }
+ // check if the current is negative and invert the gain if so
+ if( _sign(c_b.b) < 0 ){
+ SIMPLEFOC_DEBUG("CS: Inv B");
+ gain_b *= -1;
+ phases_inverted = true; // signal that pins have been inverted
+ }
+ }else if(_isset(pinB) && _isset(pinC)){
+ // if all three currents are measured and none of them is significantly higher
+ // we have a problem with the current sense
+ SIMPLEFOC_DEBUG("CS: Err B - all currents same magnitude!");
+ return 0;
+ }else{ //phase B is not measured so put the _NC to the phase B
+ if(_isset(pinB) && !_isset(pinC)){
+ SIMPLEFOC_DEBUG("CS: Switch B-(C)NC");
+ _swap(pinB, pinC);
+ _swap(offset_ib, offset_ic);
+ _swap(gain_b, gain_c);
+ _swap(c_b.b, c_b.c);
+ phases_switched = true; // signal that pins have been switched
+ }
+ }
+ // at this point the current sensing on phase A and B can be either:
+ // - aligned with the driver phase A and B
+ // - or the phase A and B are not measured and the _NC is connected to the phase A and B
+ //
+ // In either case A and B is done, now we have to check the phase C
+ // phase C is also aligned if it is measured (not _NC)
+ // we have to check if the current is negative and invert the gain if so
+ if(_isset(pinC)){
+ if( _sign(c_b.c) > 0 ){ // the expected current is -I/2 (if the phase A and B are aligned and C has correct polarity)
+ SIMPLEFOC_DEBUG("CS: Inv C");
+ gain_c *= -1;
+ phases_inverted = true; // signal that pins have been inverted
+ }
+ }
+
+ // construct the return flag
+ // if the phases have been switched return 2
+ // if the gains have been inverted return 3
+ // if both return 4
+ uint8_t exit_flag = 1;
+ if(phases_switched) exit_flag += 1;
+ if(phases_inverted) exit_flag += 2;
+ return exit_flag;
+}
+
+
+// Function aligning the current sense with motor driver
+// if all pins are connected well none of this is really necessary! - can be avoided
+// returns flag
+// 0 - fail
+// 1 - success and nothing changed
+// 2 - success but pins reconfigured
+// 3 - success but gains inverted
+// 4 - success but pins reconfigured and gains inverted
+int CurrentSense::alignStepperDriver(float voltage, StepperDriver* stepper_driver, bool modulation_centered){
+
+ _UNUSED(modulation_centered);
+
+ bool phases_switched = 0;
+ bool phases_inverted = 0;
+
+ if(!_isset(pinA) || !_isset(pinB)){
+ SIMPLEFOC_DEBUG("CS: Pins A & B not specified!");
+ return 0;
+ }
+
+ // set phase A active and phases B down
+ // ramp 300ms
+ for(int i=0; i < 100; i++){
+ stepper_driver->setPwm(voltage/100.0*((float)i), 0);
+ _delay(3);
+ }
+ _delay(500);
+ PhaseCurrent_s c = readAverageCurrents();
+ // disable the phases
+ stepper_driver->setPwm(0, 0);
+ if (fabs(c.a) < 0.1f && fabs(c.b) < 0.1f ){
+ SIMPLEFOC_DEBUG("CS: Err too low current!");
+ return 0; // measurement current too low
+ }
+ // align phase A
+ // 1) only one phase can be measured so we first measure which ADC pin corresponds
+ // to the phase A by comparing the magnitude
+ if (fabs(c.a) < fabs(c.b)){
+ SIMPLEFOC_DEBUG("CS: Switch A-B");
+ // switch phase A and B
+ _swap(pinA, pinB);
+ _swap(offset_ia, offset_ib);
+ _swap(gain_a, gain_b);
+ phases_switched = true; // signal that pins have been switched
+ }
+ // 2) check if measured current a is positive and invert if not
+ if (c.a < 0){
+ SIMPLEFOC_DEBUG("CS: Inv A");
+ gain_a *= -1;
+ phases_inverted = true; // signal that pins have been inverted
+ }
+
+ // at this point the driver's phase A is aligned with the ADC pinA
+ // and the pin B should be the phase B
+
+ // set phase B active and phases A down
+ // ramp 300ms
+ for(int i=0; i < 100; i++){
+ stepper_driver->setPwm(0, voltage/100.0*((float)i));
+ _delay(3);
+ }
+ _delay(500);
+ c = readAverageCurrents();
+ stepper_driver->setPwm(0, 0);
+
+ // phase B should be aligned
+ // 1) we just need to verify that it has been measured
+ if (fabs(c.a) < 0.1f && fabs(c.b) < 0.1f ){
+ SIMPLEFOC_DEBUG("CS: Err too low current!");
+ return 0; // measurement current too low
+ }
+ // 2) check if measured current a is positive and invert if not
+ if (c.b < 0){
+ SIMPLEFOC_DEBUG("CS: Inv B");
+ gain_b *= -1;
+ phases_inverted = true; // signal that pins have been inverted
+ }
+
+ // construct the return flag
+ // if success and nothing changed return 1
+ // if the phases have been switched return 2
+ // if the gains have been inverted return 3
+ // if both return 4
+ uint8_t exit_flag = 1;
+ if(phases_switched) exit_flag += 1;
+ if(phases_inverted) exit_flag += 2;
+ return exit_flag;
+}
+
+
diff --git a/src/common/base_classes/CurrentSense.h b/src/common/base_classes/CurrentSense.h
index 1c839053..d3f7f8ed 100644
--- a/src/common/base_classes/CurrentSense.h
+++ b/src/common/base_classes/CurrentSense.h
@@ -1,12 +1,15 @@
#ifndef CURRENTSENSE_H
#define CURRENTSENSE_H
-#include "BLDCDriver.h"
+#include "FOCDriver.h"
#include "../foc_utils.h"
+#include "../time_utils.h"
+#include "StepperDriver.h"
+#include "BLDCDriver.h"
/**
* Current sensing abstract class defintion
- * Each current sensoring implementation needs to extend this interface
+ * Each current sensing implementation needs to extend this interface
*/
class CurrentSense{
public:
@@ -23,24 +26,49 @@ class CurrentSense{
* Linking the current sense with the motor driver
* Only necessary if synchronisation in between the two is required
*/
- void linkDriver(BLDCDriver *driver);
+ void linkDriver(FOCDriver *driver);
// variables
bool skip_align = false; //!< variable signaling that the phase current direction should be verified during initFOC()
- BLDCDriver* driver = nullptr; //!< driver link
+ FOCDriver* driver = nullptr; //!< driver link
bool initialized = false; // true if current sense was successfully initialized
void* params = 0; //!< pointer to hardware specific parameters of current sensing
+ DriverType driver_type = DriverType::Unknown; //!< driver type (BLDC or Stepper)
+
+ // ADC measurement gain for each phase
+ // support for different gains for different phases of more commonly - inverted phase currents
+ // this should be automated later
+ float gain_a; //!< phase A gain
+ float gain_b; //!< phase B gain
+ float gain_c; //!< phase C gain
+
+ float offset_ia; //!< zero current A voltage value (center of the adc reading)
+ float offset_ib; //!< zero current B voltage value (center of the adc reading)
+ float offset_ic; //!< zero current C voltage value (center of the adc reading)
+
+ // hardware variables
+ int pinA; //!< pin A analog pin for current measurement
+ int pinB; //!< pin B analog pin for current measurement
+ int pinC; //!< pin C analog pin for current measurement
+
/**
* Function intended to verify if:
* - phase current are oriented properly
* - if their order is the same as driver phases
*
* This function corrects the alignment errors if possible ans if no such thing is needed it can be left empty (return 1)
- * @returns - 0 - for failure & positive number (with status) - for success
+ * @returns -
+ 0 - failure
+ 1 - success and nothing changed
+ 2 - success but pins reconfigured
+ 3 - success but gains inverted
+ 4 - success but pins reconfigured and gains inverted
+ *
+ * IMPORTANT: Default implementation provided in the CurrentSense class, but can be overriden in the child classes
*/
- virtual int driverAlign(float align_voltage) = 0;
+ virtual int driverAlign(float align_voltage, bool modulation_centered = false);
/**
* Function rading the phase currents a, b and c
@@ -80,7 +108,7 @@ class CurrentSense{
/**
* Function used for Park transform in FOC control
- * It reads the Alpha Beta currents and electircal angle of the motor
+ * It reads the Alpha Beta currents and electrical angle of the motor
* It returns the D and Q currents
*
* @param current - phase current
@@ -98,6 +126,20 @@ class CurrentSense{
* override it to do something useful.
*/
virtual void disable();
+
+ /**
+ * Function used to align the current sense with the BLDC motor driver
+ */
+ int alignBLDCDriver(float align_voltage, BLDCDriver* driver, bool modulation_centered);
+ /**
+ * Function used to align the current sense with the Stepper motor driver
+ */
+ int alignStepperDriver(float align_voltage, StepperDriver* driver, bool modulation_centered);
+ /**
+ * Function used to read the average current values over N samples
+ */
+ PhaseCurrent_s readAverageCurrents(int N=100);
+
};
#endif
\ No newline at end of file
diff --git a/src/common/base_classes/FOCDriver.h b/src/common/base_classes/FOCDriver.h
new file mode 100644
index 00000000..944251a4
--- /dev/null
+++ b/src/common/base_classes/FOCDriver.h
@@ -0,0 +1,47 @@
+#ifndef FOCDRIVER_H
+#define FOCDRIVER_H
+
+#include "Arduino.h"
+
+
+enum PhaseState : uint8_t {
+ PHASE_OFF = 0, // both sides of the phase are off
+ PHASE_ON = 1, // both sides of the phase are driven with PWM, dead time is applied in 6-PWM mode
+ PHASE_HI = 2, // only the high side of the phase is driven with PWM (6-PWM mode only)
+ PHASE_LO = 3, // only the low side of the phase is driven with PWM (6-PWM mode only)
+};
+
+
+enum DriverType{
+ Unknown=0,
+ BLDC=1,
+ Stepper=2
+};
+
+/**
+ * FOC driver class
+ */
+class FOCDriver{
+ public:
+
+ /** Initialise hardware */
+ virtual int init() = 0;
+ /** Enable hardware */
+ virtual void enable() = 0;
+ /** Disable hardware */
+ virtual void disable() = 0;
+
+ long pwm_frequency; //!< pwm frequency value in hertz
+ float voltage_power_supply; //!< power supply voltage
+ float voltage_limit; //!< limiting voltage set to the motor
+
+ bool initialized = false; //!< true if driver was successfully initialized
+ void* params = 0; //!< pointer to hardware specific parameters of driver
+
+ bool enable_active_high = true; //!< enable pin should be set to high to enable the driver (default is HIGH)
+
+ /** get the driver type*/
+ virtual DriverType type() = 0;
+};
+
+#endif
diff --git a/src/common/base_classes/StepperDriver.h b/src/common/base_classes/StepperDriver.h
index 2006fc8c..9864b235 100644
--- a/src/common/base_classes/StepperDriver.h
+++ b/src/common/base_classes/StepperDriver.h
@@ -1,32 +1,30 @@
#ifndef STEPPERDRIVER_H
#define STEPPERDRIVER_H
-#include "drivers/hardware_api.h"
+#include "Arduino.h"
+#include "FOCDriver.h"
-class StepperDriver{
+class StepperDriver: public FOCDriver{
public:
- /** Initialise hardware */
- virtual int init() = 0;
- /** Enable hardware */
- virtual void enable() = 0;
- /** Disable hardware */
- virtual void disable() = 0;
-
- long pwm_frequency; //!< pwm frequency value in hertz
- float voltage_power_supply; //!< power supply voltage
- float voltage_limit; //!< limiting voltage set to the motor
-
- bool initialized = false; // true if driver was successfully initialized
- void* params = 0; // pointer to hardware specific parameters of driver
-
/**
- * Set phase voltages to the harware
+ * Set phase voltages to the hardware
*
* @param Ua phase A voltage
* @param Ub phase B voltage
*/
virtual void setPwm(float Ua, float Ub) = 0;
+
+ /**
+ * Set phase state, enable/disable
+ *
+ * @param sc - phase A state : active / disabled ( high impedance )
+ * @param sb - phase B state : active / disabled ( high impedance )
+ */
+ virtual void setPhaseState(PhaseState sa, PhaseState sb) = 0;
+
+ /** driver type getter function */
+ virtual DriverType type() override { return DriverType::Stepper; } ;
};
#endif
\ No newline at end of file
diff --git a/src/common/foc_utils.h b/src/common/foc_utils.h
index f2bc9ef6..2094ab26 100644
--- a/src/common/foc_utils.h
+++ b/src/common/foc_utils.h
@@ -14,6 +14,8 @@
#define _UNUSED(v) (void) (v)
#define _powtwo(x) (1 << (x))
+#define _swap(a, b) { auto temp = a; a = b; b = temp; }
+
// utility defines
#define _2_SQRT3 1.15470053838f
#define _SQRT3 1.73205080757f
@@ -33,7 +35,7 @@
#define _HIGH_IMPEDANCE 0
#define _HIGH_Z _HIGH_IMPEDANCE
#define _ACTIVE 1
-#define _NC (NOT_SET)
+#define _NC ((int) NOT_SET)
#define MIN_ANGLE_DETECT_MOVEMENT (_2PI/101.0f)
diff --git a/src/communication/SimpleFOCDebug.h b/src/communication/SimpleFOCDebug.h
index 668e08af..d0ff611f 100644
--- a/src/communication/SimpleFOCDebug.h
+++ b/src/communication/SimpleFOCDebug.h
@@ -32,6 +32,7 @@
*
**/
+// #define SIMPLEFOC_DISABLE_DEBUG
#ifndef SIMPLEFOC_DISABLE_DEBUG
@@ -65,10 +66,6 @@ class SimpleFOCDebug {
#define SIMPLEFOC_DEBUG(msg, ...) \
SimpleFOCDebug::println(F(msg), ##__VA_ARGS__)
-
-
-
-
#else //ifndef SIMPLEFOC_DISABLE_DEBUG
diff --git a/src/current_sense/GenericCurrentSense.cpp b/src/current_sense/GenericCurrentSense.cpp
index 635535fa..54c4f12e 100644
--- a/src/current_sense/GenericCurrentSense.cpp
+++ b/src/current_sense/GenericCurrentSense.cpp
@@ -54,110 +54,10 @@ PhaseCurrent_s GenericCurrentSense::getPhaseCurrents(){
// returns flag
// 0 - fail
// 1 - success and nothing changed
-// 2 - success but pins reconfigured
-// 3 - success but gains inverted
-// 4 - success but pins reconfigured and gains inverted
-int GenericCurrentSense::driverAlign(float voltage){
+int GenericCurrentSense::driverAlign(float voltage, bool modulation_centered){
_UNUSED(voltage) ; // remove unused parameter warning
int exit_flag = 1;
if(skip_align) return exit_flag;
-
if (!initialized) return 0;
-
- // // set phase A active and phases B and C down
- // driver->setPwm(voltage, 0, 0);
- // _delay(200);
- // PhaseCurrent_s c = getPhaseCurrents();
- // // read the current 100 times ( arbitrary number )
- // for (int i = 0; i < 100; i++) {
- // PhaseCurrent_s c1 = getPhaseCurrents();
- // c.a = c.a*0.6f + 0.4f*c1.a;
- // c.b = c.b*0.6f + 0.4f*c1.b;
- // c.c = c.c*0.6f + 0.4f*c1.c;
- // _delay(3);
- // }
- // driver->setPwm(0, 0, 0);
- // // align phase A
- // float ab_ratio = fabs(c.a / c.b);
- // float ac_ratio = c.c ? fabs(c.a / c.c) : 0;
- // if( ab_ratio > 1.5f ){ // should be ~2
- // gain_a *= _sign(c.a);
- // }else if( ab_ratio < 0.7f ){ // should be ~0.5
- // // switch phase A and B
- // int tmp_pinA = pinA;
- // pinA = pinB;
- // pinB = tmp_pinA;
- // gain_a *= _sign(c.b);
- // exit_flag = 2; // signal that pins have been switched
- // }else if(_isset(pinC) && ac_ratio < 0.7f ){ // should be ~0.5
- // // switch phase A and C
- // int tmp_pinA = pinA;
- // pinA = pinC;
- // pinC= tmp_pinA;
- // gain_a *= _sign(c.c);
- // exit_flag = 2;// signal that pins have been switched
- // }else{
- // // error in current sense - phase either not measured or bad connection
- // return 0;
- // }
-
- // // set phase B active and phases A and C down
- // driver->setPwm(0, voltage, 0);
- // _delay(200);
- // c = getPhaseCurrents();
- // // read the current 50 times
- // for (int i = 0; i < 100; i++) {
- // PhaseCurrent_s c1 = getPhaseCurrents();
- // c.a = c.a*0.6f + 0.4f*c1.a;
- // c.b = c.b*0.6f + 0.4f*c1.b;
- // c.c = c.c*0.6f + 0.4f*c1.c;
- // _delay(3);
- // }
- // driver->setPwm(0, 0, 0);
- // float ba_ratio = fabs(c.b/c.a);
- // float bc_ratio = c.c ? fabs(c.b / c.c) : 0;
- // if( ba_ratio > 1.5f ){ // should be ~2
- // gain_b *= _sign(c.b);
- // }else if( ba_ratio < 0.7f ){ // it should be ~0.5
- // // switch phase A and B
- // int tmp_pinB = pinB;
- // pinB = pinA;
- // pinA = tmp_pinB;
- // gain_b *= _sign(c.a);
- // exit_flag = 2; // signal that pins have been switched
- // }else if(_isset(pinC) && bc_ratio < 0.7f ){ // should be ~0.5
- // // switch phase A and C
- // int tmp_pinB = pinB;
- // pinB = pinC;
- // pinC = tmp_pinB;
- // gain_b *= _sign(c.c);
- // exit_flag = 2; // signal that pins have been switched
- // }else{
- // // error in current sense - phase either not measured or bad connection
- // return 0;
- // }
-
- // // if phase C measured
- // if(_isset(pinC)){
- // // set phase B active and phases A and C down
- // driver->setPwm(0, 0, voltage);
- // _delay(200);
- // c = getPhaseCurrents();
- // // read the adc voltage 500 times ( arbitrary number )
- // for (int i = 0; i < 50; i++) {
- // PhaseCurrent_s c1 = getPhaseCurrents();
- // c.c = (c.c+c1.c)/50.0f;
- // }
- // driver->setPwm(0, 0, 0);
- // gain_c *= _sign(c.c);
- // }
-
- // if(gain_a < 0 || gain_b < 0 || gain_c < 0) exit_flag +=2;
- // exit flag is either
- // 0 - fail
- // 1 - success and nothing changed
- // 2 - success but pins reconfigured
- // 3 - success but gains inverted
- // 4 - success but pins reconfigured and gains inverted
return exit_flag;
}
diff --git a/src/current_sense/GenericCurrentSense.h b/src/current_sense/GenericCurrentSense.h
index a63df49d..02bf8fa9 100644
--- a/src/current_sense/GenericCurrentSense.h
+++ b/src/current_sense/GenericCurrentSense.h
@@ -20,7 +20,7 @@ class GenericCurrentSense: public CurrentSense{
// CurrentSense interface implementing functions
int init() override;
PhaseCurrent_s getPhaseCurrents() override;
- int driverAlign(float align_voltage) override;
+ int driverAlign(float align_voltage, bool modulation_centered) override;
PhaseCurrent_s (*readCallback)() = nullptr; //!< function pointer to sensor reading
diff --git a/src/current_sense/InlineCurrentSense.cpp b/src/current_sense/InlineCurrentSense.cpp
index c3db74ef..35c97765 100644
--- a/src/current_sense/InlineCurrentSense.cpp
+++ b/src/current_sense/InlineCurrentSense.cpp
@@ -44,8 +44,14 @@ int InlineCurrentSense::init(){
params = _configureADCInline(drv_params,pinA,pinB,pinC);
// if init failed return fail
if (params == SIMPLEFOC_CURRENT_SENSE_INIT_FAILED) return 0;
+ // set the center pwm (0 voltage vector)
+ if(driver_type==DriverType::BLDC)
+ static_cast(driver)->setPwm(driver->voltage_limit/2, driver->voltage_limit/2, driver->voltage_limit/2);
// calibrate zero offsets
calibrateOffsets();
+ // set zero voltage to all phases
+ if(driver_type==DriverType::BLDC)
+ static_cast(driver)->setPwm(0,0,0);
// set the initialized flag
initialized = (params!=SIMPLEFOC_CURRENT_SENSE_INIT_FAILED);
// return success
@@ -80,174 +86,3 @@ PhaseCurrent_s InlineCurrentSense::getPhaseCurrents(){
current.c = (!_isset(pinC)) ? 0 : (_readADCVoltageInline(pinC, params) - offset_ic)*gain_c; // amps
return current;
}
-
-// Function aligning the current sense with motor driver
-// if all pins are connected well none of this is really necessary! - can be avoided
-// returns flag
-// 0 - fail
-// 1 - success and nothing changed
-// 2 - success but pins reconfigured
-// 3 - success but gains inverted
-// 4 - success but pins reconfigured and gains inverted
-int InlineCurrentSense::driverAlign(float voltage){
-
- int exit_flag = 1;
- if(skip_align) return exit_flag;
-
- if (driver==nullptr) {
- SIMPLEFOC_DEBUG("CUR: No driver linked!");
- return 0;
- }
-
- if (!initialized) return 0;
-
- if(_isset(pinA)){
- // set phase A active and phases B and C down
- driver->setPwm(voltage, 0, 0);
- _delay(2000);
- PhaseCurrent_s c = getPhaseCurrents();
- // read the current 100 times ( arbitrary number )
- for (int i = 0; i < 100; i++) {
- PhaseCurrent_s c1 = getPhaseCurrents();
- c.a = c.a*0.6f + 0.4f*c1.a;
- c.b = c.b*0.6f + 0.4f*c1.b;
- c.c = c.c*0.6f + 0.4f*c1.c;
- _delay(3);
- }
- driver->setPwm(0, 0, 0);
- // align phase A
- float ab_ratio = c.b ? fabs(c.a / c.b) : 0;
- float ac_ratio = c.c ? fabs(c.a / c.c) : 0;
- if(_isset(pinB) && ab_ratio > 1.5f ){ // should be ~2
- gain_a *= _sign(c.a);
- }else if(_isset(pinC) && ac_ratio > 1.5f ){ // should be ~2
- gain_a *= _sign(c.a);
- }else if(_isset(pinB) && ab_ratio < 0.7f ){ // should be ~0.5
- // switch phase A and B
- int tmp_pinA = pinA;
- pinA = pinB;
- pinB = tmp_pinA;
- float tmp_offsetA = offset_ia;
- offset_ia = offset_ib;
- offset_ib = tmp_offsetA;
- gain_a *= _sign(c.b);
- exit_flag = 2; // signal that pins have been switched
- }else if(_isset(pinC) && ac_ratio < 0.7f ){ // should be ~0.5
- // switch phase A and C
- int tmp_pinA = pinA;
- pinA = pinC;
- pinC= tmp_pinA;
- float tmp_offsetA = offset_ia;
- offset_ia = offset_ic;
- offset_ic = tmp_offsetA;
- gain_a *= _sign(c.c);
- exit_flag = 2;// signal that pins have been switched
- }else{
- // error in current sense - phase either not measured or bad connection
- return 0;
- }
- }
-
- if(_isset(pinB)){
- // set phase B active and phases A and C down
- driver->setPwm(0, voltage, 0);
- _delay(200);
- PhaseCurrent_s c = getPhaseCurrents();
- // read the current 50 times
- for (int i = 0; i < 100; i++) {
- PhaseCurrent_s c1 = getPhaseCurrents();
- c.a = c.a*0.6f + 0.4f*c1.a;
- c.b = c.b*0.6f + 0.4f*c1.b;
- c.c = c.c*0.6f + 0.4f*c1.c;
- _delay(3);
- }
- driver->setPwm(0, 0, 0);
- float ba_ratio = c.a ? fabs(c.b / c.a) : 0;
- float bc_ratio = c.c ? fabs(c.b / c.c) : 0;
- if(_isset(pinA) && ba_ratio > 1.5f ){ // should be ~2
- gain_b *= _sign(c.b);
- }else if(_isset(pinC) && bc_ratio > 1.5f ){ // should be ~2
- gain_b *= _sign(c.b);
- }else if(_isset(pinA) && ba_ratio < 0.7f ){ // it should be ~0.5
- // switch phase A and B
- int tmp_pinB = pinB;
- pinB = pinA;
- pinA = tmp_pinB;
- float tmp_offsetB = offset_ib;
- offset_ib = offset_ia;
- offset_ia = tmp_offsetB;
- gain_b *= _sign(c.a);
- exit_flag = 2; // signal that pins have been switched
- }else if(_isset(pinC) && bc_ratio < 0.7f ){ // should be ~0.5
- // switch phase A and C
- int tmp_pinB = pinB;
- pinB = pinC;
- pinC = tmp_pinB;
- float tmp_offsetB = offset_ib;
- offset_ib = offset_ic;
- offset_ic = tmp_offsetB;
- gain_b *= _sign(c.c);
- exit_flag = 2; // signal that pins have been switched
- }else{
- // error in current sense - phase either not measured or bad connection
- return 0;
- }
- }
-
- // if phase C measured
- if(_isset(pinC)){
- // set phase C active and phases A and B down
- driver->setPwm(0, 0, voltage);
- _delay(200);
- PhaseCurrent_s c = getPhaseCurrents();
- // read the adc voltage 500 times ( arbitrary number )
- for (int i = 0; i < 100; i++) {
- PhaseCurrent_s c1 = getPhaseCurrents();
- c.a = c.a*0.6f + 0.4f*c1.a;
- c.b = c.b*0.6f + 0.4f*c1.b;
- c.c = c.c*0.6f + 0.4f*c1.c;
- _delay(3);
- }
- driver->setPwm(0, 0, 0);
- float ca_ratio = c.a ? fabs(c.c / c.a) : 0;
- float cb_ratio = c.b ? fabs(c.c / c.b) : 0;
- if(_isset(pinA) && ca_ratio > 1.5f ){ // should be ~2
- gain_c *= _sign(c.c);
- }else if(_isset(pinB) && cb_ratio > 1.5f ){ // should be ~2
- gain_c *= _sign(c.c);
- }else if(_isset(pinA) && ca_ratio < 0.7f ){ // it should be ~0.5
- // switch phase A and C
- int tmp_pinC = pinC;
- pinC = pinA;
- pinA = tmp_pinC;
- float tmp_offsetC = offset_ic;
- offset_ic = offset_ia;
- offset_ia = tmp_offsetC;
- gain_c *= _sign(c.a);
- exit_flag = 2; // signal that pins have been switched
- }else if(_isset(pinB) && cb_ratio < 0.7f ){ // should be ~0.5
- // switch phase B and C
- int tmp_pinC = pinC;
- pinC = pinB;
- pinB = tmp_pinC;
- float tmp_offsetC = offset_ic;
- offset_ic = offset_ib;
- offset_ib = tmp_offsetC;
- gain_c *= _sign(c.b);
- exit_flag = 2; // signal that pins have been switched
- }else{
- // error in current sense - phase either not measured or bad connection
- return 0;
- }
- }
-
- if(gain_a < 0 || gain_b < 0 || gain_c < 0) exit_flag +=2;
- // exit flag is either
- // 0 - fail
- // 1 - success and nothing changed
- // 2 - success but pins reconfigured
- // 3 - success but gains inverted
- // 4 - success but pins reconfigured and gains inverted
-
- return exit_flag;
-}
diff --git a/src/current_sense/InlineCurrentSense.h b/src/current_sense/InlineCurrentSense.h
index 5fdca7d7..94be0f75 100644
--- a/src/current_sense/InlineCurrentSense.h
+++ b/src/current_sense/InlineCurrentSense.h
@@ -6,10 +6,13 @@
#include "../common/time_utils.h"
#include "../common/defaults.h"
#include "../common/base_classes/CurrentSense.h"
+#include "../common/base_classes/StepperDriver.h"
+#include "../common/base_classes/BLDCDriver.h"
#include "../common/lowpass_filter.h"
#include "hardware_api.h"
+
class InlineCurrentSense: public CurrentSense{
public:
/**
@@ -33,31 +36,9 @@ class InlineCurrentSense: public CurrentSense{
// CurrentSense interface implementing functions
int init() override;
PhaseCurrent_s getPhaseCurrents() override;
- int driverAlign(float align_voltage) override;
-
- // ADC measuremnet gain for each phase
- // support for different gains for different phases of more commonly - inverted phase currents
- // this should be automated later
- float gain_a; //!< phase A gain
- float gain_b; //!< phase B gain
- float gain_c; //!< phase C gain
-
- // // per phase low pass fileters
- // LowPassFilter lpf_a{DEF_LPF_PER_PHASE_CURRENT_SENSE_Tf}; //!< current A low pass filter
- // LowPassFilter lpf_b{DEF_LPF_PER_PHASE_CURRENT_SENSE_Tf}; //!< current B low pass filter
- // LowPassFilter lpf_c{DEF_LPF_PER_PHASE_CURRENT_SENSE_Tf}; //!< current C low pass filter
- float offset_ia; //!< zero current A voltage value (center of the adc reading)
- float offset_ib; //!< zero current B voltage value (center of the adc reading)
- float offset_ic; //!< zero current C voltage value (center of the adc reading)
-
private:
- // hardware variables
- int pinA; //!< pin A analog pin for current measurement
- int pinB; //!< pin B analog pin for current measurement
- int pinC; //!< pin C analog pin for current measurement
-
// gain variables
float shunt_resistor; //!< Shunt resistor value
float amp_gain; //!< amp gain value
diff --git a/src/current_sense/LowsideCurrentSense.cpp b/src/current_sense/LowsideCurrentSense.cpp
index 9b07d353..a0026ae3 100644
--- a/src/current_sense/LowsideCurrentSense.cpp
+++ b/src/current_sense/LowsideCurrentSense.cpp
@@ -49,8 +49,14 @@ int LowsideCurrentSense::init(){
// sync the driver
void* r = _driverSyncLowSide(driver->params, params);
if(r == SIMPLEFOC_CURRENT_SENSE_INIT_FAILED) return 0;
+ // set the center pwm (0 voltage vector)
+ if(driver_type==DriverType::BLDC)
+ static_cast(driver)->setPwm(driver->voltage_limit/2, driver->voltage_limit/2, driver->voltage_limit/2);
// calibrate zero offsets
calibrateOffsets();
+ // set zero voltage to all phases
+ if(driver_type==DriverType::BLDC)
+ static_cast(driver)->setPwm(0,0,0);
// set the initialized flag
initialized = (params!=SIMPLEFOC_CURRENT_SENSE_INIT_FAILED);
// return success
@@ -87,169 +93,3 @@ PhaseCurrent_s LowsideCurrentSense::getPhaseCurrents(){
current.c = (!_isset(pinC)) ? 0 : (_readADCVoltageLowSide(pinC, params) - offset_ic)*gain_c; // amps
return current;
}
-
-// Function aligning the current sense with motor driver
-// if all pins are connected well none of this is really necessary! - can be avoided
-// returns flag
-// 0 - fail
-// 1 - success and nothing changed
-// 2 - success but pins reconfigured
-// 3 - success but gains inverted
-// 4 - success but pins reconfigured and gains inverted
-int LowsideCurrentSense::driverAlign(float voltage){
-
- int exit_flag = 1;
- if(skip_align) return exit_flag;
-
- if (!initialized) return 0;
-
- if(_isset(pinA)){
- // set phase A active and phases B and C down
- driver->setPwm(voltage, 0, 0);
- _delay(2000);
- PhaseCurrent_s c = getPhaseCurrents();
- // read the current 100 times ( arbitrary number )
- for (int i = 0; i < 100; i++) {
- PhaseCurrent_s c1 = getPhaseCurrents();
- c.a = c.a*0.6f + 0.4f*c1.a;
- c.b = c.b*0.6f + 0.4f*c1.b;
- c.c = c.c*0.6f + 0.4f*c1.c;
- _delay(3);
- }
- driver->setPwm(0, 0, 0);
- // align phase A
- float ab_ratio = c.b ? fabs(c.a / c.b) : 0;
- float ac_ratio = c.c ? fabs(c.a / c.c) : 0;
- if(_isset(pinB) && ab_ratio > 1.5f ){ // should be ~2
- gain_a *= _sign(c.a);
- }else if(_isset(pinC) && ac_ratio > 1.5f ){ // should be ~2
- gain_a *= _sign(c.a);
- }else if(_isset(pinB) && ab_ratio < 0.7f ){ // should be ~0.5
- // switch phase A and B
- int tmp_pinA = pinA;
- pinA = pinB;
- pinB = tmp_pinA;
- float tmp_offsetA = offset_ia;
- offset_ia = offset_ib;
- offset_ib = tmp_offsetA;
- gain_a *= _sign(c.b);
- exit_flag = 2; // signal that pins have been switched
- }else if(_isset(pinC) && ac_ratio < 0.7f ){ // should be ~0.5
- // switch phase A and C
- int tmp_pinA = pinA;
- pinA = pinC;
- pinC= tmp_pinA;
- float tmp_offsetA = offset_ia;
- offset_ia = offset_ic;
- offset_ic = tmp_offsetA;
- gain_a *= _sign(c.c);
- exit_flag = 2;// signal that pins have been switched
- }else{
- // error in current sense - phase either not measured or bad connection
- return 0;
- }
- }
-
- if(_isset(pinB)){
- // set phase B active and phases A and C down
- driver->setPwm(0, voltage, 0);
- _delay(200);
- PhaseCurrent_s c = getPhaseCurrents();
- // read the current 50 times
- for (int i = 0; i < 100; i++) {
- PhaseCurrent_s c1 = getPhaseCurrents();
- c.a = c.a*0.6f + 0.4f*c1.a;
- c.b = c.b*0.6f + 0.4f*c1.b;
- c.c = c.c*0.6f + 0.4f*c1.c;
- _delay(3);
- }
- driver->setPwm(0, 0, 0);
- float ba_ratio = c.a ? fabs(c.b / c.a) : 0;
- float bc_ratio = c.c ? fabs(c.b / c.c) : 0;
- if(_isset(pinA) && ba_ratio > 1.5f ){ // should be ~2
- gain_b *= _sign(c.b);
- }else if(_isset(pinC) && bc_ratio > 1.5f ){ // should be ~2
- gain_b *= _sign(c.b);
- }else if(_isset(pinA) && ba_ratio < 0.7f ){ // it should be ~0.5
- // switch phase A and B
- int tmp_pinB = pinB;
- pinB = pinA;
- pinA = tmp_pinB;
- float tmp_offsetB = offset_ib;
- offset_ib = offset_ia;
- offset_ia = tmp_offsetB;
- gain_b *= _sign(c.a);
- exit_flag = 2; // signal that pins have been switched
- }else if(_isset(pinC) && bc_ratio < 0.7f ){ // should be ~0.5
- // switch phase A and C
- int tmp_pinB = pinB;
- pinB = pinC;
- pinC = tmp_pinB;
- float tmp_offsetB = offset_ib;
- offset_ib = offset_ic;
- offset_ic = tmp_offsetB;
- gain_b *= _sign(c.c);
- exit_flag = 2; // signal that pins have been switched
- }else{
- // error in current sense - phase either not measured or bad connection
- return 0;
- }
- }
-
- // if phase C measured
- if(_isset(pinC)){
- // set phase C active and phases A and B down
- driver->setPwm(0, 0, voltage);
- _delay(200);
- PhaseCurrent_s c = getPhaseCurrents();
- // read the adc voltage 500 times ( arbitrary number )
- for (int i = 0; i < 100; i++) {
- PhaseCurrent_s c1 = getPhaseCurrents();
- c.a = c.a*0.6f + 0.4f*c1.a;
- c.b = c.b*0.6f + 0.4f*c1.b;
- c.c = c.c*0.6f + 0.4f*c1.c;
- _delay(3);
- }
- driver->setPwm(0, 0, 0);
- float ca_ratio = c.a ? fabs(c.c / c.a) : 0;
- float cb_ratio = c.b ? fabs(c.c / c.b) : 0;
- if(_isset(pinA) && ca_ratio > 1.5f ){ // should be ~2
- gain_c *= _sign(c.c);
- }else if(_isset(pinB) && cb_ratio > 1.5f ){ // should be ~2
- gain_c *= _sign(c.c);
- }else if(_isset(pinA) && ca_ratio < 0.7f ){ // it should be ~0.5
- // switch phase A and C
- int tmp_pinC = pinC;
- pinC = pinA;
- pinA = tmp_pinC;
- float tmp_offsetC = offset_ic;
- offset_ic = offset_ia;
- offset_ia = tmp_offsetC;
- gain_c *= _sign(c.a);
- exit_flag = 2; // signal that pins have been switched
- }else if(_isset(pinB) && cb_ratio < 0.7f ){ // should be ~0.5
- // switch phase B and C
- int tmp_pinC = pinC;
- pinC = pinB;
- pinB = tmp_pinC;
- float tmp_offsetC = offset_ic;
- offset_ic = offset_ib;
- offset_ib = tmp_offsetC;
- gain_c *= _sign(c.b);
- exit_flag = 2; // signal that pins have been switched
- }else{
- // error in current sense - phase either not measured or bad connection
- return 0;
- }
- }
-
- if(gain_a < 0 || gain_b < 0 || gain_c < 0) exit_flag +=2;
- // exit flag is either
- // 0 - fail
- // 1 - success and nothing changed
- // 2 - success but pins reconfigured
- // 3 - success but gains inverted
- // 4 - success but pins reconfigured and gains inverted
-
- return exit_flag;
-}
diff --git a/src/current_sense/LowsideCurrentSense.h b/src/current_sense/LowsideCurrentSense.h
index 1652b330..6651bcd2 100644
--- a/src/current_sense/LowsideCurrentSense.h
+++ b/src/current_sense/LowsideCurrentSense.h
@@ -7,6 +7,8 @@
#include "../common/defaults.h"
#include "../common/base_classes/CurrentSense.h"
#include "../common/base_classes/FOCMotor.h"
+#include "../common/base_classes/StepperDriver.h"
+#include "../common/base_classes/BLDCDriver.h"
#include "../common/lowpass_filter.h"
#include "hardware_api.h"
@@ -34,30 +36,9 @@ class LowsideCurrentSense: public CurrentSense{
// CurrentSense interface implementing functions
int init() override;
PhaseCurrent_s getPhaseCurrents() override;
- int driverAlign(float align_voltage) override;
- // ADC measuremnet gain for each phase
- // support for different gains for different phases of more commonly - inverted phase currents
- // this should be automated later
- float gain_a; //!< phase A gain
- float gain_b; //!< phase B gain
- float gain_c; //!< phase C gain
-
- // // per phase low pass fileters
- // LowPassFilter lpf_a{DEF_LPF_PER_PHASE_CURRENT_SENSE_Tf}; //!< current A low pass filter
- // LowPassFilter lpf_b{DEF_LPF_PER_PHASE_CURRENT_SENSE_Tf}; //!< current B low pass filter
- // LowPassFilter lpf_c{DEF_LPF_PER_PHASE_CURRENT_SENSE_Tf}; //!< current C low pass filter
-
- float offset_ia; //!< zero current A voltage value (center of the adc reading)
- float offset_ib; //!< zero current B voltage value (center of the adc reading)
- float offset_ic; //!< zero current C voltage value (center of the adc reading)
private:
- // hardware variables
- int pinA; //!< pin A analog pin for current measurement
- int pinB; //!< pin B analog pin for current measurement
- int pinC; //!< pin C analog pin for current measurement
-
// gain variables
float shunt_resistor; //!< Shunt resistor value
float amp_gain; //!< amp gain value
diff --git a/src/drivers/BLDCDriver3PWM.h b/src/drivers/BLDCDriver3PWM.h
index 1942f60b..75ee478c 100644
--- a/src/drivers/BLDCDriver3PWM.h
+++ b/src/drivers/BLDCDriver3PWM.h
@@ -38,10 +38,9 @@ class BLDCDriver3PWM: public BLDCDriver
int enableA_pin; //!< enable pin number
int enableB_pin; //!< enable pin number
int enableC_pin; //!< enable pin number
- bool enable_active_high = true;
/**
- * Set phase voltages to the harware
+ * Set phase voltages to the hardware
*
* @param Ua - phase A voltage
* @param Ub - phase B voltage
@@ -50,7 +49,8 @@ class BLDCDriver3PWM: public BLDCDriver
void setPwm(float Ua, float Ub, float Uc) override;
/**
- * Set phase voltages to the harware
+ * Set phase voltages to the hardware
+ * > Only possible is the driver has separate enable pins for all phases!
*
* @param sc - phase A state : active / disabled ( high impedance )
* @param sb - phase B state : active / disabled ( high impedance )
diff --git a/src/drivers/BLDCDriver6PWM.h b/src/drivers/BLDCDriver6PWM.h
index e8643cc5..7ba7631c 100644
--- a/src/drivers/BLDCDriver6PWM.h
+++ b/src/drivers/BLDCDriver6PWM.h
@@ -37,7 +37,6 @@ class BLDCDriver6PWM: public BLDCDriver
int pwmB_h,pwmB_l; //!< phase B pwm pin number
int pwmC_h,pwmC_l; //!< phase C pwm pin number
int enable_pin; //!< enable pin number
- bool enable_active_high = true;
float dead_zone; //!< a percentage of dead-time(zone) (both high and low side in low) for each pwm cycle [0,1]
diff --git a/src/drivers/StepperDriver2PWM.cpp b/src/drivers/StepperDriver2PWM.cpp
index dbbf5b8f..e8ccc6c6 100644
--- a/src/drivers/StepperDriver2PWM.cpp
+++ b/src/drivers/StepperDriver2PWM.cpp
@@ -44,8 +44,8 @@ StepperDriver2PWM::StepperDriver2PWM(int _pwm1, int _dir1, int _pwm2, int _dir2,
// enable motor driver
void StepperDriver2PWM::enable(){
// enable_pin the driver - if enable_pin pin available
- if ( _isset(enable_pin1) ) digitalWrite(enable_pin1, HIGH);
- if ( _isset(enable_pin2) ) digitalWrite(enable_pin2, HIGH);
+ if ( _isset(enable_pin1) ) digitalWrite(enable_pin1, enable_active_high);
+ if ( _isset(enable_pin2) ) digitalWrite(enable_pin2, enable_active_high);
// set zero to PWM
setPwm(0,0);
}
@@ -56,8 +56,8 @@ void StepperDriver2PWM::disable()
// set zero to PWM
setPwm(0, 0);
// disable the driver - if enable_pin pin available
- if ( _isset(enable_pin1) ) digitalWrite(enable_pin1, LOW);
- if ( _isset(enable_pin2) ) digitalWrite(enable_pin2, LOW);
+ if ( _isset(enable_pin1) ) digitalWrite(enable_pin1, !enable_active_high);
+ if ( _isset(enable_pin2) ) digitalWrite(enable_pin2, !enable_active_high);
}
@@ -84,6 +84,14 @@ int StepperDriver2PWM::init() {
return params!=SIMPLEFOC_DRIVER_INIT_FAILED;
}
+// Set voltage to the pwm pin
+void StepperDriver2PWM::setPhaseState(PhaseState sa, PhaseState sb) {
+ // disable if needed
+ if( _isset(enable_pin1) && _isset(enable_pin2)){
+ digitalWrite(enable_pin1, sa == PhaseState::PHASE_ON ? enable_active_high:!enable_active_high);
+ digitalWrite(enable_pin2, sb == PhaseState::PHASE_ON ? enable_active_high:!enable_active_high);
+ }
+}
// Set voltage to the pwm pin
void StepperDriver2PWM::setPwm(float Ua, float Ub) {
diff --git a/src/drivers/StepperDriver2PWM.h b/src/drivers/StepperDriver2PWM.h
index b349af06..1bd00db9 100644
--- a/src/drivers/StepperDriver2PWM.h
+++ b/src/drivers/StepperDriver2PWM.h
@@ -60,6 +60,15 @@ class StepperDriver2PWM: public StepperDriver
*/
void setPwm(float Ua, float Ub) override;
+ /**
+ * Set phase voltages to the hardware
+ * > Only possible is the driver has separate enable pins for both phases!
+ *
+ * @param sa phase A state : active / disabled ( high impedance )
+ * @param sb phase B state : active / disabled ( high impedance )
+ */
+ virtual void setPhaseState(PhaseState sa, PhaseState sb) override;
+
private:
};
diff --git a/src/drivers/StepperDriver4PWM.cpp b/src/drivers/StepperDriver4PWM.cpp
index 836f5472..52f1c1d1 100644
--- a/src/drivers/StepperDriver4PWM.cpp
+++ b/src/drivers/StepperDriver4PWM.cpp
@@ -21,8 +21,8 @@ StepperDriver4PWM::StepperDriver4PWM(int ph1A,int ph1B,int ph2A,int ph2B,int en1
// enable motor driver
void StepperDriver4PWM::enable(){
// enable_pin the driver - if enable_pin pin available
- if ( _isset(enable_pin1) ) digitalWrite(enable_pin1, HIGH);
- if ( _isset(enable_pin2) ) digitalWrite(enable_pin2, HIGH);
+ if ( _isset(enable_pin1) ) digitalWrite(enable_pin1, enable_active_high);
+ if ( _isset(enable_pin2) ) digitalWrite(enable_pin2, enable_active_high);
// set zero to PWM
setPwm(0,0);
}
@@ -33,8 +33,8 @@ void StepperDriver4PWM::disable()
// set zero to PWM
setPwm(0, 0);
// disable the driver - if enable_pin pin available
- if ( _isset(enable_pin1) ) digitalWrite(enable_pin1, LOW);
- if ( _isset(enable_pin2) ) digitalWrite(enable_pin2, LOW);
+ if ( _isset(enable_pin1) ) digitalWrite(enable_pin1, !enable_active_high);
+ if ( _isset(enable_pin2) ) digitalWrite(enable_pin2, !enable_active_high);
}
@@ -59,6 +59,15 @@ int StepperDriver4PWM::init() {
return params!=SIMPLEFOC_DRIVER_INIT_FAILED;
}
+// Set voltage to the pwm pin
+void StepperDriver4PWM::setPhaseState(PhaseState sa, PhaseState sb) {
+ // disable if needed
+ if( _isset(enable_pin1) && _isset(enable_pin2)){
+ digitalWrite(enable_pin1, sa == PhaseState::PHASE_ON ? enable_active_high:!enable_active_high);
+ digitalWrite(enable_pin2, sb == PhaseState::PHASE_ON ? enable_active_high:!enable_active_high);
+ }
+}
+
// Set voltage to the pwm pin
void StepperDriver4PWM::setPwm(float Ualpha, float Ubeta) {
diff --git a/src/drivers/StepperDriver4PWM.h b/src/drivers/StepperDriver4PWM.h
index e4b2ee42..33e7d0cf 100644
--- a/src/drivers/StepperDriver4PWM.h
+++ b/src/drivers/StepperDriver4PWM.h
@@ -47,6 +47,16 @@ class StepperDriver4PWM: public StepperDriver
*/
void setPwm(float Ua, float Ub) override;
+
+ /**
+ * Set phase voltages to the hardware.
+ * > Only possible is the driver has separate enable pins for both phases!
+ *
+ * @param sa phase A state : active / disabled ( high impedance )
+ * @param sb phase B state : active / disabled ( high impedance )
+ */
+ virtual void setPhaseState(PhaseState sa, PhaseState sb) override;
+
private:
};