try to fix submodule

firmware/lib/Arduino-FOC-drivers/src/motors/HybridStepperMotor/HybridStepperMotor.cpp

@@ -0,0 +1,548 @@
+#include "HybridStepperMotor.h"
+#include "./communication/SimpleFOCDebug.h"
+
+// HybridStepperMotor(int pp)
+// - pp            - pole pair number
+// - R             - motor phase resistance
+// - KV            - motor kv rating (rmp/v)
+// - L             - motor phase inductance [H]
+HybridStepperMotor::HybridStepperMotor(int pp, float _R, float _KV, float _inductance)
+    : FOCMotor()
+{
+  // number od pole pairs
+  pole_pairs = pp;
+  // save phase resistance number
+  phase_resistance = _R;
+  // save back emf constant KV = 1/K_bemf
+  // usually used rms
+  KV_rating = _KV * _SQRT2;
+  // save phase inductance
+  phase_inductance = _inductance;
+
+  // torque control type is voltage by default
+  // current and foc_current not supported yet
+  torque_controller = TorqueControlType::voltage;
+}
+
+/**
+  Link the driver which controls the motor
+*/
+void HybridStepperMotor::linkDriver(BLDCDriver *_driver)
+{
+  driver = _driver;
+}
+
+// init hardware pins
+void HybridStepperMotor::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 (_isset(phase_resistance))
+  {
+    // velocity control loop controls current
+    PID_velocity.limit = current_limit;
+  }
+  else
+  {
+    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 HybridStepperMotor::disable()
+{
+  // set zero to PWM
+  driver->setPwm(0, 0, 0);
+  // disable driver
+  driver->disable();
+  // motor status update
+  enabled = 0;
+}
+// enable motor driver
+void HybridStepperMotor::enable()
+{
+  // disable enable
+  driver->enable();
+  // set zero to PWM
+  driver->setPwm(0, 0, 0);
+  // motor status update
+  enabled = 1;
+}
+
+/**
+ * FOC functions
+ */
+int HybridStepperMotor::initFOC()
+{
+  int exit_flag = 1;
+
+  motor_status = FOCMotorStatus::motor_calibrating;
+
+  // 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();
+  }
+  else
+    SIMPLEFOC_DEBUG("MOT: No sensor.");
+
+  if (exit_flag)
+  {
+    SIMPLEFOC_DEBUG("MOT: Ready.");
+    motor_status = FOCMotorStatus::motor_ready;
+  }
+  else
+  {
+    SIMPLEFOC_DEBUG("MOT: Init FOC failed.");
+    motor_status = FOCMotorStatus::motor_calib_failed;
+    disable();
+  }
+
+  return exit_flag;
+}
+
+// Encoder alignment to electrical 0 angle
+int HybridStepperMotor::alignSensor()
+{
+  int exit_flag = 1; // success
+  SIMPLEFOC_DEBUG("MOT: Align sensor.");
+
+  // if unknown natural direction
+  if(sensor_direction==Direction::UNKNOWN) {
+    // check if sensor needs zero search
+    if (sensor->needsSearch())
+      exit_flag = absoluteZeroSearch();
+    // stop init if not found index
+    if (!exit_flag)
+      return exit_flag;
+
+    // find natural direction
+    // move one electrical revolution forward
+    for (int i = 0; i <= 500; i++)
+    {
+      float angle = _3PI_2 + _2PI * i / 500.0f;
+      setPhaseVoltage(voltage_sensor_align, 0, angle);
+      sensor->update();
+      _delay(2);
+    }
+    // take and angle in the middle
+    sensor->update();
+    float mid_angle = sensor->getAngle();
+    // move one electrical revolution backwards
+    for (int i = 500; i >= 0; i--)
+    {
+      float angle = _3PI_2 + _2PI * i / 500.0f;
+      setPhaseVoltage(voltage_sensor_align, 0, angle);
+      sensor->update();
+      _delay(2);
+    }
+    sensor->update();
+    float end_angle = sensor->getAngle();
+    setPhaseVoltage(0, 0, 0);
+    _delay(200);
+    // determine the direction the sensor moved
+    if (mid_angle == end_angle)
+    {
+      SIMPLEFOC_DEBUG("MOT: Failed to notice movement");
+      return 0; // failed calibration
+    }
+    else if (mid_angle < end_angle)
+    {
+      SIMPLEFOC_DEBUG("MOT: sensor_direction==CCW");
+      sensor_direction = Direction::CCW;
+    }
+    else
+    {
+      SIMPLEFOC_DEBUG("MOT: sensor_direction==CW");
+      sensor_direction = Direction::CW;
+    }
+    // check pole pair number
+    float moved = fabs(mid_angle - end_angle);
+    if (fabs(moved * pole_pairs - _2PI) > 0.5f)
+    { // 0.5f is arbitrary number it can be lower or higher!
+      SIMPLEFOC_DEBUG("MOT: PP check: fail - estimated pp: ", _2PI / moved);
+    }
+    else
+      SIMPLEFOC_DEBUG("MOT: PP check: OK!");
+  }
+  else
+    SIMPLEFOC_DEBUG("MOT: Skip dir calib.");
+
+  // zero electric angle not known
+  if (!_isset(zero_electric_angle))
+  {
+    // align the electrical phases of the motor and sensor
+    // set angle -90(270 = 3PI/2) degrees
+    setPhaseVoltage(voltage_sensor_align, 0, _3PI_2);
+    _delay(700);
+    // read the sensor
+    sensor->update();
+    // get the current zero electric angle
+    zero_electric_angle = 0;
+    zero_electric_angle = electricalAngle();
+    _delay(20);
+    if (monitor_port)
+    {
+      SIMPLEFOC_DEBUG("MOT: Zero elec. angle: ", zero_electric_angle);
+    }
+    // stop everything
+    setPhaseVoltage(0, 0, 0);
+    _delay(200);
+  }
+  else
+    SIMPLEFOC_DEBUG("MOT: Skip offset calib.");
+  return exit_flag;
+}
+
+// Encoder alignment the absolute zero angle
+// - to the index
+int HybridStepperMotor::absoluteZeroSearch()
+{
+
+  SIMPLEFOC_DEBUG("MOT: Index search...");
+  // search the absolute zero with small velocity
+  float limit_vel = velocity_limit;
+  float limit_volt = voltage_limit;
+  velocity_limit = velocity_index_search;
+  voltage_limit = voltage_sensor_align;
+  shaft_angle = 0;
+  while (sensor->needsSearch() && shaft_angle < _2PI)
+  {
+    angleOpenloop(1.5f * _2PI);
+    // call important for some sensors not to loose count
+    // not needed for the search
+    sensor->update();
+  }
+  // disable motor
+  setPhaseVoltage(0, 0, 0);
+  // reinit the limits
+  velocity_limit = limit_vel;
+  voltage_limit = limit_volt;
+  // check if the zero found
+  if (monitor_port)
+  {
+    if (sensor->needsSearch())
+      SIMPLEFOC_DEBUG("MOT: Error: Not found!");
+    else
+      SIMPLEFOC_DEBUG("MOT: Success!");
+  }
+  return !sensor->needsSearch();
+}
+
+// Iterative function looping FOC algorithm, setting Uq on the Motor
+// The faster it can be run the better
+void HybridStepperMotor::loopFOC()
+{
+
+  // update 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;
+  // shaft angle
+  shaft_angle = shaftAngle();
+
+  // 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();
+
+  // set the phase voltage - FOC heart function :)
+  setPhaseVoltage(voltage.q, voltage.d, electrical_angle);
+}
+
+// Iterative function running outer loop of the FOC algorithm
+// Behavior of this function is determined by the motor.controller variable
+// It runs either angle, velocity or voltage loop
+// - needs to be called iteratively it is asynchronous function
+// - if target is not set it uses motor.target value
+void HybridStepperMotor::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 KV_rating available U_bemf = vel*(1/KV)
+  if (_isset(KV_rating))
+    voltage_bemf = shaft_velocity / KV_rating / _RPM_TO_RADS;
+  // 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
+  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);
+    break;
+  case MotionControlType::angle:
+    // 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
+    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);
+    break;
+  case MotionControlType::velocity:
+    // velocity set point
+    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);
+    break;
+  case MotionControlType::velocity_openloop:
+    // velocity control in open loop
+    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
+    break;
+  case MotionControlType::angle_openloop:
+    // angle control in open loop
+    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
+    break;
+  }
+}
+
+void HybridStepperMotor::setPhaseVoltage(float Uq, float Ud, float angle_el)
+{
+  angle_el = _normalizeAngle(angle_el);
+  float _ca = _cos(angle_el);
+  float _sa = _sin(angle_el);
+  float center;
+
+  switch (foc_modulation) {
+  case FOCModulationType::Trapezoid_120:
+  case FOCModulationType::Trapezoid_150:
+    // not handled
+    Ua = 0;
+    Ub = 0;
+    Uc = 0;
+    break;
+  case FOCModulationType::SinePWM:
+    // C phase is fixed at half-rail to provide bias point for A, B legs
+    Ua = (_ca * Ud) - (_sa * Uq);
+    Ub = (_sa * Ud) + (_ca * Uq);
+
+    center = driver->voltage_limit / 2;
+
+    Ua += center;
+    Ub += center;
+    Uc = center;
+    break;
+
+  case FOCModulationType::SpaceVectorPWM:
+    // C phase moves in order to increase max bias on coils
+    uint8_t sector = floor(4.0 * angle_el / _PI) + 1;
+    Ua = (_ca * Ud) - (_sa * Uq);
+    Ub = (_sa * Ud) + (_ca * Uq);
+
+    // Determine max/ min of [Ua, Ub, 0] based on phase angle.
+    switch (sector)
+    {
+    case 1:
+    case 8:
+      center = (driver->voltage_limit - Ua - Ub) / 2;
+      break;
+
+    case 2:
+      center = (driver->voltage_limit - Ua - 0) / 2;
+      break;
+
+    case 3:
+      center = (driver->voltage_limit - Ub - 0) / 2;
+      break;
+
+    case 4:
+    case 5:
+      center = (driver->voltage_limit - Ub - Ua) / 2;
+      break;
+
+    case 6:
+      center = (driver->voltage_limit - 0 - Ua) / 2;
+      break;
+
+    case 7:
+      center = (driver->voltage_limit - 0 - Ub) / 2;
+      break;
+
+    default: // this case does not occur, but compilers complain about uninitialized variables
+      center = (driver->voltage_limit - 0) / 2;
+      break;
+    }
+
+    Ua += center;
+    Ub += center;
+    Uc = center;
+    break;
+  }
+
+  driver->setPwm(Ua, Ub, Uc);
+}
+
+// Function (iterative) generating open loop movement for target velocity
+// - target_velocity - rad/s
+// it uses voltage_limit variable
+float HybridStepperMotor::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 + fabs(voltage_bemf), -voltage_limit, voltage_limit);
+    // recalculate the current
+    current.q = (Uq - fabs(voltage_bemf)) / phase_resistance;
+  }
+
+  // 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 HybridStepperMotor::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)
+  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 + fabs(voltage_bemf), -voltage_limit, voltage_limit);
+    // recalculate the current
+    current.q = (Uq - fabs(voltage_bemf)) / phase_resistance;
+  }
+  // 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;
+}

