FOCMotor.cpp

Go to the documentation of this file.

#include "FOCMotor.h"
 
FOCMotor::FOCMotor()
{
  // maximum angular velocity to be used for positioning 
  velocity_limit = DEF_VEL_LIM;
  // maximum voltage to be set to the motor
  voltage_limit = DEF_POWER_SUPPLY;
  // not set on the begining
  current_limit = DEF_CURRENT_LIM;
 
  // index search velocity
  velocity_index_search = DEF_INDEX_SEARCH_TARGET_VELOCITY;
  // sensor and motor align voltage
  voltage_sensor_align = DEF_VOLTAGE_SENSOR_ALIGN;
 
  // default modulation is SinePWM
  foc_modulation = FOCModulationType::SinePWM;
 
  // default target value
  target = 0;
  voltage.d = 0;
  voltage.q = 0;
  // current target values
  current_sp = 0;
  current.q = 0;
  current.d = 0;
  
  //monitor_port 
  monitor_port = nullptr;
  //sensor 
  sensor = nullptr;
  //current sensor 
  current_sense = nullptr;
}
 
 
void FOCMotor::linkSensor(Sensor* _sensor) {
  sensor = _sensor;
}
 
void FOCMotor::linkCurrentSense(CurrentSense* _current_sense) {
  current_sense = _current_sense;
}
 
// shaft angle calculation
float FOCMotor::shaftAngle() {
  // if no sensor linked return previous value ( for open loop )
  if(!sensor) return shaft_angle;
  return sensor_direction*LPF_angle(sensor->getAngle()) - sensor_offset;
}
// shaft velocity calculation
float FOCMotor::shaftVelocity() {
  // if no sensor linked return previous value ( for open loop )
  if(!sensor) return shaft_velocity;
  return sensor_direction*LPF_velocity(sensor->getVelocity());
}
 
float FOCMotor::electricalAngle(){
  return _normalizeAngle((shaft_angle + sensor_offset) * pole_pairs - zero_electric_angle);
}
 
// function implementing the monitor_port setter
void FOCMotor::useMonitoring(Print &print){
  monitor_port = &print; //operate on the address of print
  if(monitor_port ) monitor_port->println(F("MOT: Monitor enabled!"));
}
 
// utility function intended to be used with serial plotter to monitor motor variables
// significantly slowing the execution down!!!!
void FOCMotor::monitor() {
  if( !monitor_downsample || monitor_cnt++ < monitor_downsample ) return;
  monitor_cnt = 0;
  if(!monitor_port) return;
  bool printed = 0;
 
  if(monitor_variables & _MON_TARGET){
    monitor_port->print(target,4);
    monitor_port->print("\t");
    printed= true;
  }
  if(monitor_variables & _MON_VOLT_Q) {
    monitor_port->print(voltage.q,4);
    monitor_port->print("\t");
    printed= true;
  }
  if(monitor_variables & _MON_VOLT_D) {
    monitor_port->print(voltage.d,4);
    monitor_port->print("\t");
    printed= true;
  }
  // read currents if possible - even in voltage mode (if current_sense available)
  if(monitor_variables & _MON_CURR_Q || monitor_variables & _MON_CURR_D) {
    DQCurrent_s c{0,0};
    if(current_sense){
      if(torque_controller == TorqueControlType::foc_current) c = current;
      else c = current_sense->getFOCCurrents(electrical_angle);
    }
    if(monitor_variables & _MON_CURR_Q) {
      monitor_port->print(c.q*1000,2); // mAmps
      monitor_port->print("\t");
      printed= true;
    }
    if(monitor_variables & _MON_CURR_D) {
      monitor_port->print(c.d*1000,2); // mAmps
      monitor_port->print("\t");
      printed= true;
    }
  }
 
  if(monitor_variables & _MON_VEL) {
    monitor_port->print(shaft_velocity,4);
    monitor_port->print("\t");
    printed= true;
  }
  if(monitor_variables & _MON_ANGLE) {
    monitor_port->print(shaft_angle,4);
    printed= true;
  }
  if(printed) monitor_port->println();
  
}