737 lines
27 KiB
C++
737 lines
27 KiB
C++
#include "BLDCMotor.h"
|
|
#include "./communication/SimpleFOCDebug.h"
|
|
|
|
|
|
// see https://www.youtube.com/watch?v=InzXA7mWBWE Slide 5
|
|
// each is 60 degrees with values for 3 phases of 1=positive -1=negative 0=high-z
|
|
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}
|
|
};
|
|
|
|
// see https://www.youtube.com/watch?v=InzXA7mWBWE Slide 8
|
|
// each is 30 degrees with values for 3 phases of 1=positive -1=negative 0=high-z
|
|
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}
|
|
};
|
|
|
|
// BLDCMotor( int pp , float R)
|
|
// - pp - pole pair number
|
|
// - R - motor phase resistance
|
|
// - KV - motor kv rating (rmp/v)
|
|
// - L - motor phase inductance
|
|
BLDCMotor::BLDCMotor(int pp, float _R, float _KV, float _inductance)
|
|
: FOCMotor()
|
|
{
|
|
// save pole pairs number
|
|
pole_pairs = pp;
|
|
// save phase resistance number
|
|
phase_resistance = _R;
|
|
// save back emf constant KV = 1/KV
|
|
// 1/sqrt(2) - rms value
|
|
KV_rating = NOT_SET;
|
|
if (_isset(_KV))
|
|
KV_rating = _KV*_SQRT2;
|
|
// save phase inductance
|
|
phase_inductance = _inductance;
|
|
|
|
// 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;
|
|
|
|
// if using open loop control, set a CW as the default direction if not already set
|
|
if ((controller==MotionControlType::angle_openloop
|
|
||controller==MotionControlType::velocity_openloop)
|
|
&& (sensor_direction == Direction::UNKNOWN)) {
|
|
sensor_direction = Direction::CW;
|
|
}
|
|
|
|
_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() {
|
|
int exit_flag = 1;
|
|
|
|
motor_status = FOCMotorStatus::motor_calibrating;
|
|
|
|
// align motor if necessary
|
|
// 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 = shaftAngle();
|
|
}else {
|
|
exit_flag = 0; // no FOC without sensor
|
|
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){
|
|
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."); }
|
|
}
|
|
|
|
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(sensor_direction==Direction::UNKNOWN){
|
|
|
|
// 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
|
|
float moved = fabs(mid_angle - end_angle);
|
|
if (moved<MIN_ANGLE_DETECT_MOVEMENT) { // minimum angle to detect movement
|
|
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
|
|
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();
|
|
//zero_electric_angle = _normalizeAngle(_electricalAngle(sensor_direction*sensor->getAngle(), pole_pairs));
|
|
_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 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();
|
|
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);
|
|
}
|
|
|
|
// 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 TODO the velocity reading probably also shouldn't happen in open loop modes?
|
|
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;
|
|
|
|
// 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);
|
|
// 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_angle_sp = _constrain(shaft_angle_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
|
|
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 - 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);
|
|
// 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 - 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 usage)
|
|
// 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
|
|
// determine the sector
|
|
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.voltage_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(PhaseState::PHASE_OFF, PhaseState::PHASE_ON, PhaseState::PHASE_ON); // 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(PhaseState::PHASE_ON, PhaseState::PHASE_OFF, PhaseState::PHASE_ON);// 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(PhaseState::PHASE_ON, PhaseState::PHASE_ON, PhaseState::PHASE_OFF);// disable phase if possible
|
|
}
|
|
|
|
break;
|
|
|
|
case FOCModulationType::Trapezoid_150 :
|
|
// see https://www.youtube.com/watch?v=InzXA7mWBWE Slide 8
|
|
// determine the sector
|
|
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.voltage_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(PhaseState::PHASE_OFF, PhaseState::PHASE_ON, PhaseState::PHASE_ON); // 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(PhaseState::PHASE_ON, PhaseState::PHASE_OFF, PhaseState::PHASE_ON); // disable phase if possible
|
|
}else if(trap_150_map[sector][2] == _HIGH_IMPEDANCE){
|
|
Ua = trap_150_map[sector][0] * Uq + center;
|
|
Ub = trap_150_map[sector][1] * Uq + center;
|
|
Uc = center;
|
|
driver->setPhaseState(PhaseState::PHASE_ON, PhaseState::PHASE_ON, PhaseState::PHASE_OFF); // disable phase if possible
|
|
}else{
|
|
Ua = trap_150_map[sector][0] * Uq + center;
|
|
Ub = trap_150_map[sector][1] * Uq + center;
|
|
Uc = trap_150_map[sector][2] * Uq + center;
|
|
driver->setPhaseState(PhaseState::PHASE_ON, PhaseState::PHASE_ON, PhaseState::PHASE_ON); // enable all phases
|
|
}
|
|
|
|
break;
|
|
|
|
case FOCModulationType::SinePWM :
|
|
// Sinusoidal PWM modulation
|
|
// Inverse Park + Clarke transformation
|
|
_sincos(angle_el, &_sa, &_ca);
|
|
|
|
// 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 + 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 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 + 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
|
|
// 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;
|
|
}
|