Skip to content

Commit

Permalink
Merge pull request #422 from simplefoc/feat_new_cs_align
Browse files Browse the repository at this point in the history
Feat new cs align
  • Loading branch information
askuric authored Jul 20, 2024
2 parents 31ed492 + dbc62a1 commit 996f312
Show file tree
Hide file tree
Showing 25 changed files with 770 additions and 600 deletions.
17 changes: 4 additions & 13 deletions README.md
Original file line number Diff line number Diff line change
Expand Up @@ -13,7 +13,8 @@
![GitHub commits since tagged version](https://img.shields.io/github/commits-since/simplefoc/arduino-foc/latest/dev)
![GitHub commit activity (branch)](https://img.shields.io/github/commit-activity/m/simplefoc/arduino-foc/dev)

[![arduino-library-badge](https://www.ardu-badge.com/badge/Simple%20FOC.svg?)](https://www.ardu-badge.com/badge/Simple%20FOC.svg)
[![arduino-library-badge](https://ardubadge.simplefoc.com?lib=Simple%20FOC)](https://www.ardu-badge.com/badge/Simple%20FOC.svg)
[![PlatformIO Registry](https://badges.registry.platformio.org/packages/askuric/library/Simple%20FOC.svg)](https://registry.platformio.org/libraries/askuric/Simple%20FOC)
[![License: MIT](https://img.shields.io/badge/License-MIT-yellow.svg)](https://opensource.org/licenses/MIT)
[![status](https://joss.theoj.org/papers/4382445f249e064e9f0a7f6c1bb06b1d/status.svg)](https://joss.theoj.org/papers/4382445f249e064e9f0a7f6c1bb06b1d)

Expand All @@ -28,18 +29,8 @@ Therefore this is an attempt to:
- For official driver boards see [<span class="simple">Simple<span class="foc">FOC</span>Boards</span>](https://docs.simplefoc.com/boards)
- Many many more boards developed by the community members, see [<span class="simple">Simple<span class="foc">FOC</span>Community</span>](https://community.simplefoc.com/)

> NEW RELEASE 📢 : <span class="simple">Simple<span class="foc">FOC</span>library</span> v2.3.3
> - Teensy4
> - support for low-side current sensing [#392](https://github.com/simplefoc/Arduino-FOC/pull/392)
> - support for center aligned 6pwm and 3pwm (optional) [#392](https://github.com/simplefoc/Arduino-FOC/pull/392)
> - stm32
> - support for center aligned pwm (even across multiple timers and motors/drivers) [#374](https://github.com/simplefoc/Arduino-FOC/pull/374), [#388](https://github.com/simplefoc/Arduino-FOC/pull/388)
> - support for DMA based low-side current sensing: [#383](https://github.com/simplefoc/Arduino-FOC/pull/383),[#378](https://github.com/simplefoc/Arduino-FOC/pull/378)
> - support for f7 architecture [#388](https://github.com/simplefoc/Arduino-FOC/pull/388),[#394](https://github.com/simplefoc/Arduino-FOC/pull/394)
> - KV rating calculation fix [#347](https://github.com/simplefoc/Arduino-FOC/pull/347)
> - Much more performant Space Vector PWM calculation [#340](https://github.com/simplefoc/Arduino-FOC/pull/340)
> - And much more:
> - See the complete list of bugfixes and new features of v2.3.3 [fixes and PRs](https://github.com/simplefoc/Arduino-FOC/milestone/10?closed=1)
> NEXT RELEASE 📢 : <span class="simple">Simple<span class="foc">FOC</span>library</span> v2.3.4
> - Current sensing support for Stepper motors (lowside and inline)

## Arduino *SimpleFOClibrary* v2.3.3
Expand Down
Original file line number Diff line number Diff line change
@@ -0,0 +1,93 @@
/**
*
* SimpleFOCMini motor control example
*
* For Arduino UNO or the other boards with the UNO headers
* the most convenient way to use the board is to stack it to the pins:
* - 12 - ENABLE
* - 11 - IN1
* - 10 - IN2
* - 9 - IN3
*
*/
#include <SimpleFOC.h>


// BLDC motor & driver instance
BLDCMotor motor = BLDCMotor(11);
// BLDCDriver3PWM driver = BLDCDriver3PWM(11, 10, 9, 8); // mini v1.0
BLDCDriver3PWM driver = BLDCDriver3PWM(9, 10, 11, 12); // mini v1.1

// instantiate the commander
Commander command = Commander(Serial);
void doMotor(char* cmd) { command.motor(&motor, cmd); }

void setup() {
// use monitoring with serial
Serial.begin(115200);
// enable more verbose output for debugging
// comment out if not needed
SimpleFOCDebug::enable(&Serial);

// if SimpleFOCMini is stacked in arduino headers
// on pins 12,11,10,9,8
// pin 12 is used as ground
pinMode(12,OUTPUT);
pinMode(12,LOW);

// driver config
// power supply voltage [V]
driver.voltage_power_supply = 12;
driver.init();
// link the motor and the driver
motor.linkDriver(&driver);

// aligning voltage [V]
motor.voltage_sensor_align = 3;

// set motion control loop to be used
motor.controller = MotionControlType::velocity_openloop;

// default voltage_power_supply
motor.voltage_limit = 2; // Volts

// comment out if not needed
motor.useMonitoring(Serial);

// initialize motor
motor.init();
// align encoder and start FOC
motor.initFOC();

// add target command M
command.add('M', doMotor, "motor");

Serial.println(F("Motor ready."));
Serial.println(F("Set the target velocity using serial terminal:"));

motor.target = 1; //initial target velocity 1 rad/s
Serial.println("Target velocity: 1 rad/s");
Serial.println("Voltage limit 2V");
_delay(1000);
}

void loop() {
// main FOC algorithm function
// the faster you run this function the better
// Arduino UNO loop ~1kHz
// Bluepill loop ~10kHz
motor.loopFOC();

// Motion control function
// velocity, position or voltage (defined in motor.controller)
// this function can be run at much lower frequency than loopFOC() function
// You can also use motor.move() and set the motor.target in the code
motor.move();

// function intended to be used with serial plotter to monitor motor variables
// significantly slowing the execution down!!!!
// motor.monitor();

// user communication
command.run();
}
2 changes: 1 addition & 1 deletion src/BLDCMotor.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -202,7 +202,7 @@ int BLDCMotor::alignCurrentSense() {
SIMPLEFOC_DEBUG("MOT: Align current sense.");

// align current sense and the driver
exit_flag = current_sense->driverAlign(voltage_sensor_align);
exit_flag = current_sense->driverAlign(voltage_sensor_align, modulation_centered);
if(!exit_flag){
// error in current sense - phase either not measured or bad connection
SIMPLEFOC_DEBUG("MOT: Align error!");
Expand Down
1 change: 1 addition & 0 deletions src/BLDCMotor.h
Original file line number Diff line number Diff line change
Expand Up @@ -4,6 +4,7 @@
#include "Arduino.h"
#include "common/base_classes/FOCMotor.h"
#include "common/base_classes/Sensor.h"
#include "common/base_classes/FOCDriver.h"
#include "common/base_classes/BLDCDriver.h"
#include "common/foc_utils.h"
#include "common/time_utils.h"
Expand Down
143 changes: 112 additions & 31 deletions src/StepperMotor.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -108,12 +108,28 @@ int StepperMotor::initFOC() {
// 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 = sensor->getAngle();
shaft_angle = shaftAngle();

// aligning the current sensor - can be skipped
// checks if driver phases are the same as current sense phases
// and checks the direction of measuremnt.
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."); }
}

} else {
SIMPLEFOC_DEBUG("MOT: No sensor.");
if ((controller == MotionControlType::angle_openloop || controller == MotionControlType::velocity_openloop)){
Expand All @@ -136,6 +152,26 @@ int StepperMotor::initFOC() {
return exit_flag;
}

// Calibrate the motor and current sense phases
int StepperMotor::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, modulation_centered);
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 StepperMotor::alignSensor() {
int exit_flag = 1; //success
Expand Down Expand Up @@ -261,8 +297,6 @@ void StepperMotor::loopFOC() {

// 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;
Expand All @@ -271,7 +305,40 @@ void StepperMotor::loopFOC() {
// 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);
}
Expand Down Expand Up @@ -310,56 +377,70 @@ void StepperMotor::move(float new_target) {
// 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
// upgrade the current based voltage limit
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);
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_velocity_sp = _constrain(shaft_velocity_sp, -velocity_limit, velocity_limit);
// calculate the torque command
shaft_velocity_sp = _constrain(shaft_velocity_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
// 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);
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
// 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
// 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);
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
// 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; // TODO d-component lag-compensation
voltage.d = 0;
break;
case MotionControlType::angle_openloop:
// angle control in open loop
// 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; // TODO d-component lag-compensation
voltage.d = 0;
break;
}
}
Expand Down
2 changes: 2 additions & 0 deletions src/StepperMotor.h
Original file line number Diff line number Diff line change
Expand Up @@ -89,6 +89,8 @@ class StepperMotor: public FOCMotor
int alignSensor();
/** Motor and sensor alignment to the sensors absolute 0 angle */
int absoluteZeroSearch();
/** Current sense and motor phase alignment */
int alignCurrentSense();

// Open loop motion control
/**
Expand Down
Loading

0 comments on commit 996f312

Please sign in to comment.