diff --git a/README.md b/README.md
index 86551f79..9fb24dd1 100644
--- a/README.md
+++ b/README.md
@@ -28,18 +28,8 @@ Therefore this is an attempt to:
- For official driver boards see [SimpleFOCBoards](https://docs.simplefoc.com/boards)
- Many many more boards developed by the community members, see [SimpleFOCCommunity](https://community.simplefoc.com/)
-> NEW RELEASE 📢 : SimpleFOClibrary v2.3.3
-> - Teensy4
-> - support for low-side current sensing [#392](https://github.com/simplefoc/Arduino-FOC/pull/392)
-> - support for center aligned 6pwm and 3pwm (optional) [#392](https://github.com/simplefoc/Arduino-FOC/pull/392)
-> - stm32
-> - support for center aligned pwm (even across multiple timers and motors/drivers) [#374](https://github.com/simplefoc/Arduino-FOC/pull/374), [#388](https://github.com/simplefoc/Arduino-FOC/pull/388)
-> - support for DMA based low-side current sensing: [#383](https://github.com/simplefoc/Arduino-FOC/pull/383),[#378](https://github.com/simplefoc/Arduino-FOC/pull/378)
-> - support for f7 architecture [#388](https://github.com/simplefoc/Arduino-FOC/pull/388),[#394](https://github.com/simplefoc/Arduino-FOC/pull/394)
-> - KV rating calculation fix [#347](https://github.com/simplefoc/Arduino-FOC/pull/347)
-> - Much more performant Space Vector PWM calculation [#340](https://github.com/simplefoc/Arduino-FOC/pull/340)
-> - And much more:
-> - See the complete list of bugfixes and new features of v2.3.3 [fixes and PRs](https://github.com/simplefoc/Arduino-FOC/milestone/10?closed=1)
+> NEXT RELEASE 📢 : SimpleFOClibrary v2.3.4
+> - Current sensing support for Stepper motors (lowside and inline)
## Arduino *SimpleFOClibrary* v2.3.3
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.old b/src/BLDCMotor.cpp.old
new file mode 100644
index 00000000..6b996342
--- /dev/null
+++ b/src/BLDCMotor.cpp.old
@@ -0,0 +1,732 @@
+#include "BLDCMotor.h"
+#include "./communication/SimpleFOCDebug.h"
+
+// BLDCMotor( int pp , float R)
+// - pp - pole pair number
+// - R - motor phase resistance
+// - KV - motor kv rating
+BLDCMotor::BLDCMotor(int pp, float _R, float _KV)
+: FOCMotor()
+{
+ // save pole pairs number
+ pole_pairs = pp;
+ // save phase resistance number
+ phase_resistance = _R;
+ // save back emf constant KV = 1/KV
+ K_bemf = _isset(_KV) ? 1.0/_KV : NOT_SET;
+
+ // torque control type is voltage by default
+ torque_controller = TorqueControlType::voltage;
+}
+
+
+/**
+ Link the driver which controls the motor
+*/
+void BLDCMotor::linkDriver(BLDCDriver* _driver) {
+ driver = _driver;
+}
+
+// init hardware pins
+void BLDCMotor::init() {
+ if (!driver || !driver->initialized) {
+ motor_status = FOCMotorStatus::motor_init_failed;
+ SIMPLEFOC_DEBUG("MOT: Init not possible, driver not initialized");
+ return;
+ }
+ motor_status = FOCMotorStatus::motor_initializing;
+ SIMPLEFOC_DEBUG("MOT: Init");
+
+ // sanity check for the voltage limit configuration
+ if(voltage_limit > driver->voltage_limit) voltage_limit = driver->voltage_limit;
+ // constrain voltage for sensor alignment
+ if(voltage_sensor_align > voltage_limit) voltage_sensor_align = voltage_limit;
+
+ // update the controller limits
+ if(current_sense){
+ // current control loop controls voltage
+ PID_current_q.limit = voltage_limit;
+ PID_current_d.limit = voltage_limit;
+ }
+ if(_isset(phase_resistance) || torque_controller != TorqueControlType::voltage){
+ // velocity control loop controls current
+ PID_velocity.limit = current_limit;
+ }else{
+ // velocity control loop controls the voltage
+ PID_velocity.limit = voltage_limit;
+ }
+ P_angle.limit = velocity_limit;
+
+ _delay(500);
+ // enable motor
+ SIMPLEFOC_DEBUG("MOT: Enable driver.");
+ enable();
+ _delay(500);
+ motor_status = FOCMotorStatus::motor_uncalibrated;
+}
+
+
+// disable motor driver
+void BLDCMotor::disable()
+{
+ // set zero to PWM
+ driver->setPwm(0, 0, 0);
+ // disable the driver
+ driver->disable();
+ // motor status update
+ enabled = 0;
+}
+// enable motor driver
+void BLDCMotor::enable()
+{
+ // enable the driver
+ driver->enable();
+ // set zero to PWM
+ driver->setPwm(0, 0, 0);
+ // motor status update
+ enabled = 1;
+}
+
+/**
+ FOC functions
+*/
+// FOC initialization function
+int BLDCMotor::initFOC( float zero_electric_offset, Direction _sensor_direction) {
+ int exit_flag = 1;
+
+ motor_status = FOCMotorStatus::motor_calibrating;
+
+ // align motor if necessary
+ // alignment necessary for encoders!
+ if(_isset(zero_electric_offset)){
+ // abosolute zero offset provided - no need to align
+ zero_electric_angle = zero_electric_offset;
+ // set the sensor direction - default CW
+ sensor_direction = _sensor_direction;
+ }
+
+ // sensor and motor alignment - can be skipped
+ // by setting motor.sensor_direction and motor.zero_electric_angle
+ _delay(500);
+ if(sensor){
+ exit_flag *= alignSensor();
+ // added the shaft_angle update
+ sensor->update();
+ shaft_angle = shaftAngle();
+ }else
+ SIMPLEFOC_DEBUG("MOT: No sensor.");
+
+ // aligning the current sensor - can be skipped
+ // checks if driver phases are the same as current sense phases
+ // and checks the direction of measuremnt.
+ _delay(500);
+ if(exit_flag){
+ if(current_sense) exit_flag *= alignCurrentSense();
+ else SIMPLEFOC_DEBUG("MOT: No current sense.");
+ }
+
+ if(exit_flag){
+ SIMPLEFOC_DEBUG("MOT: Ready.");
+ motor_status = FOCMotorStatus::motor_ready;
+ }else{
+ SIMPLEFOC_DEBUG("MOT: Init FOC failed.");
+ motor_status = FOCMotorStatus::motor_calib_failed;
+ disable();
+ }
+
+ return exit_flag;
+}
+
+// Calibarthe the motor and current sense phases
+int BLDCMotor::alignCurrentSense() {
+ int exit_flag = 1; // success
+
+ SIMPLEFOC_DEBUG("MOT: Align current sense.");
+
+ // align current sense and the driver
+ exit_flag = current_sense->driverAlign(voltage_sensor_align);
+ if(!exit_flag){
+ // error in current sense - phase either not measured or bad connection
+ SIMPLEFOC_DEBUG("MOT: Align error!");
+ exit_flag = 0;
+ }else{
+ // output the alignment status flag
+ SIMPLEFOC_DEBUG("MOT: Success: ", exit_flag);
+ }
+
+ return exit_flag > 0;
+}
+
+// Encoder alignment to electrical 0 angle
+int BLDCMotor::alignSensor() {
+ int exit_flag = 1; //success
+ SIMPLEFOC_DEBUG("MOT: Align sensor.");
+
+ // check if sensor needs zero search
+ if(sensor->needsSearch()) exit_flag = absoluteZeroSearch();
+ // stop init if not found index
+ if(!exit_flag) return exit_flag;
+
+ // if unknown natural direction
+ if(!_isset(sensor_direction)){
+
+ // find natural direction
+ // move one electrical revolution forward
+ for (int i = 0; i <=500; i++ ) {
+ float angle = _3PI_2 + _2PI * i / 500.0f;
+ setPhaseVoltage(voltage_sensor_align, 0, angle);
+ _delay(2);
+ }
+ // take and angle in the middle
+ sensor->update();
+ float mid_angle = sensor->getAngle();
+ // move one electrical revolution backwards
+ for (int i = 500; i >=0; i-- ) {
+ float angle = _3PI_2 + _2PI * i / 500.0f ;
+ setPhaseVoltage(voltage_sensor_align, 0, angle);
+ _delay(2);
+ }
+ sensor->update();
+ float end_angle = sensor->getAngle();
+ setPhaseVoltage(0, 0, 0);
+ _delay(200);
+ // determine the direction the sensor moved
+ if (mid_angle == end_angle) {
+ SIMPLEFOC_DEBUG("MOT: Failed to notice movement");
+ return 0; // failed calibration
+ } else if (mid_angle < end_angle) {
+ SIMPLEFOC_DEBUG("MOT: sensor_direction==CCW");
+ sensor_direction = Direction::CCW;
+ } else{
+ SIMPLEFOC_DEBUG("MOT: sensor_direction==CW");
+ sensor_direction = Direction::CW;
+ }
+ // check pole pair number
+ float moved = fabs(mid_angle - end_angle);
+ if( fabs(moved*pole_pairs - _2PI) > 0.5f ) { // 0.5f is arbitrary number it can be lower or higher!
+ SIMPLEFOC_DEBUG("MOT: PP check: fail - estimated pp: ", _2PI/moved);
+ } else
+ SIMPLEFOC_DEBUG("MOT: PP check: OK!");
+
+ } else SIMPLEFOC_DEBUG("MOT: Skip dir calib.");
+
+ // zero electric angle not known
+ if(!_isset(zero_electric_angle)){
+
+ // // align the electrical phases of the motor and sensor
+ // // set angle -90(270 = 3PI/2) degrees
+ // setPhaseVoltage(voltage_sensor_align, 0, _3PI_2);
+ // _delay(700);
+ // // read the sensor
+ // sensor->update();
+ // // get the current zero electric angle
+ // zero_electric_angle = 0;
+ // zero_electric_angle = electricalAngle();
+ // _delay(20);
+ // if(monitor_port){
+ // SIMPLEFOC_DEBUG("MOT: Zero elec. angle: ", zero_electric_angle);
+ // }
+ // // stop everything
+ // setPhaseVoltage(0, 0, 0);
+ // _delay(200);
+
+
+ // align the electrical phases of the motor and sensor
+ // set angle -90(270 = 3PI/2) degrees
+ setPhaseVoltage(voltage_sensor_align, 0, _3PI_2);
+ _delay(700);
+ // read the sensor
+ sensor->update();
+
+ float d_angle = pole_pairs*_2PI/((float) calib_points);
+ for(int i=0; i< calib_points; i++){
+ setPhaseVoltage(voltage_sensor_align, 0, _3PI_2+d_angle*i);
+ _delay(50);
+ // read the sensor
+ sensor->update();
+ // get the current zero electric angle
+ float sensor_angle = (float)(sensor_direction * pole_pairs) * sensor->getMechanicalAngle();
+ zero_electric_angle_array[i] = _normalizeAngle( sensor_angle - d_angle*i);
+ // current_angle_offset_array[i] = 0;
+ // for(int n=0; n<20;n++){
+ // DQCurrent_s c = current_sense->getFOCCurrents(0); // get alpha(d) and beta(q) currents
+ // current_angle_offset_array[i] += sensor_angle - atan2(c.q, c.d);
+ // _delay(1);
+ // }
+ // current_angle_offset_array[i] = _normalizeAngle(current_angle_offset_array[i]/20.0f );
+ // if(i>=1){
+ // float d_an = current_angle_offset_array[i] - current_angle_offset_array[i-1];
+ // if( fabs(d_an) > 0.05){
+ // current_angle_offset_array[i] = _normalizeAngle(current_angle_offset_array[i-1]+_sign(d_an)*0.05);
+ // }
+ // }
+ if(monitor_port){
+ SIMPLEFOC_DEBUG("MOT: Zero elec. angle: ", zero_electric_angle_array[i]);
+ // SimpleFOCDebug::print(_normalizeAngle(zero_electric_angle_array[i]+_PI_2)-_PI_2);
+ // SimpleFOCDebug::print("\t");
+ // SimpleFOCDebug::println(_normalizeAngle(current_angle_offset_array[i]+_PI_2)-_PI_2);
+ }
+ }
+ // stop everything
+ setPhaseVoltage(0, 0, 0);
+ // _delay(200);
+
+ }else SIMPLEFOC_DEBUG("MOT: Skip offset calib.");
+ return exit_flag;
+}
+
+float BLDCMotor::electricalAngle(){
+ // if no sensor linked return previous value ( for open loop )
+ if(!sensor) return electrical_angle;
+ float angle = sensor->getMechanicalAngle();
+ zero_electric_angle = zero_electric_angle_array[_round(angle/_2PI*calib_points) % calib_points ];
+ return _normalizeAngle( (float)(sensor_direction * pole_pairs) * angle - zero_electric_angle );
+}
+
+// Encoder alignment the absolute zero angle
+// - to the index
+int BLDCMotor::absoluteZeroSearch() {
+ // sensor precision: this is all ok, as the search happens near the 0-angle, where the precision
+ // of float is sufficient.
+ SIMPLEFOC_DEBUG("MOT: Index search...");
+ // search the absolute zero with small velocity
+ float limit_vel = velocity_limit;
+ float limit_volt = voltage_limit;
+ velocity_limit = velocity_index_search;
+ voltage_limit = voltage_sensor_align;
+ shaft_angle = 0;
+ while(sensor->needsSearch() && shaft_angle < _2PI){
+ angleOpenloop(1.5f*_2PI);
+ // call important for some sensors not to loose count
+ // not needed for the search
+ sensor->update();
+ }
+ // disable motor
+ setPhaseVoltage(0, 0, 0);
+ // reinit the limits
+ velocity_limit = limit_vel;
+ voltage_limit = limit_volt;
+ // check if the zero found
+ if(monitor_port){
+ if(sensor->needsSearch()) SIMPLEFOC_DEBUG("MOT: Error: Not found!");
+ else SIMPLEFOC_DEBUG("MOT: Success!");
+ }
+ return !sensor->needsSearch();
+}
+
+// Iterative function looping FOC algorithm, setting Uq on the Motor
+// The faster it can be run the better
+void BLDCMotor::loopFOC() {
+ // update sensor - do this even in open-loop mode, as user may be switching between modes and we could lose track
+ // of full rotations otherwise.
+ if (sensor) sensor->update();
+
+ // if open-loop do nothing
+ if( controller==MotionControlType::angle_openloop || controller==MotionControlType::velocity_openloop ) return;
+
+ // if disabled do nothing
+ if(!enabled) return;
+
+ // Needs the update() to be called first
+ // This function will not have numerical issues because it uses Sensor::getMechanicalAngle()
+ // which is in range 0-2PI
+ electrical_angle = electricalAngle();
+ float current_electrical_angle = electrical_angle;
+ // float angle = sensor->getMechanicalAngle();
+ // float current_angle_offset = current_angle_offset_array[_round(angle/_2PI*calib_points) % calib_points ];
+ // float current_electrical_angle = _normalizeAngle( (float)(sensor_direction * pole_pairs) * angle - current_angle_offset );
+ switch (torque_controller) {
+
+ case TorqueControlType::voltage:
+ // no need to do anything really
+ break;
+ case TorqueControlType::dc_current:
+ if(!current_sense) return;
+ // read overall current magnitude
+ current.q = current_sense->getDCCurrent(current_electrical_angle);
+ // filter the value values
+ current.q = LPF_current_q(current.q);
+ // calculate the phase voltage
+ voltage.q = PID_current_q(current_sp - current.q);
+ voltage.d = 0;
+ break;
+ case TorqueControlType::foc_current:
+ if(!current_sense) return;
+ // read dq currents
+ current = current_sense->getFOCCurrents(current_electrical_angle);
+ // filter values
+ current.q = LPF_current_q(current.q);
+ current.d = LPF_current_d(current.d);
+ // calculate the phase voltages
+ voltage.q = PID_current_q(current_sp - current.q);
+ voltage.d = PID_current_d(-current.d);
+ break;
+ default:
+ // no torque control selected
+ SIMPLEFOC_DEBUG("MOT: no torque control selected!");
+ break;
+ }
+
+ // set the phase voltage - FOC heart function :)
+ setPhaseVoltage(voltage.q, voltage.d, electrical_angle);
+}
+
+// Iterative function running outer loop of the FOC algorithm
+// Behavior of this function is determined by the motor.controller variable
+// It runs either angle, velocity or torque loop
+// - needs to be called iteratively it is asynchronous function
+// - if target is not set it uses motor.target value
+void BLDCMotor::move(float new_target) {
+
+ // downsampling (optional)
+ if(motion_cnt++ < motion_downsample) return;
+ motion_cnt = 0;
+
+ // shaft angle/velocity need the update() to be called first
+ // get shaft angle
+ // TODO sensor precision: the shaft_angle actually stores the complete position, including full rotations, as a float
+ // For this reason it is NOT precise when the angles become large.
+ // Additionally, the way LPF works on angle is a precision issue, and the angle-LPF is a problem
+ // when switching to a 2-component representation.
+ if( controller!=MotionControlType::angle_openloop && controller!=MotionControlType::velocity_openloop )
+ shaft_angle = shaftAngle(); // read value even if motor is disabled to keep the monitoring updated but not in openloop mode
+ // get angular velocity
+ shaft_velocity = shaftVelocity(); // read value even if motor is disabled to keep the monitoring updated
+
+ // if disabled do nothing
+ if(!enabled) return;
+ // set internal target variable
+ if(_isset(new_target)) target = new_target;
+
+ // calculate the back-emf voltage if K_bemf available
+ if (_isset(K_bemf)) voltage_bemf = K_bemf*shaft_velocity;
+ // estimate the motor current if phase reistance available and current_sense not available
+ if(!current_sense && _isset(phase_resistance)) current.q = (voltage.q - voltage_bemf)/phase_resistance;
+
+ // upgrade the current based voltage limit
+ switch (controller) {
+ case MotionControlType::torque:
+ if(torque_controller == TorqueControlType::voltage){ // if voltage torque control
+ if(!_isset(phase_resistance)) voltage.q = target;
+ else voltage.q = target*phase_resistance + voltage_bemf;
+ voltage.q = _constrain(voltage.q, -voltage_limit, voltage_limit);
+ voltage.d = 0;
+ }else{
+ current_sp = target; // if current/foc_current torque control
+ }
+ break;
+ case MotionControlType::angle:
+ // TODO sensor precision: this calculation is not numerically precise. The target value cannot express precise positions when
+ // the angles are large. This results in not being able to command small changes at high position values.
+ // to solve this, the delta-angle has to be calculated in a numerically precise way.
+ // angle set point
+ shaft_angle_sp = target;
+ // calculate velocity set point
+ shaft_velocity_sp = P_angle( shaft_angle_sp - shaft_angle );
+ // calculate the torque command - sensor precision: this calculation is ok, but based on bad value from previous calculation
+ current_sp = PID_velocity(shaft_velocity_sp - shaft_velocity); // if voltage torque control
+ // if torque controlled through voltage
+ if(torque_controller == TorqueControlType::voltage){
+ // use voltage if phase-resistance not provided
+ if(!_isset(phase_resistance)) voltage.q = current_sp;
+ else voltage.q = _constrain( current_sp*phase_resistance + voltage_bemf , -voltage_limit, voltage_limit);
+ voltage.d = 0;
+ }
+ break;
+ case MotionControlType::velocity:
+ // velocity set point - sensor precision: this calculation is numerically precise.
+ shaft_velocity_sp = target;
+ // calculate the torque command
+ current_sp = PID_velocity(shaft_velocity_sp - shaft_velocity); // if current/foc_current torque control
+ // if torque controlled through voltage control
+ if(torque_controller == TorqueControlType::voltage){
+ // use voltage if phase-resistance not provided
+ if(!_isset(phase_resistance)) voltage.q = current_sp;
+ else voltage.q = _constrain( current_sp*phase_resistance + voltage_bemf , -voltage_limit, voltage_limit);
+ voltage.d = 0;
+ }
+ break;
+ case MotionControlType::velocity_openloop:
+ // velocity control in open loop - sensor precision: this calculation is numerically precise.
+ shaft_velocity_sp = target;
+ voltage.q = velocityOpenloop(shaft_velocity_sp); // returns the voltage that is set to the motor
+ voltage.d = 0;
+ break;
+ case MotionControlType::angle_openloop:
+ // angle control in open loop -
+ // TODO sensor precision: this calculation NOT numerically precise, and subject
+ // to the same problems in small set-point changes at high angles
+ // as the closed loop version.
+ shaft_angle_sp = target;
+ voltage.q = angleOpenloop(shaft_angle_sp); // returns the voltage that is set to the motor
+ voltage.d = 0;
+ break;
+ }
+}
+
+
+// Method using FOC to set Uq and Ud to the motor at the optimal angle
+// Function implementing Space Vector PWM and Sine PWM algorithms
+//
+// Function using sine approximation
+// regular sin + cos ~300us (no memory usaage)
+// approx _sin + _cos ~110us (400Byte ~ 20% of memory)
+void BLDCMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) {
+
+ float center;
+ int sector;
+ float _ca,_sa;
+
+ switch (foc_modulation)
+ {
+ case FOCModulationType::Trapezoid_120 :
+ // see https://www.youtube.com/watch?v=InzXA7mWBWE Slide 5
+ static int trap_120_map[6][3] = {
+ {_HIGH_IMPEDANCE,1,-1},{-1,1,_HIGH_IMPEDANCE},{-1,_HIGH_IMPEDANCE,1},{_HIGH_IMPEDANCE,-1,1},{1,-1,_HIGH_IMPEDANCE},{1,_HIGH_IMPEDANCE,-1} // each is 60 degrees with values for 3 phases of 1=positive -1=negative 0=high-z
+ };
+ // static int trap_120_state = 0;
+ sector = 6 * (_normalizeAngle(angle_el + _PI_6 ) / _2PI); // adding PI/6 to align with other modes
+ // centering the voltages around either
+ // modulation_centered == true > driver.volage_limit/2
+ // modulation_centered == false > or Adaptable centering, all phases drawn to 0 when Uq=0
+ center = modulation_centered ? (driver->voltage_limit)/2 : Uq;
+
+ if(trap_120_map[sector][0] == _HIGH_IMPEDANCE){
+ Ua= center;
+ Ub = trap_120_map[sector][1] * Uq + center;
+ Uc = trap_120_map[sector][2] * Uq + center;
+ driver->setPhaseState(_HIGH_IMPEDANCE, _ACTIVE, _ACTIVE); // disable phase if possible
+ }else if(trap_120_map[sector][1] == _HIGH_IMPEDANCE){
+ Ua = trap_120_map[sector][0] * Uq + center;
+ Ub = center;
+ Uc = trap_120_map[sector][2] * Uq + center;
+ driver->setPhaseState(_ACTIVE, _HIGH_IMPEDANCE, _ACTIVE);// disable phase if possible
+ }else{
+ Ua = trap_120_map[sector][0] * Uq + center;
+ Ub = trap_120_map[sector][1] * Uq + center;
+ Uc = center;
+ driver->setPhaseState(_ACTIVE,_ACTIVE, _HIGH_IMPEDANCE);// disable phase if possible
+ }
+
+ break;
+
+ case FOCModulationType::Trapezoid_150 :
+ // see https://www.youtube.com/watch?v=InzXA7mWBWE Slide 8
+ static int trap_150_map[12][3] = {
+ {_HIGH_IMPEDANCE,1,-1},{-1,1,-1},{-1,1,_HIGH_IMPEDANCE},{-1,1,1},{-1,_HIGH_IMPEDANCE,1},{-1,-1,1},{_HIGH_IMPEDANCE,-1,1},{1,-1,1},{1,-1,_HIGH_IMPEDANCE},{1,-1,-1},{1,_HIGH_IMPEDANCE,-1},{1,1,-1} // each is 30 degrees with values for 3 phases of 1=positive -1=negative 0=high-z
+ };
+ // static int trap_150_state = 0;
+ sector = 12 * (_normalizeAngle(angle_el + _PI_6 ) / _2PI); // adding PI/6 to align with other modes
+ // centering the voltages around either
+ // modulation_centered == true > driver.volage_limit/2
+ // modulation_centered == false > or Adaptable centering, all phases drawn to 0 when Uq=0
+ center = modulation_centered ? (driver->voltage_limit)/2 : Uq;
+
+ if(trap_150_map[sector][0] == _HIGH_IMPEDANCE){
+ Ua= center;
+ Ub = trap_150_map[sector][1] * Uq + center;
+ Uc = trap_150_map[sector][2] * Uq + center;
+ driver->setPhaseState(_HIGH_IMPEDANCE, _ACTIVE, _ACTIVE); // disable phase if possible
+ }else if(trap_150_map[sector][1] == _HIGH_IMPEDANCE){
+ Ua = trap_150_map[sector][0] * Uq + center;
+ Ub = center;
+ Uc = trap_150_map[sector][2] * Uq + center;
+ driver->setPhaseState(_ACTIVE, _HIGH_IMPEDANCE, _ACTIVE);// disable phase if possible
+ }else{
+ Ua = trap_150_map[sector][0] * Uq + center;
+ Ub = trap_150_map[sector][1] * Uq + center;
+ Uc = center;
+ driver->setPhaseState(_ACTIVE, _ACTIVE, _HIGH_IMPEDANCE);// disable phase if possible
+ }
+
+ break;
+
+ case FOCModulationType::SinePWM :
+ // Sinusoidal PWM modulation
+ // Inverse Park + Clarke transformation
+
+ // angle normalization in between 0 and 2pi
+ // only necessary if using _sin and _cos - approximation functions
+ angle_el = _normalizeAngle(angle_el);
+ _ca = _cos(angle_el);
+ _sa = _sin(angle_el);
+ // Inverse park transform
+ Ualpha = _ca * Ud - _sa * Uq; // -sin(angle) * Uq;
+ Ubeta = _sa * Ud + _ca * Uq; // cos(angle) * Uq;
+
+ // center = modulation_centered ? (driver->voltage_limit)/2 : Uq;
+ center = driver->voltage_limit/2;
+ // Clarke transform
+ Ua = Ualpha + center;
+ Ub = -0.5f * Ualpha + _SQRT3_2 * Ubeta + center;
+ Uc = -0.5f * Ualpha - _SQRT3_2 * Ubeta + center;
+
+ if (!modulation_centered) {
+ float Umin = min(Ua, min(Ub, Uc));
+ Ua -= Umin;
+ Ub -= Umin;
+ Uc -= Umin;
+ }
+
+ break;
+
+ case FOCModulationType::SpaceVectorPWM :
+ // Nice video explaining the SpaceVectorModulation (SVPWM) algorithm
+ // https://www.youtube.com/watch?v=QMSWUMEAejg
+
+ // the algorithm goes
+ // 1) Ualpha, Ubeta
+ // 2) Uout = sqrt(Ualpha^2 + Ubeta^2)
+ // 3) angle_el = atan2(Ubeta, Ualpha)
+ //
+ // equivalent to 2) because the magnitude does not change is:
+ // Uout = sqrt(Ud^2 + Uq^2)
+ // equivalent to 3) is
+ // angle_el = angle_el + atan2(Uq,Ud)
+
+ float Uout;
+ // a bit of optitmisation
+ if(Ud){ // only if Ud and Uq set
+ // _sqrt is an approx of sqrt (3-4% error)
+ Uout = _sqrt(Ud*Ud + Uq*Uq) / driver->voltage_limit;
+ // angle normalisation in between 0 and 2pi
+ // only necessary if using _sin and _cos - approximation functions
+ angle_el = _normalizeAngle(angle_el + atan2(Uq, Ud));
+ }else{// only Uq available - no need for atan2 and sqrt
+ Uout = Uq / driver->voltage_limit;
+ // angle normalisation in between 0 and 2pi
+ // only necessary if using _sin and _cos - approximation functions
+ angle_el = _normalizeAngle(angle_el + _PI_2);
+ }
+ // find the sector we are in currently
+ sector = floor(angle_el / _PI_3) + 1;
+ // calculate the duty cycles
+ float T1 = _SQRT3*_sin(sector*_PI_3 - angle_el) * Uout;
+ float T2 = _SQRT3*_sin(angle_el - (sector-1.0f)*_PI_3) * Uout;
+ // two versions possible
+ float T0 = 0; // pulled to 0 - better for low power supply voltage
+ if (modulation_centered) {
+ T0 = 1 - T1 - T2; // modulation_centered around driver->voltage_limit/2
+ }
+
+ // calculate the duty cycles(times)
+ float Ta,Tb,Tc;
+ switch(sector){
+ case 1:
+ Ta = T1 + T2 + T0/2;
+ Tb = T2 + T0/2;
+ Tc = T0/2;
+ break;
+ case 2:
+ Ta = T1 + T0/2;
+ Tb = T1 + T2 + T0/2;
+ Tc = T0/2;
+ break;
+ case 3:
+ Ta = T0/2;
+ Tb = T1 + T2 + T0/2;
+ Tc = T2 + T0/2;
+ break;
+ case 4:
+ Ta = T0/2;
+ Tb = T1+ T0/2;
+ Tc = T1 + T2 + T0/2;
+ break;
+ case 5:
+ Ta = T2 + T0/2;
+ Tb = T0/2;
+ Tc = T1 + T2 + T0/2;
+ break;
+ case 6:
+ Ta = T1 + T2 + T0/2;
+ Tb = T0/2;
+ Tc = T1 + T0/2;
+ break;
+ default:
+ // possible error state
+ Ta = 0;
+ Tb = 0;
+ Tc = 0;
+ }
+
+ // calculate the phase voltages and center
+ Ua = Ta*driver->voltage_limit;
+ Ub = Tb*driver->voltage_limit;
+ Uc = Tc*driver->voltage_limit;
+ break;
+
+ }
+
+ // set the voltages in driver
+ driver->setPwm(Ua, Ub, Uc);
+}
+
+
+
+// Function (iterative) generating open loop movement for target velocity
+// - target_velocity - rad/s
+// it uses voltage_limit variable
+float BLDCMotor::velocityOpenloop(float target_velocity){
+ // get current timestamp
+ unsigned long now_us = _micros();
+ // calculate the sample time from last call
+ float Ts = (now_us - open_loop_timestamp) * 1e-6f;
+ // quick fix for strange cases (micros overflow + timestamp not defined)
+ if(Ts <= 0 || Ts > 0.5f) Ts = 1e-3f;
+
+ // calculate the necessary angle to achieve target velocity
+ shaft_angle = _normalizeAngle(shaft_angle + target_velocity*Ts);
+ // for display purposes
+ shaft_velocity = target_velocity;
+
+ // use voltage limit or current limit
+ float Uq = voltage_limit;
+ if(_isset(phase_resistance))
+ Uq = _constrain(current_limit*phase_resistance + voltage_bemf,-voltage_limit, voltage_limit);
+
+ // set the maximal allowed voltage (voltage_limit) with the necessary angle
+ setPhaseVoltage(Uq, 0, _electricalAngle(shaft_angle, pole_pairs));
+
+ // save timestamp for next call
+ open_loop_timestamp = now_us;
+
+ return Uq;
+}
+
+// Function (iterative) generating open loop movement towards the target angle
+// - target_angle - rad
+// it uses voltage_limit and velocity_limit variables
+float BLDCMotor::angleOpenloop(float target_angle){
+ // get current timestamp
+ unsigned long now_us = _micros();
+ // calculate the sample time from last call
+ float Ts = (now_us - open_loop_timestamp) * 1e-6f;
+ // quick fix for strange cases (micros overflow + timestamp not defined)
+ if(Ts <= 0 || Ts > 0.5f) Ts = 1e-3f;
+
+ // calculate the necessary angle to move from current position towards target angle
+ // with maximal velocity (velocity_limit)
+ // TODO sensor precision: this calculation is not numerically precise. The angle can grow to the point
+ // where small position changes are no longer captured by the precision of floats
+ // when the total position is large.
+ if(abs( target_angle - shaft_angle ) > abs(velocity_limit*Ts)){
+ shaft_angle += _sign(target_angle - shaft_angle) * abs( velocity_limit )*Ts;
+ shaft_velocity = velocity_limit;
+ }else{
+ shaft_angle = target_angle;
+ shaft_velocity = 0;
+ }
+
+ // use voltage limit or current limit
+ float Uq = voltage_limit;
+ if(_isset(phase_resistance))
+ Uq = _constrain(current_limit*phase_resistance + voltage_bemf,-voltage_limit, voltage_limit);
+ // set the maximal allowed voltage (voltage_limit) with the necessary angle
+ // sensor precision: this calculation is OK due to the normalisation
+ setPhaseVoltage(Uq, 0, _electricalAngle(_normalizeAngle(shaft_angle), pole_pairs));
+
+ // save timestamp for next call
+ open_loop_timestamp = now_us;
+
+ return Uq;
+}
diff --git a/src/BLDCMotor.h 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/BLDCMotor.h.old b/src/BLDCMotor.h.old
new file mode 100644
index 00000000..04c54b92
--- /dev/null
+++ b/src/BLDCMotor.h.old
@@ -0,0 +1,121 @@
+#ifndef BLDCMotor_h
+#define BLDCMotor_h
+
+#include "Arduino.h"
+#include "common/base_classes/FOCMotor.h"
+#include "common/base_classes/Sensor.h"
+#include "common/base_classes/BLDCDriver.h"
+#include "common/foc_utils.h"
+#include "common/time_utils.h"
+#include "common/defaults.h"
+
+/**
+ BLDC motor class
+*/
+class BLDCMotor: public FOCMotor
+{
+ public:
+ /**
+ BLDCMotor class constructor
+ @param pp pole pairs number
+ @param R motor phase resistance
+ @param KV motor KV rating (1/K_bemf)
+ */
+ BLDCMotor(int pp, float R = NOT_SET, float KV = NOT_SET);
+
+ /**
+ * Function linking a motor and a foc driver
+ *
+ * @param driver BLDCDriver class implementing all the hardware specific functions necessary PWM setting
+ */
+ void linkDriver(BLDCDriver* driver);
+
+ /**
+ * BLDCDriver link:
+ * - 3PWM
+ * - 6PWM
+ */
+ BLDCDriver* driver;
+
+ /** Motor hardware init function */
+ void init() override;
+ /** Motor disable function */
+ void disable() override;
+ /** Motor enable function */
+ void enable() override;
+
+ /**
+ * Function initializing FOC algorithm
+ * and aligning sensor's and motors' zero position
+ */
+ int initFOC( float zero_electric_offset = NOT_SET , Direction sensor_direction = Direction::CW) override;
+ /**
+ * Function running FOC algorithm in real-time
+ * it calculates the gets motor angle and sets the appropriate voltages
+ * to the phase pwm signals
+ * - the faster you can run it the better Arduino UNO ~1ms, Bluepill ~ 100us
+ */
+ void loopFOC() override;
+
+ /**
+ * Function executing the control loops set by the controller parameter of the BLDCMotor.
+ *
+ * @param target Either voltage, angle or velocity based on the motor.controller
+ * If it is not set the motor will use the target set in its variable motor.target
+ *
+ * This function doesn't need to be run upon each loop execution - depends of the use case
+ */
+ void move(float target = NOT_SET) override;
+
+ float Ua, Ub, Uc;//!< Current phase voltages Ua,Ub and Uc set to motor
+ float Ualpha, Ubeta; //!< Phase voltages U alpha and U beta used for inverse Park and Clarke transform
+
+
+
+ float electricalAngle();
+ int calib_points = 1000;
+ float zero_electric_angle_array[1000];
+ float current_angle_offset_array[1000];
+
+ private:
+ // FOC methods
+ /**
+ * Method using FOC to set Uq to the motor at the optimal angle
+ * Heart of the FOC algorithm
+ *
+ * @param Uq Current voltage in q axis to set to the motor
+ * @param Ud Current voltage in d axis to set to the motor
+ * @param angle_el current electrical angle of the motor
+ */
+ void setPhaseVoltage(float Uq, float Ud, float angle_el);
+ /** Sensor alignment to electrical 0 angle of the motor */
+ int alignSensor();
+ /** Current sense and motor phase alignment */
+ int alignCurrentSense();
+ /** Motor and sensor alignment to the sensors absolute 0 angle */
+ int absoluteZeroSearch();
+
+
+ // Open loop motion control
+ /**
+ * Function (iterative) generating open loop movement for target velocity
+ * it uses voltage_limit variable
+ *
+ * @param target_velocity - rad/s
+ */
+ float velocityOpenloop(float target_velocity);
+ /**
+ * Function (iterative) generating open loop movement towards the target angle
+ * it uses voltage_limit and velocity_limit variables
+ *
+ * @param target_angle - rad
+ */
+ float angleOpenloop(float target_angle);
+ // open loop variables
+ long open_loop_timestamp;
+
+
+};
+
+
+#endif
diff --git a/src/SimpleFOC_empty.h b/src/SimpleFOC_empty.h
new file mode 100644
index 00000000..bf0049dd
--- /dev/null
+++ b/src/SimpleFOC_empty.h
@@ -0,0 +1,119 @@
+/*!
+ * @file SimpleFOC.h
+ *
+ * @mainpage Simple Field Oriented Control BLDC motor control library
+ *
+ * @section intro_sec Introduction
+ *
+ * Proper low-cost and low-power FOC supporting boards are very hard to find these days and even may not exist.
Even harder to find is a stable and simple FOC algorithm code capable of running on Arduino devices. Therefore this is an attempt to:
+ * - Demystify FOC algorithm and make a robust but simple Arduino library: Arduino SimpleFOC library
+ * - Develop a modular BLDC driver board: Arduino SimpleFOC shield.
+ *
+ * @section features Features
+ * - Arduino compatible: Arduino library code
+ * - Easy to setup and configure:
+ * - Easy hardware configuration
+ * - Easy tuning the control loops
+ * - Modular:
+ * - Supports as many sensors , BLDC motors and driver boards as possible
+ * - Supports as many application requirements as possible
+ * - Plug & play: Arduino SimpleFOC shield
+ *
+ * @section dependencies Supported Hardware
+ * - Motors
+ * - BLDC motors
+ * - Stepper motors
+ * - Drivers
+ * - BLDC drivers
+ * - Gimbal drivers
+ * - Stepper drivers
+ * - Position sensors
+ * - Encoders
+ * - Magnetic sensors
+ * - Hall sensors
+ * - Open-loop control
+ * - Microcontrollers
+ * - Arduino
+ * - STM32
+ * - ESP32
+ * - Teensy
+ *
+ * @section example_code Example code
+ * @code
+#include
+
+// BLDCMotor( pole_pairs )
+BLDCMotor motor = BLDCMotor(11);
+// BLDCDriver( pin_pwmA, pin_pwmB, pin_pwmC, enable (optional) )
+BLDCDriver3PWM motor = BLDCDriver3PWM(9, 10, 11, 8);
+// Encoder(pin_A, pin_B, CPR)
+Encoder encoder = Encoder(2, 3, 2048);
+// channel A and B callbacks
+void doA(){encoder.handleA();}
+void doB(){encoder.handleB();}
+
+
+void setup() {
+ // initialize encoder hardware
+ encoder.init();
+ // hardware interrupt enable
+ encoder.enableInterrupts(doA, doB);
+ // link the motor to the sensor
+ motor.linkSensor(&encoder);
+
+ // power supply voltage [V]
+ driver.voltage_power_supply = 12;
+ // initialise driver hardware
+ driver.init();
+ // link driver
+ motor.linkDriver(&driver);
+
+ // set control loop type to be used
+ motor.controller = MotionControlType::velocity;
+ // initialize motor
+ motor.init();
+
+ // align encoder and start FOC
+ motor.initFOC();
+}
+
+void loop() {
+ // FOC algorithm function
+ motor.loopFOC();
+
+ // velocity control loop function
+ // setting the target velocity or 2rad/s
+ motor.move(2);
+}
+ * @endcode
+ *
+ * @section license License
+ *
+ * MIT license, all text here must be included in any redistribution.
+ *
+ */
+
+#ifndef SIMPLEFOC_EMPTY_H
+#define SIMPLEFOC_EMPTY_H
+
+// #include "BLDCMotor.h"
+// #include "StepperMotor.h"
+// #include "sensors/Encoder.h"
+// #include "sensors/MagneticSensorSPI.h"
+// #include "sensors/MagneticSensorI2C.h"
+// #include "sensors/MagneticSensorAnalog.h"
+// #include "sensors/MagneticSensorPWM.h"
+// #include "sensors/HallSensor.h"
+// #include "sensors/GenericSensor.h"
+// #include "drivers/BLDCDriver3PWM.h"
+// #include "drivers/BLDCDriver6PWM.h"
+// #include "drivers/StepperDriver4PWM.h"
+// #include "drivers/StepperDriver2PWM.h"
+// #include "current_sense/InlineCurrentSense.h"
+// #include "current_sense/LowsideCurrentSense.h"
+// #include "current_sense/GenericCurrentSense.h"
+// #include "communication/Commander.h"
+// #include "communication/StepDirListener.h"
+// #include "communication/SimpleFOCDebug.h"
+
+#endif
diff --git a/src/StepperMotor.cpp b/src/StepperMotor.cpp
index 19c96cd7..df710aad 100644
--- a/src/StepperMotor.cpp
+++ b/src/StepperMotor.cpp
@@ -108,12 +108,28 @@ int StepperMotor::initFOC() {
// alignment necessary for encoders!
// sensor and motor alignment - can be skipped
// by setting motor.sensor_direction and motor.zero_electric_angle
- _delay(500);
if(sensor){
exit_flag *= alignSensor();
// added the shaft_angle update
sensor->update();
- shaft_angle = sensor->getAngle();
+ shaft_angle = shaftAngle();
+
+ // aligning the current sensor - can be skipped
+ // checks if driver phases are the same as current sense phases
+ // and checks the direction of measuremnt.
+ if(exit_flag){
+ if(current_sense){
+ if (!current_sense->initialized) {
+ motor_status = FOCMotorStatus::motor_calib_failed;
+ SIMPLEFOC_DEBUG("MOT: Init FOC error, current sense not initialized");
+ exit_flag = 0;
+ }else{
+ exit_flag *= alignCurrentSense();
+ }
+ }
+ else { SIMPLEFOC_DEBUG("MOT: No current sense."); }
+ }
+
} else {
SIMPLEFOC_DEBUG("MOT: No sensor.");
if ((controller == MotionControlType::angle_openloop || controller == MotionControlType::velocity_openloop)){
@@ -136,6 +152,26 @@ int StepperMotor::initFOC() {
return exit_flag;
}
+// Calibrate the motor and current sense phases
+int StepperMotor::alignCurrentSense() {
+ int exit_flag = 1; // success
+
+ SIMPLEFOC_DEBUG("MOT: Align current sense.");
+
+ // align current sense and the driver
+ exit_flag = current_sense->driverAlign(voltage_sensor_align);
+ if(!exit_flag){
+ // error in current sense - phase either not measured or bad connection
+ SIMPLEFOC_DEBUG("MOT: Align error!");
+ exit_flag = 0;
+ }else{
+ // output the alignment status flag
+ SIMPLEFOC_DEBUG("MOT: Success: ", exit_flag);
+ }
+
+ return exit_flag > 0;
+}
+
// Encoder alignment to electrical 0 angle
int StepperMotor::alignSensor() {
int exit_flag = 1; //success
@@ -261,8 +297,6 @@ void StepperMotor::loopFOC() {
// if open-loop do nothing
if( controller==MotionControlType::angle_openloop || controller==MotionControlType::velocity_openloop ) return;
- // shaft angle
- shaft_angle = shaftAngle();
// if disabled do nothing
if(!enabled) return;
@@ -272,6 +306,44 @@ void StepperMotor::loopFOC() {
// which is in range 0-2PI
electrical_angle = electricalAngle();
+ // Needs the update() to be called first
+ // This function will not have numerical issues because it uses Sensor::getMechanicalAngle()
+ // which is in range 0-2PI
+ electrical_angle = electricalAngle();
+ switch (torque_controller) {
+ case TorqueControlType::voltage:
+ // no need to do anything really
+ break;
+ case TorqueControlType::dc_current:
+ if(!current_sense) return;
+ // read overall current magnitude
+ current.q = current_sense->getDCCurrent(electrical_angle);
+ // filter the value values
+ current.q = LPF_current_q(current.q);
+ // calculate the phase voltage
+ voltage.q = PID_current_q(current_sp - current.q);
+ // d voltage - lag compensation
+ if(_isset(phase_inductance)) voltage.d = _constrain( -current_sp*shaft_velocity*pole_pairs*phase_inductance, -voltage_limit, voltage_limit);
+ else voltage.d = 0;
+ break;
+ case TorqueControlType::foc_current:
+ if(!current_sense) return;
+ // read dq currents
+ current = current_sense->getFOCCurrents(electrical_angle);
+ // filter values
+ current.q = LPF_current_q(current.q);
+ current.d = LPF_current_d(current.d);
+ // calculate the phase voltages
+ voltage.q = PID_current_q(current_sp - current.q);
+ voltage.d = PID_current_d(-current.d);
+ // d voltage - lag compensation - TODO verify
+ // if(_isset(phase_inductance)) voltage.d = _constrain( voltage.d - current_sp*shaft_velocity*pole_pairs*phase_inductance, -voltage_limit, voltage_limit);
+ break;
+ default:
+ // no torque control selected
+ SIMPLEFOC_DEBUG("MOT: no torque control selected!");
+ break;
+ }
// set the phase voltage - FOC heart function :)
setPhaseVoltage(voltage.q, voltage.d, electrical_angle);
}
@@ -310,56 +382,70 @@ void StepperMotor::move(float new_target) {
// estimate the motor current if phase reistance available and current_sense not available
if(!current_sense && _isset(phase_resistance)) current.q = (voltage.q - voltage_bemf)/phase_resistance;
- // choose control loop
+ // upgrade the current based voltage limit
switch (controller) {
case MotionControlType::torque:
- if(!_isset(phase_resistance)) voltage.q = target; // if voltage torque control
- else voltage.q = target*phase_resistance + voltage_bemf;
- voltage.q = _constrain(voltage.q, -voltage_limit, voltage_limit);
- // set d-component (lag compensation if known inductance)
- if(!_isset(phase_inductance)) voltage.d = 0;
- else voltage.d = _constrain( -target*shaft_velocity*pole_pairs*phase_inductance, -voltage_limit, voltage_limit);
+ if(torque_controller == TorqueControlType::voltage){ // if voltage torque control
+ if(!_isset(phase_resistance)) voltage.q = target;
+ else voltage.q = target*phase_resistance + voltage_bemf;
+ voltage.q = _constrain(voltage.q, -voltage_limit, voltage_limit);
+ // set d-component (lag compensation if known inductance)
+ if(!_isset(phase_inductance)) voltage.d = 0;
+ else voltage.d = _constrain( -target*shaft_velocity*pole_pairs*phase_inductance, -voltage_limit, voltage_limit);
+ }else{
+ current_sp = target; // if current/foc_current torque control
+ }
break;
case MotionControlType::angle:
+ // TODO sensor precision: this calculation is not numerically precise. The target value cannot express precise positions when
+ // the angles are large. This results in not being able to command small changes at high position values.
+ // to solve this, the delta-angle has to be calculated in a numerically precise way.
// angle set point
shaft_angle_sp = target;
// calculate velocity set point
shaft_velocity_sp = feed_forward_velocity + P_angle( shaft_angle_sp - shaft_angle );
- shaft_velocity_sp = _constrain(shaft_velocity_sp, -velocity_limit, velocity_limit);
- // calculate the torque command
+ shaft_velocity_sp = _constrain(shaft_velocity_sp,-velocity_limit, velocity_limit);
+ // calculate the torque command - sensor precision: this calculation is ok, but based on bad value from previous calculation
current_sp = PID_velocity(shaft_velocity_sp - shaft_velocity); // if voltage torque control
// if torque controlled through voltage
- // use voltage if phase-resistance not provided
- if(!_isset(phase_resistance)) voltage.q = current_sp;
- else voltage.q = _constrain( current_sp*phase_resistance + voltage_bemf , -voltage_limit, voltage_limit);
- // set d-component (lag compensation if known inductance)
- if(!_isset(phase_inductance)) voltage.d = 0;
- else voltage.d = _constrain( -current_sp*shaft_velocity*pole_pairs*phase_inductance, -voltage_limit, voltage_limit);
+ if(torque_controller == TorqueControlType::voltage){
+ // use voltage if phase-resistance not provided
+ if(!_isset(phase_resistance)) voltage.q = current_sp;
+ else voltage.q = _constrain( current_sp*phase_resistance + voltage_bemf , -voltage_limit, voltage_limit);
+ // set d-component (lag compensation if known inductance)
+ if(!_isset(phase_inductance)) voltage.d = 0;
+ else voltage.d = _constrain( -current_sp*shaft_velocity*pole_pairs*phase_inductance, -voltage_limit, voltage_limit);
+ }
break;
case MotionControlType::velocity:
- // velocity set point
+ // velocity set point - sensor precision: this calculation is numerically precise.
shaft_velocity_sp = target;
// calculate the torque command
current_sp = PID_velocity(shaft_velocity_sp - shaft_velocity); // if current/foc_current torque control
// if torque controlled through voltage control
- // use voltage if phase-resistance not provided
- if(!_isset(phase_resistance)) voltage.q = current_sp;
- else voltage.q = _constrain( current_sp*phase_resistance + voltage_bemf , -voltage_limit, voltage_limit);
- // set d-component (lag compensation if known inductance)
- if(!_isset(phase_inductance)) voltage.d = 0;
- else voltage.d = _constrain( -current_sp*shaft_velocity*pole_pairs*phase_inductance, -voltage_limit, voltage_limit);
+ if(torque_controller == TorqueControlType::voltage){
+ // use voltage if phase-resistance not provided
+ if(!_isset(phase_resistance)) voltage.q = current_sp;
+ else voltage.q = _constrain( current_sp*phase_resistance + voltage_bemf , -voltage_limit, voltage_limit);
+ // set d-component (lag compensation if known inductance)
+ if(!_isset(phase_inductance)) voltage.d = 0;
+ else voltage.d = _constrain( -current_sp*shaft_velocity*pole_pairs*phase_inductance, -voltage_limit, voltage_limit);
+ }
break;
case MotionControlType::velocity_openloop:
- // velocity control in open loop
+ // velocity control in open loop - sensor precision: this calculation is numerically precise.
shaft_velocity_sp = target;
voltage.q = velocityOpenloop(shaft_velocity_sp); // returns the voltage that is set to the motor
- voltage.d = 0; // TODO d-component lag-compensation
+ voltage.d = 0;
break;
case MotionControlType::angle_openloop:
- // angle control in open loop
+ // angle control in open loop -
+ // TODO sensor precision: this calculation NOT numerically precise, and subject
+ // to the same problems in small set-point changes at high angles
+ // as the closed loop version.
shaft_angle_sp = target;
voltage.q = angleOpenloop(shaft_angle_sp); // returns the voltage that is set to the motor
- voltage.d = 0; // TODO d-component lag-compensation
+ voltage.d = 0;
break;
}
}
diff --git a/src/StepperMotor.h b/src/StepperMotor.h
index 45d20c63..f76e7bcf 100644
--- a/src/StepperMotor.h
+++ b/src/StepperMotor.h
@@ -89,6 +89,8 @@ class StepperMotor: public FOCMotor
int alignSensor();
/** Motor and sensor alignment to the sensors absolute 0 angle */
int absoluteZeroSearch();
+ /** Current sense and motor phase alignment */
+ int alignCurrentSense();
// Open loop motion control
/**
diff --git a/src/common/base_classes/BLDCDriver.h b/src/common/base_classes/BLDCDriver.h
index f405d785..6ae8186f 100644
--- a/src/common/base_classes/BLDCDriver.h
+++ b/src/common/base_classes/BLDCDriver.h
@@ -2,40 +2,17 @@
#define BLDCDRIVER_H
#include "Arduino.h"
+#include "FOCDriver.h"
-
-enum PhaseState : uint8_t {
- PHASE_OFF = 0, // both sides of the phase are off
- PHASE_ON = 1, // both sides of the phase are driven with PWM, dead time is applied in 6-PWM mode
- PHASE_HI = 2, // only the high side of the phase is driven with PWM (6-PWM mode only)
- PHASE_LO = 3, // only the low side of the phase is driven with PWM (6-PWM mode only)
-};
-
-
-class BLDCDriver{
+class BLDCDriver: public FOCDriver{
public:
- /** Initialise hardware */
- virtual int init() = 0;
- /** Enable hardware */
- virtual void enable() = 0;
- /** Disable hardware */
- virtual void disable() = 0;
-
- long pwm_frequency; //!< pwm frequency value in hertz
- float voltage_power_supply; //!< power supply voltage
- float voltage_limit; //!< limiting voltage set to the motor
-
-
float dc_a; //!< currently set duty cycle on phaseA
float dc_b; //!< currently set duty cycle on phaseB
float dc_c; //!< currently set duty cycle on phaseC
- bool initialized = false; // true if driver was successfully initialized
- void* params = 0; // pointer to hardware specific parameters of driver
-
/**
- * Set phase voltages to the harware
+ * Set phase voltages to the hardware
*
* @param Ua - phase A voltage
* @param Ub - phase B voltage
@@ -51,6 +28,9 @@ class BLDCDriver{
* @param sa - phase C state : active / disabled ( high impedance )
*/
virtual void setPhaseState(PhaseState sa, PhaseState sb, PhaseState sc) = 0;
+
+ /** driver type getter function */
+ virtual DriverType type() override { return DriverType::BLDC; };
};
#endif
diff --git a/src/common/base_classes/CurrentSense.cpp b/src/common/base_classes/CurrentSense.cpp
index 03ea19ea..4e6c3386 100644
--- a/src/common/base_classes/CurrentSense.cpp
+++ b/src/common/base_classes/CurrentSense.cpp
@@ -1,4 +1,5 @@
#include "CurrentSense.h"
+#include "../../communication/SimpleFOCDebug.h"
// get current magnitude
@@ -27,7 +28,7 @@ float CurrentSense::getDCCurrent(float motor_electrical_angle){
return sign*_sqrt(ABcurrent.alpha*ABcurrent.alpha + ABcurrent.beta*ABcurrent.beta);
}
-// function used with the foc algorihtm
+// function used with the foc algorithm
// calculating DQ currents from phase currents
// - function calculating park and clarke transform of the phase currents
// - using getPhaseCurrents and getABCurrents internally
@@ -44,11 +45,21 @@ DQCurrent_s CurrentSense::getFOCCurrents(float angle_el){
return return_current;
}
-// function used with the foc algorihtm
+// function used with the foc algorithm
// calculating Alpha Beta currents from phase currents
// - function calculating Clarke transform of the phase currents
ABCurrent_s CurrentSense::getABCurrents(PhaseCurrent_s current){
+ // check if driver is an instance of StepperDriver
+ // if so there is no need to Clarke transform
+ if (driver_type == DriverType::Stepper){
+ ABCurrent_s return_ABcurrent;
+ return_ABcurrent.alpha = current.a;
+ return_ABcurrent.beta = current.b;
+ return return_ABcurrent;
+ }
+
+ // otherwise it's a BLDC motor and
// calculate clarke transform
float i_alpha, i_beta;
if(!current.c){
@@ -80,7 +91,7 @@ ABCurrent_s CurrentSense::getABCurrents(PhaseCurrent_s current){
return return_ABcurrent;
}
-// function used with the foc algorihtm
+// function used with the foc algorithm
// calculating D and Q currents from Alpha Beta currents and electrical angle
// - function calculating Clarke transform of the phase currents
DQCurrent_s CurrentSense::getDQCurrents(ABCurrent_s current, float angle_el){
@@ -97,8 +108,10 @@ DQCurrent_s CurrentSense::getDQCurrents(ABCurrent_s current, float angle_el){
/**
Driver linking to the current sense
*/
-void CurrentSense::linkDriver(BLDCDriver* _driver) {
- driver = _driver;
+void CurrentSense::linkDriver(FOCDriver* _driver) {
+ driver = _driver;
+ // save the driver type for easier access
+ driver_type = driver->type();
}
@@ -108,4 +121,232 @@ void CurrentSense::enable(){
void CurrentSense::disable(){
// nothing is done here, but you can override this function
-};
\ No newline at end of file
+};
+
+
+// Function aligning the current sense with motor driver
+// if all pins are connected well none of this is really necessary! - can be avoided
+// returns flag
+// 0 - fail
+// 1 - success and nothing changed
+// 2 - success but pins reconfigured
+// 3 - success but gains inverted
+// 4 - success but pins reconfigured and gains inverted
+// IMPORTANT, this function can be overriden in the child class
+int CurrentSense::driverAlign(float voltage){
+
+ int exit_flag = 1;
+ if(skip_align) return exit_flag;
+
+ if (!initialized) return 0;
+
+ // check if stepper or BLDC
+ if(driver_type == DriverType::Stepper)
+ return alignStepperDriver(voltage, (StepperDriver*)driver);
+ else
+ return alignBLDCDriver(voltage, (BLDCDriver*)driver);
+}
+
+
+
+// Helper function to read and average phase currents
+PhaseCurrent_s CurrentSense::readAverageCurrents(int N) {
+ PhaseCurrent_s c = getPhaseCurrents();
+ for (int i = 0; i < N; i++) {
+ PhaseCurrent_s c1 = getPhaseCurrents();
+ c.a = c.a * 0.6f + 0.4f * c1.a;
+ c.b = c.b * 0.6f + 0.4f * c1.b;
+ c.c = c.c * 0.6f + 0.4f * c1.c;
+ _delay(3);
+ }
+ return c;
+};
+
+
+
+// Function aligning the current sense with motor driver
+// if all pins are connected well none of this is really necessary! - can be avoided
+// returns flag
+// 0 - fail
+// 1 - success and nothing changed
+// 2 - success but pins reconfigured
+// 3 - success but gains inverted
+// 4 - success but pins reconfigured and gains inverted
+int CurrentSense::alignBLDCDriver(float voltage, BLDCDriver* bldc_driver){
+
+ int exit_flag = 1;
+ if(_isset(pinA)){
+ // set phase A active and phases B and C down
+ bldc_driver->setPwm(voltage, 0, 0);
+ _delay(500);
+ PhaseCurrent_s c = readAverageCurrents();
+ bldc_driver->setPwm(0, 0, 0);
+ // align phase A
+ float ab_ratio = c.b ? fabs(c.a / c.b) : 0;
+ float ac_ratio = c.c ? fabs(c.a / c.c) : 0;
+ if(_isset(pinB) && ab_ratio > 1.5f ){ // should be ~2
+ gain_a *= _sign(c.a);
+ }else if(_isset(pinC) && ac_ratio > 1.5f ){ // should be ~2
+ gain_a *= _sign(c.a);
+ }else if(_isset(pinB) && ab_ratio < 0.7f ){ // should be ~0.5
+ SIMPLEFOC_DEBUG("CS: Switch A-B");
+ // switch phase A and B
+ _swap(pinA, pinB);
+ _swap(offset_ia, offset_ib);
+ _swap(gain_a, gain_b);
+ gain_a *= _sign(c.b);
+ exit_flag = 2; // signal that pins have been switched
+ }else if(_isset(pinC) && ac_ratio < 0.7f ){ // should be ~0.5
+ SIMPLEFOC_DEBUG("CS: Switch A-C");
+ // switch phase A and C
+ _swap(pinA, pinC);
+ _swap(offset_ia, offset_ic);
+ _swap(gain_a, gain_c);
+ gain_a *= _sign(c.c);
+ exit_flag = 2;// signal that pins have been switched
+ }else{
+ SIMPLEFOC_DEBUG("CS: Err read A");
+ // error in current sense - phase either not measured or bad connection
+ return 0;
+ }
+ }
+
+ if(_isset(pinB)){
+ // set phase B active and phases A and C down
+ bldc_driver->setPwm(0, voltage, 0);
+ _delay(500);
+ PhaseCurrent_s c = readAverageCurrents();
+ bldc_driver->setPwm(0, 0, 0);
+ float ba_ratio = c.a ? fabs(c.b / c.a) : 0;
+ float bc_ratio = c.c ? fabs(c.b / c.c) : 0;
+ if(_isset(pinA) && ba_ratio > 1.5f ){ // should be ~2);
+ gain_b *= _sign(c.b);
+ }else if(_isset(pinC) && bc_ratio > 1.5f ){ // should be ~2
+ gain_b *= _sign(c.b);
+ }else if(_isset(pinA) && ba_ratio < 0.7f ){ // it should be ~0.5
+ SIMPLEFOC_DEBUG("CS: Switch B-A");
+ // switch phase A and B
+ _swap(pinB, pinA);
+ _swap(offset_ib, offset_ia);
+ _swap(gain_b, gain_a);
+ gain_b *= _sign(c.a);
+ exit_flag = 2; // signal that pins have been switched
+ }else if(_isset(pinC) && bc_ratio < 0.7f ){ // should be ~0.5
+ SIMPLEFOC_DEBUG("CS: Switch B-C");
+ _swap(pinB, pinC);
+ _swap(offset_ib, offset_ic);
+ _swap(gain_b, gain_c);
+ gain_b *= _sign(c.c);
+ exit_flag = 2; // signal that pins have been switched
+ }else{
+ SIMPLEFOC_DEBUG("CS: Error read B");
+ // error in current sense - phase either not measured or bad connection
+ return 0;
+ }
+ }
+
+ // if phase C measured
+ if(_isset(pinC)){
+ // set phase C active and phases A and B down
+ bldc_driver->setPwm(0, 0, voltage);
+ _delay(500);
+ PhaseCurrent_s c = readAverageCurrents();
+ bldc_driver->setPwm(0, 0, 0);
+ float ca_ratio = c.a ? fabs(c.c / c.a) : 0;
+ float cb_ratio = c.b ? fabs(c.c / c.b) : 0;
+ if(_isset(pinA) && ca_ratio > 1.5f ){ // should be ~2
+ gain_c *= _sign(c.c);
+ }else if(_isset(pinB) && cb_ratio > 1.5f ){ // should be ~2
+ gain_c *= _sign(c.c);
+ }else if(_isset(pinA) && ca_ratio < 0.7f ){ // it should be ~0.5
+ SIMPLEFOC_DEBUG("CS: Switch C-A");
+ // switch phase A and C
+ _swap(pinC, pinA);
+ _swap(offset_ic, offset_ia);
+ _swap(gain_c, gain_a);
+ gain_c *= _sign(c.a);
+ exit_flag = 2; // signal that pins have been switched
+ }else if(_isset(pinB) && cb_ratio < 0.7f ){ // should be ~0.5
+ SIMPLEFOC_DEBUG("CS: Switch C-B");
+ _swap(pinC, pinB);
+ _swap(offset_ic, offset_ib);
+ _swap(gain_c, gain_b);
+ gain_b *= _sign(c.b);
+ exit_flag = 2; // signal that pins have been switched
+ }else{
+ SIMPLEFOC_DEBUG("CS: Err read C");
+ // error in current sense - phase either not measured or bad connection
+ return 0;
+ }
+ }
+ // add 2 if pin gains negative
+ if(gain_a < 0 || gain_b < 0 || gain_c < 0) exit_flag +=2;
+ return exit_flag;
+}
+
+
+// Function aligning the current sense with motor driver
+// if all pins are connected well none of this is really necessary! - can be avoided
+// returns flag
+// 0 - fail
+// 1 - success and nothing changed
+// 2 - success but pins reconfigured
+// 3 - success but gains inverted
+// 4 - success but pins reconfigured and gains inverted
+int CurrentSense::alignStepperDriver(float voltage, StepperDriver* stepper_driver){
+
+ int exit_flag = 1;
+
+ if(_isset(pinA)){
+ // set phase A active to high and B to low
+ stepper_driver->setPwm(voltage, 0);
+ _delay(500);
+ PhaseCurrent_s c = readAverageCurrents();
+ // disable the phases
+ stepper_driver->setPwm(0, 0);
+ if (fabs(c.a) < 0.1f && fabs(c.b) < 0.1f ){
+ SIMPLEFOC_DEBUG("CS: Err too low current!");
+ return 0; // measurement current too low
+ }
+ // align phase A
+ // check if measured current a is positive and invert if not
+ // check if current b is around zero and if its not
+ // check if current a is near zero and if it is invert them
+ if (fabs(c.a) < fabs(c.b)){
+ SIMPLEFOC_DEBUG("CS: Switch A-B");
+ // switch phase A and B
+ _swap(pinA, pinB);
+ _swap(offset_ia, offset_ib);
+ _swap(gain_a, gain_b);
+ gain_a *= _sign(c.b);
+ exit_flag = 2; // signal that pins have been switched
+ }else if (c.a < 0){
+ SIMPLEFOC_DEBUG("CS: Neg A");
+ gain_a *= -1;
+ }
+ }
+
+ if(_isset(pinB)){
+ // set phase B active and phases A and C down
+ stepper_driver->setPwm(voltage, 0);
+ _delay(500);
+ PhaseCurrent_s c = readAverageCurrents();
+ stepper_driver->setPwm(0, 0);
+ if (fabs(c.a) < 0.1f && fabs(c.b) < 0.1f ){
+ SIMPLEFOC_DEBUG("CS: Err too low current!");
+ return 0; // measurement current too low
+ }
+ // align phase A
+ // check if measured current a is positive and invert if not
+ if (c.b < 0){
+ SIMPLEFOC_DEBUG("CS: Neg B");
+ gain_b *= -1;
+ }
+ }
+
+ // add 2 if pin gains negative
+ if(gain_a < 0 || gain_b < 0 ) exit_flag +=2;
+ return exit_flag;
+}
+
+
diff --git a/src/common/base_classes/CurrentSense.h b/src/common/base_classes/CurrentSense.h
index 1c839053..c9c79650 100644
--- a/src/common/base_classes/CurrentSense.h
+++ b/src/common/base_classes/CurrentSense.h
@@ -1,12 +1,15 @@
#ifndef CURRENTSENSE_H
#define CURRENTSENSE_H
-#include "BLDCDriver.h"
+#include "FOCDriver.h"
#include "../foc_utils.h"
+#include "../time_utils.h"
+#include "StepperDriver.h"
+#include "BLDCDriver.h"
/**
* Current sensing abstract class defintion
- * Each current sensoring implementation needs to extend this interface
+ * Each current sensing implementation needs to extend this interface
*/
class CurrentSense{
public:
@@ -23,24 +26,49 @@ class CurrentSense{
* Linking the current sense with the motor driver
* Only necessary if synchronisation in between the two is required
*/
- void linkDriver(BLDCDriver *driver);
+ void linkDriver(FOCDriver *driver);
// variables
bool skip_align = false; //!< variable signaling that the phase current direction should be verified during initFOC()
- BLDCDriver* driver = nullptr; //!< driver link
+ FOCDriver* driver = nullptr; //!< driver link
bool initialized = false; // true if current sense was successfully initialized
void* params = 0; //!< pointer to hardware specific parameters of current sensing
+ DriverType driver_type = DriverType::Unknown; //!< driver type (BLDC or Stepper)
+
+ // ADC measurement gain for each phase
+ // support for different gains for different phases of more commonly - inverted phase currents
+ // this should be automated later
+ float gain_a; //!< phase A gain
+ float gain_b; //!< phase B gain
+ float gain_c; //!< phase C gain
+
+ float offset_ia; //!< zero current A voltage value (center of the adc reading)
+ float offset_ib; //!< zero current B voltage value (center of the adc reading)
+ float offset_ic; //!< zero current C voltage value (center of the adc reading)
+
+ // hardware variables
+ int pinA; //!< pin A analog pin for current measurement
+ int pinB; //!< pin B analog pin for current measurement
+ int pinC; //!< pin C analog pin for current measurement
+
/**
* Function intended to verify if:
* - phase current are oriented properly
* - if their order is the same as driver phases
*
* This function corrects the alignment errors if possible ans if no such thing is needed it can be left empty (return 1)
- * @returns - 0 - for failure & positive number (with status) - for success
+ * @returns -
+ 0 - failure
+ 1 - success and nothing changed
+ 2 - success but pins reconfigured
+ 3 - success but gains inverted
+ 4 - success but pins reconfigured and gains inverted
+ *
+ * IMPORTANT: Default implementation provided in the CurrentSense class, but can be overriden in the child classes
*/
- virtual int driverAlign(float align_voltage) = 0;
+ virtual int driverAlign(float align_voltage);
/**
* Function rading the phase currents a, b and c
@@ -80,7 +108,7 @@ class CurrentSense{
/**
* Function used for Park transform in FOC control
- * It reads the Alpha Beta currents and electircal angle of the motor
+ * It reads the Alpha Beta currents and electrical angle of the motor
* It returns the D and Q currents
*
* @param current - phase current
@@ -98,6 +126,20 @@ class CurrentSense{
* override it to do something useful.
*/
virtual void disable();
+
+ /**
+ * Function used to align the current sense with the BLDC motor driver
+ */
+ int alignBLDCDriver(float align_voltage, BLDCDriver* driver);
+ /**
+ * Function used to align the current sense with the Stepper motor driver
+ */
+ int alignStepperDriver(float align_voltage, StepperDriver* driver);
+ /**
+ * Function used to read the average current values over N samples
+ */
+ PhaseCurrent_s readAverageCurrents(int N=100);
+
};
#endif
\ No newline at end of file
diff --git a/src/common/base_classes/FOCDriver.h b/src/common/base_classes/FOCDriver.h
new file mode 100644
index 00000000..944251a4
--- /dev/null
+++ b/src/common/base_classes/FOCDriver.h
@@ -0,0 +1,47 @@
+#ifndef FOCDRIVER_H
+#define FOCDRIVER_H
+
+#include "Arduino.h"
+
+
+enum PhaseState : uint8_t {
+ PHASE_OFF = 0, // both sides of the phase are off
+ PHASE_ON = 1, // both sides of the phase are driven with PWM, dead time is applied in 6-PWM mode
+ PHASE_HI = 2, // only the high side of the phase is driven with PWM (6-PWM mode only)
+ PHASE_LO = 3, // only the low side of the phase is driven with PWM (6-PWM mode only)
+};
+
+
+enum DriverType{
+ Unknown=0,
+ BLDC=1,
+ Stepper=2
+};
+
+/**
+ * FOC driver class
+ */
+class FOCDriver{
+ public:
+
+ /** Initialise hardware */
+ virtual int init() = 0;
+ /** Enable hardware */
+ virtual void enable() = 0;
+ /** Disable hardware */
+ virtual void disable() = 0;
+
+ long pwm_frequency; //!< pwm frequency value in hertz
+ float voltage_power_supply; //!< power supply voltage
+ float voltage_limit; //!< limiting voltage set to the motor
+
+ bool initialized = false; //!< true if driver was successfully initialized
+ void* params = 0; //!< pointer to hardware specific parameters of driver
+
+ bool enable_active_high = true; //!< enable pin should be set to high to enable the driver (default is HIGH)
+
+ /** get the driver type*/
+ virtual DriverType type() = 0;
+};
+
+#endif
diff --git a/src/common/base_classes/StepperDriver.h b/src/common/base_classes/StepperDriver.h
index 2006fc8c..9864b235 100644
--- a/src/common/base_classes/StepperDriver.h
+++ b/src/common/base_classes/StepperDriver.h
@@ -1,32 +1,30 @@
#ifndef STEPPERDRIVER_H
#define STEPPERDRIVER_H
-#include "drivers/hardware_api.h"
+#include "Arduino.h"
+#include "FOCDriver.h"
-class StepperDriver{
+class StepperDriver: public FOCDriver{
public:
- /** Initialise hardware */
- virtual int init() = 0;
- /** Enable hardware */
- virtual void enable() = 0;
- /** Disable hardware */
- virtual void disable() = 0;
-
- long pwm_frequency; //!< pwm frequency value in hertz
- float voltage_power_supply; //!< power supply voltage
- float voltage_limit; //!< limiting voltage set to the motor
-
- bool initialized = false; // true if driver was successfully initialized
- void* params = 0; // pointer to hardware specific parameters of driver
-
/**
- * Set phase voltages to the harware
+ * Set phase voltages to the hardware
*
* @param Ua phase A voltage
* @param Ub phase B voltage
*/
virtual void setPwm(float Ua, float Ub) = 0;
+
+ /**
+ * Set phase state, enable/disable
+ *
+ * @param sc - phase A state : active / disabled ( high impedance )
+ * @param sb - phase B state : active / disabled ( high impedance )
+ */
+ virtual void setPhaseState(PhaseState sa, PhaseState sb) = 0;
+
+ /** driver type getter function */
+ virtual DriverType type() override { return DriverType::Stepper; } ;
};
#endif
\ No newline at end of file
diff --git a/src/common/foc_utils.h b/src/common/foc_utils.h
index f2bc9ef6..9a5b53ea 100644
--- a/src/common/foc_utils.h
+++ b/src/common/foc_utils.h
@@ -14,6 +14,8 @@
#define _UNUSED(v) (void) (v)
#define _powtwo(x) (1 << (x))
+#define _swap(a, b) { auto temp = a; a = b; b = temp; }
+
// utility defines
#define _2_SQRT3 1.15470053838f
#define _SQRT3 1.73205080757f
diff --git a/src/communication/SimpleFOCDebug.h b/src/communication/SimpleFOCDebug.h
index 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..9d90f0ca 100644
--- a/src/current_sense/GenericCurrentSense.cpp
+++ b/src/current_sense/GenericCurrentSense.cpp
@@ -54,110 +54,10 @@ PhaseCurrent_s GenericCurrentSense::getPhaseCurrents(){
// returns flag
// 0 - fail
// 1 - success and nothing changed
-// 2 - success but pins reconfigured
-// 3 - success but gains inverted
-// 4 - success but pins reconfigured and gains inverted
int GenericCurrentSense::driverAlign(float voltage){
_UNUSED(voltage) ; // remove unused parameter warning
int exit_flag = 1;
if(skip_align) return exit_flag;
-
if (!initialized) return 0;
-
- // // set phase A active and phases B and C down
- // driver->setPwm(voltage, 0, 0);
- // _delay(200);
- // PhaseCurrent_s c = getPhaseCurrents();
- // // read the current 100 times ( arbitrary number )
- // for (int i = 0; i < 100; i++) {
- // PhaseCurrent_s c1 = getPhaseCurrents();
- // c.a = c.a*0.6f + 0.4f*c1.a;
- // c.b = c.b*0.6f + 0.4f*c1.b;
- // c.c = c.c*0.6f + 0.4f*c1.c;
- // _delay(3);
- // }
- // driver->setPwm(0, 0, 0);
- // // align phase A
- // float ab_ratio = fabs(c.a / c.b);
- // float ac_ratio = c.c ? fabs(c.a / c.c) : 0;
- // if( ab_ratio > 1.5f ){ // should be ~2
- // gain_a *= _sign(c.a);
- // }else if( ab_ratio < 0.7f ){ // should be ~0.5
- // // switch phase A and B
- // int tmp_pinA = pinA;
- // pinA = pinB;
- // pinB = tmp_pinA;
- // gain_a *= _sign(c.b);
- // exit_flag = 2; // signal that pins have been switched
- // }else if(_isset(pinC) && ac_ratio < 0.7f ){ // should be ~0.5
- // // switch phase A and C
- // int tmp_pinA = pinA;
- // pinA = pinC;
- // pinC= tmp_pinA;
- // gain_a *= _sign(c.c);
- // exit_flag = 2;// signal that pins have been switched
- // }else{
- // // error in current sense - phase either not measured or bad connection
- // return 0;
- // }
-
- // // set phase B active and phases A and C down
- // driver->setPwm(0, voltage, 0);
- // _delay(200);
- // c = getPhaseCurrents();
- // // read the current 50 times
- // for (int i = 0; i < 100; i++) {
- // PhaseCurrent_s c1 = getPhaseCurrents();
- // c.a = c.a*0.6f + 0.4f*c1.a;
- // c.b = c.b*0.6f + 0.4f*c1.b;
- // c.c = c.c*0.6f + 0.4f*c1.c;
- // _delay(3);
- // }
- // driver->setPwm(0, 0, 0);
- // float ba_ratio = fabs(c.b/c.a);
- // float bc_ratio = c.c ? fabs(c.b / c.c) : 0;
- // if( ba_ratio > 1.5f ){ // should be ~2
- // gain_b *= _sign(c.b);
- // }else if( ba_ratio < 0.7f ){ // it should be ~0.5
- // // switch phase A and B
- // int tmp_pinB = pinB;
- // pinB = pinA;
- // pinA = tmp_pinB;
- // gain_b *= _sign(c.a);
- // exit_flag = 2; // signal that pins have been switched
- // }else if(_isset(pinC) && bc_ratio < 0.7f ){ // should be ~0.5
- // // switch phase A and C
- // int tmp_pinB = pinB;
- // pinB = pinC;
- // pinC = tmp_pinB;
- // gain_b *= _sign(c.c);
- // exit_flag = 2; // signal that pins have been switched
- // }else{
- // // error in current sense - phase either not measured or bad connection
- // return 0;
- // }
-
- // // if phase C measured
- // if(_isset(pinC)){
- // // set phase B active and phases A and C down
- // driver->setPwm(0, 0, voltage);
- // _delay(200);
- // c = getPhaseCurrents();
- // // read the adc voltage 500 times ( arbitrary number )
- // for (int i = 0; i < 50; i++) {
- // PhaseCurrent_s c1 = getPhaseCurrents();
- // c.c = (c.c+c1.c)/50.0f;
- // }
- // driver->setPwm(0, 0, 0);
- // gain_c *= _sign(c.c);
- // }
-
- // if(gain_a < 0 || gain_b < 0 || gain_c < 0) exit_flag +=2;
- // exit flag is either
- // 0 - fail
- // 1 - success and nothing changed
- // 2 - success but pins reconfigured
- // 3 - success but gains inverted
- // 4 - success but pins reconfigured and gains inverted
return exit_flag;
}
diff --git a/src/current_sense/InlineCurrentSense.cpp b/src/current_sense/InlineCurrentSense.cpp
index c3db74ef..35c97765 100644
--- a/src/current_sense/InlineCurrentSense.cpp
+++ b/src/current_sense/InlineCurrentSense.cpp
@@ -44,8 +44,14 @@ int InlineCurrentSense::init(){
params = _configureADCInline(drv_params,pinA,pinB,pinC);
// if init failed return fail
if (params == SIMPLEFOC_CURRENT_SENSE_INIT_FAILED) return 0;
+ // set the center pwm (0 voltage vector)
+ if(driver_type==DriverType::BLDC)
+ static_cast(driver)->setPwm(driver->voltage_limit/2, driver->voltage_limit/2, driver->voltage_limit/2);
// calibrate zero offsets
calibrateOffsets();
+ // set zero voltage to all phases
+ if(driver_type==DriverType::BLDC)
+ static_cast(driver)->setPwm(0,0,0);
// set the initialized flag
initialized = (params!=SIMPLEFOC_CURRENT_SENSE_INIT_FAILED);
// return success
@@ -80,174 +86,3 @@ PhaseCurrent_s InlineCurrentSense::getPhaseCurrents(){
current.c = (!_isset(pinC)) ? 0 : (_readADCVoltageInline(pinC, params) - offset_ic)*gain_c; // amps
return current;
}
-
-// Function aligning the current sense with motor driver
-// if all pins are connected well none of this is really necessary! - can be avoided
-// returns flag
-// 0 - fail
-// 1 - success and nothing changed
-// 2 - success but pins reconfigured
-// 3 - success but gains inverted
-// 4 - success but pins reconfigured and gains inverted
-int InlineCurrentSense::driverAlign(float voltage){
-
- int exit_flag = 1;
- if(skip_align) return exit_flag;
-
- if (driver==nullptr) {
- SIMPLEFOC_DEBUG("CUR: No driver linked!");
- return 0;
- }
-
- if (!initialized) return 0;
-
- if(_isset(pinA)){
- // set phase A active and phases B and C down
- driver->setPwm(voltage, 0, 0);
- _delay(2000);
- PhaseCurrent_s c = getPhaseCurrents();
- // read the current 100 times ( arbitrary number )
- for (int i = 0; i < 100; i++) {
- PhaseCurrent_s c1 = getPhaseCurrents();
- c.a = c.a*0.6f + 0.4f*c1.a;
- c.b = c.b*0.6f + 0.4f*c1.b;
- c.c = c.c*0.6f + 0.4f*c1.c;
- _delay(3);
- }
- driver->setPwm(0, 0, 0);
- // align phase A
- float ab_ratio = c.b ? fabs(c.a / c.b) : 0;
- float ac_ratio = c.c ? fabs(c.a / c.c) : 0;
- if(_isset(pinB) && ab_ratio > 1.5f ){ // should be ~2
- gain_a *= _sign(c.a);
- }else if(_isset(pinC) && ac_ratio > 1.5f ){ // should be ~2
- gain_a *= _sign(c.a);
- }else if(_isset(pinB) && ab_ratio < 0.7f ){ // should be ~0.5
- // switch phase A and B
- int tmp_pinA = pinA;
- pinA = pinB;
- pinB = tmp_pinA;
- float tmp_offsetA = offset_ia;
- offset_ia = offset_ib;
- offset_ib = tmp_offsetA;
- gain_a *= _sign(c.b);
- exit_flag = 2; // signal that pins have been switched
- }else if(_isset(pinC) && ac_ratio < 0.7f ){ // should be ~0.5
- // switch phase A and C
- int tmp_pinA = pinA;
- pinA = pinC;
- pinC= tmp_pinA;
- float tmp_offsetA = offset_ia;
- offset_ia = offset_ic;
- offset_ic = tmp_offsetA;
- gain_a *= _sign(c.c);
- exit_flag = 2;// signal that pins have been switched
- }else{
- // error in current sense - phase either not measured or bad connection
- return 0;
- }
- }
-
- if(_isset(pinB)){
- // set phase B active and phases A and C down
- driver->setPwm(0, voltage, 0);
- _delay(200);
- PhaseCurrent_s c = getPhaseCurrents();
- // read the current 50 times
- for (int i = 0; i < 100; i++) {
- PhaseCurrent_s c1 = getPhaseCurrents();
- c.a = c.a*0.6f + 0.4f*c1.a;
- c.b = c.b*0.6f + 0.4f*c1.b;
- c.c = c.c*0.6f + 0.4f*c1.c;
- _delay(3);
- }
- driver->setPwm(0, 0, 0);
- float ba_ratio = c.a ? fabs(c.b / c.a) : 0;
- float bc_ratio = c.c ? fabs(c.b / c.c) : 0;
- if(_isset(pinA) && ba_ratio > 1.5f ){ // should be ~2
- gain_b *= _sign(c.b);
- }else if(_isset(pinC) && bc_ratio > 1.5f ){ // should be ~2
- gain_b *= _sign(c.b);
- }else if(_isset(pinA) && ba_ratio < 0.7f ){ // it should be ~0.5
- // switch phase A and B
- int tmp_pinB = pinB;
- pinB = pinA;
- pinA = tmp_pinB;
- float tmp_offsetB = offset_ib;
- offset_ib = offset_ia;
- offset_ia = tmp_offsetB;
- gain_b *= _sign(c.a);
- exit_flag = 2; // signal that pins have been switched
- }else if(_isset(pinC) && bc_ratio < 0.7f ){ // should be ~0.5
- // switch phase A and C
- int tmp_pinB = pinB;
- pinB = pinC;
- pinC = tmp_pinB;
- float tmp_offsetB = offset_ib;
- offset_ib = offset_ic;
- offset_ic = tmp_offsetB;
- gain_b *= _sign(c.c);
- exit_flag = 2; // signal that pins have been switched
- }else{
- // error in current sense - phase either not measured or bad connection
- return 0;
- }
- }
-
- // if phase C measured
- if(_isset(pinC)){
- // set phase C active and phases A and B down
- driver->setPwm(0, 0, voltage);
- _delay(200);
- PhaseCurrent_s c = getPhaseCurrents();
- // read the adc voltage 500 times ( arbitrary number )
- for (int i = 0; i < 100; i++) {
- PhaseCurrent_s c1 = getPhaseCurrents();
- c.a = c.a*0.6f + 0.4f*c1.a;
- c.b = c.b*0.6f + 0.4f*c1.b;
- c.c = c.c*0.6f + 0.4f*c1.c;
- _delay(3);
- }
- driver->setPwm(0, 0, 0);
- float ca_ratio = c.a ? fabs(c.c / c.a) : 0;
- float cb_ratio = c.b ? fabs(c.c / c.b) : 0;
- if(_isset(pinA) && ca_ratio > 1.5f ){ // should be ~2
- gain_c *= _sign(c.c);
- }else if(_isset(pinB) && cb_ratio > 1.5f ){ // should be ~2
- gain_c *= _sign(c.c);
- }else if(_isset(pinA) && ca_ratio < 0.7f ){ // it should be ~0.5
- // switch phase A and C
- int tmp_pinC = pinC;
- pinC = pinA;
- pinA = tmp_pinC;
- float tmp_offsetC = offset_ic;
- offset_ic = offset_ia;
- offset_ia = tmp_offsetC;
- gain_c *= _sign(c.a);
- exit_flag = 2; // signal that pins have been switched
- }else if(_isset(pinB) && cb_ratio < 0.7f ){ // should be ~0.5
- // switch phase B and C
- int tmp_pinC = pinC;
- pinC = pinB;
- pinB = tmp_pinC;
- float tmp_offsetC = offset_ic;
- offset_ic = offset_ib;
- offset_ib = tmp_offsetC;
- gain_c *= _sign(c.b);
- exit_flag = 2; // signal that pins have been switched
- }else{
- // error in current sense - phase either not measured or bad connection
- return 0;
- }
- }
-
- if(gain_a < 0 || gain_b < 0 || gain_c < 0) exit_flag +=2;
- // exit flag is either
- // 0 - fail
- // 1 - success and nothing changed
- // 2 - success but pins reconfigured
- // 3 - success but gains inverted
- // 4 - success but pins reconfigured and gains inverted
-
- return exit_flag;
-}
diff --git a/src/current_sense/InlineCurrentSense.h b/src/current_sense/InlineCurrentSense.h
index 5fdca7d7..94be0f75 100644
--- a/src/current_sense/InlineCurrentSense.h
+++ b/src/current_sense/InlineCurrentSense.h
@@ -6,10 +6,13 @@
#include "../common/time_utils.h"
#include "../common/defaults.h"
#include "../common/base_classes/CurrentSense.h"
+#include "../common/base_classes/StepperDriver.h"
+#include "../common/base_classes/BLDCDriver.h"
#include "../common/lowpass_filter.h"
#include "hardware_api.h"
+
class InlineCurrentSense: public CurrentSense{
public:
/**
@@ -33,31 +36,9 @@ class InlineCurrentSense: public CurrentSense{
// CurrentSense interface implementing functions
int init() override;
PhaseCurrent_s getPhaseCurrents() override;
- int driverAlign(float align_voltage) override;
-
- // ADC measuremnet gain for each phase
- // support for different gains for different phases of more commonly - inverted phase currents
- // this should be automated later
- float gain_a; //!< phase A gain
- float gain_b; //!< phase B gain
- float gain_c; //!< phase C gain
-
- // // per phase low pass fileters
- // LowPassFilter lpf_a{DEF_LPF_PER_PHASE_CURRENT_SENSE_Tf}; //!< current A low pass filter
- // LowPassFilter lpf_b{DEF_LPF_PER_PHASE_CURRENT_SENSE_Tf}; //!< current B low pass filter
- // LowPassFilter lpf_c{DEF_LPF_PER_PHASE_CURRENT_SENSE_Tf}; //!< current C low pass filter
- float offset_ia; //!< zero current A voltage value (center of the adc reading)
- float offset_ib; //!< zero current B voltage value (center of the adc reading)
- float offset_ic; //!< zero current C voltage value (center of the adc reading)
-
private:
- // hardware variables
- int pinA; //!< pin A analog pin for current measurement
- int pinB; //!< pin B analog pin for current measurement
- int pinC; //!< pin C analog pin for current measurement
-
// gain variables
float shunt_resistor; //!< Shunt resistor value
float amp_gain; //!< amp gain value
diff --git a/src/current_sense/LowsideCurrentSense.cpp b/src/current_sense/LowsideCurrentSense.cpp
index 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:
};