firmware/lib/Arduino-FOC-drivers/src/motors/HybridStepperMotor/HybridStepperMotor.h

@@ -0,0 +1,112 @@
+/**
+ *  @file HybridStepperMotor.h
+ *
+ */
+
+#ifndef HybridStepperMotor_h
+#define HybridStepperMotor_h
+
+#include "Arduino.h"
+#include "common/base_classes/FOCMotor.h"
+#include "common/base_classes/StepperDriver.h"
+#include "common/base_classes/Sensor.h"
+#include "common/foc_utils.h"
+#include "common/time_utils.h"
+#include "common/defaults.h"
+
+/**
+ Stepper Motor class
+*/
+class HybridStepperMotor : public FOCMotor
+{
+public:
+    /**
+      HybridStepperMotor class constructor
+      @param pp  pole pair number
+     @param R  motor phase resistance - [Ohm]
+     @param KV  motor KV rating (1/K_bemf) - rpm/V
+     @param L  motor phase inductance - [H]
+    */
+    HybridStepperMotor(int pp, float R = NOT_SET, float KV = NOT_SET, float L = NOT_SET);
+
+    /**
+     * Function linking a motor and a foc driver
+     *
+     * @param driver BLDCDriver handle for hardware peripheral control
+     */
+    void linkDriver(BLDCDriver *driver);
+
+    /**
+     * BLDCDriver link:
+     */
+    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() override;
+
+    /**
+     * Function running FOC algorithm in real-time
+     * it calculates the gets motor angle and sets the appropriate voltages
+     * to the phase pwm signals
+     * - the faster you can run it the better Arduino UNO ~1ms, Bluepill ~ 100us
+     */
+    void loopFOC() override;
+
+    /**
+     * Function executing the control loops set by the controller parameter of the HybridStepperMotor.
+     *
+     * @param target  Either voltage, angle or velocity based on the motor.controller
+     *                If it is not set the motor will use the target set in its variable motor.target
+     *
+     * This function doesn't need to be run upon each loop execution - depends of the use case
+     */
+    void move(float target = NOT_SET) override;
+
+    float Ua, Ub, Uc; //!< Phase voltages used for inverse Park and Clarke transform
+
+    /**
+     * Method using FOC to set Uq to the motor at the optimal angle
+     * Heart of the FOC algorithm
+     *
+     * @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) override;
+
+private:
+    /** Sensor alignment to electrical 0 angle of the motor */
+    int alignSensor();
+    /** 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

firmware/lib/Arduino-FOC-drivers/src/motors/HybridStepperMotor/README.md

@@ -0,0 +1,45 @@
+# Three Phase / Hybrid Stepper Motor Class
+
+By tying two phases together, you can use a standard bipolar stepper motor with three-phase drivers!
+Of course, it requires some different math for commutation. Another limitation is that with SinePWM the motor voltage is limited to 50% of the supply voltage. Using some tricks in SpaceVectorPWM, this max bias can be increased to about 70%.
+
+## Warning
+
+This code has not been tested much! 
+It has killed one B-G431B-ESC1 board before. 
+However, it has been succesfully used on a few different hardware platforms, so it does work.
+BEMF may be the danger here, so take caution with fast decelerations.
+Keep in mind that potentially twice the current can be sunk in the shorted phase as the two other legs.
+
+## Hardware setup
+
+Tie two of the phases of the motor together. This is phase "C". The other two phases are A & B. Phase C must go last in the driver constructor. 
+
+
+## Software setup
+
+Usage is quite simple: 
+
+```c++
+#include "Arduino.h"
+#include "SimpleFOC.h"
+#include "SimpleFOCDrivers.h"
+#include "motors/HybridStepperMotor/HybridStepperMotor.h"
+
+HybridStepperMotor motor = HybridStepperMotor(pole_pairs, phase_resistance, kv, winding_inductance);
+BLDCDriver3PWM driver = BLDCDriver3PWM(phase_A, phase_B, phase_C, enable);
+
+void setup() {
+    driver.init();
+    motor.linkDriver(&driver);
+
+    motor.foc_modulation = FOCModulationType::SpaceVectorPWM; //or SinePWM
+    motor.init();
+    motor.initFOC();
+}
+
+void loop(){
+    motor.loopFOC();
+    motor.move();
+}
+```