From 9aee1aa6a10efe3526b2c3e85273fe372feafc01 Mon Sep 17 00:00:00 2001 From: askuric Date: Mon, 15 Jul 2024 13:21:20 +0200 Subject: [PATCH] removed accidentally added files --- src/BLDCMotor.cpp.old | 732 ------------------------------------------ src/BLDCMotor.h.old | 121 ------- src/SimpleFOC_empty.h | 119 ------- 3 files changed, 972 deletions(-) delete mode 100644 src/BLDCMotor.cpp.old delete mode 100644 src/BLDCMotor.h.old delete mode 100644 src/SimpleFOC_empty.h diff --git a/src/BLDCMotor.cpp.old b/src/BLDCMotor.cpp.old deleted file mode 100644 index 6b996342..00000000 --- a/src/BLDCMotor.cpp.old +++ /dev/null @@ -1,732 +0,0 @@ -#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.old b/src/BLDCMotor.h.old deleted file mode 100644 index 04c54b92..00000000 --- a/src/BLDCMotor.h.old +++ /dev/null @@ -1,121 +0,0 @@ -#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 deleted file mode 100644 index bf0049dd..00000000 --- a/src/SimpleFOC_empty.h +++ /dev/null @@ -1,119 +0,0 @@ -/*! - * @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