text: #include "BLDCMotor.h"
  
  // BLDCMotor( int pp , float R)
  // - pp            - pole pair number
  // - R             - motor phase resistance
  BLDCMotor::BLDCMotor(int pp, float _R)
  : FOCMotor()
  {
    // save pole pairs number
    pole_pairs = pp;
    // save phase resistance number
    phase_resistance = _R;
    // 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(monitor_port) monitor_port->println(F("MOT: Init"));
  
    // if no current sensing and the user has set the phase resistance of the motor use current limit to calculate the voltage limit
    if( !current_sense && _isset(phase_resistance)) {
      float new_voltage_limit = current_limit * (phase_resistance); // v_lim = current_lim / (3/2 phase resistance) - worst case
      // use it if it is less then voltage_limit set by the user
      voltage_limit = new_voltage_limit < voltage_limit ? new_voltage_limit : voltage_limit;
    }
    // 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
    if(monitor_port) monitor_port->println(F("MOT: Enable driver."));
    enable();
    _delay(500);
  }
  
  
  // 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;
    // 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 if(monitor_port) monitor_port->println(F("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 if(monitor_port) monitor_port->println(F("MOT: No current sense."));
    }
  
    if(exit_flag){
      if(monitor_port) monitor_port->println(F("MOT: Ready."));
    }else{
      if(monitor_port) monitor_port->println(F("MOT: Init FOC failed."));
      disable();
    }
  
    return exit_flag;
  }
  
  // Calibarthe the motor and current sense phases
  int BLDCMotor::alignCurrentSense() {
    int exit_flag = 1; // success
  
    if(monitor_port) monitor_port->println(F("MOT: Align current sense."));
  
    // align current sense and the driver
    exit_flag = current_sense->driverAlign(driver, voltage_sensor_align);
    if(!exit_flag){
      // error in current sense - phase either not measured or bad connection
      if(monitor_port) monitor_port->println(F("MOT: Align error!"));
      exit_flag = 0;
    }else{
      // output the alignment status flag
      if(monitor_port) monitor_port->print(F("MOT: Success: "));
      if(monitor_port) monitor_port->println(exit_flag);
    }
  
    return exit_flag > 0;
  }
  
  // Encoder alignment to electrical 0 angle
  int BLDCMotor::alignSensor() {
    int exit_flag = 1; //success
    if(monitor_port) monitor_port->println(F("MOT: Align sensor."));
  
    // if unknown natural direction
    if(!_isset(sensor_direction)){
      // 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);
        _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) {
        if(monitor_port) monitor_port->println(F("MOT: Failed to notice movement"));
        return 0; // failed calibration
      } else if (mid_angle < end_angle) {
        if(monitor_port) monitor_port->println(F("MOT: sensor_direction==CCW"));
        sensor_direction = Direction::CCW;
      } else{
        if(monitor_port) monitor_port->println(F("MOT: sensor_direction==CW"));
        sensor_direction = Direction::CW;
      }
      // check pole pair number
      if(monitor_port) monitor_port->print(F("MOT: PP check: "));
      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!
        if(monitor_port) monitor_port->print(F("fail - estimated pp:"));
        if(monitor_port) monitor_port->println(_2PI/moved,4);
      }else if(monitor_port) monitor_port->println(F("OK!"));
  
    }else if(monitor_port) monitor_port->println(F("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();
      //zero_electric_angle =  _normalizeAngle(_electricalAngle(sensor_direction*sensor->getAngle(), pole_pairs));
      _delay(20);
      if(monitor_port){
        monitor_port->print(F("MOT: Zero elec. angle: "));
        monitor_port->println(zero_electric_angle);
      }
      // stop everything
      setPhaseVoltage(0, 0, 0);
      _delay(200);
    }else if(monitor_port) monitor_port->println(F("MOT: Skip offset calib."));
    return exit_flag;
  }
  
  // Encoder alignment the absolute zero angle
  // - to the index
  int BLDCMotor::absoluteZeroSearch() {
    // sensor precision: this is all ok, as the search happens near the 0-angle, where the precision
    //                    of float is sufficient.
    if(monitor_port) monitor_port->println(F("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()) monitor_port->println(F("MOT: Error: Not found!"));
      else monitor_port->println(F("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();
    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);
        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);
        break;
      default:
        // no torque control selected
        if(monitor_port) monitor_port->println(F("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;
  
    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.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 = current_sp*phase_resistance;
          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 = current_sp*phase_resistance;
          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 =  current_limit*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 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 =  current_limit*phase_resistance;
    // 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;
  }
  