Skip to content

Commit 1aeb15a

Browse files
askuricaskuric
committed
final updates
1 parent 20baaec commit 1aeb15a

9 files changed

Lines changed: 105 additions & 99 deletions

File tree

‎examples/hardware_specific_examples/DRV8302_driver/esp32_current_control_low_side/esp32_current_control_low_side.ino‎

Lines changed: 0 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -103,8 +103,6 @@ void setup() {
103103
motor.LPF_velocity.Tf = 0.02;
104104
// angle loop PID
105105
motor.P_angle.P = 20.0;
106-
// Low pass filtering time constant
107-
motor.LPF_angle.Tf = 0.0;
108106
// current q loop PID
109107
motor.PID_current_q.P = 3.0;
110108
motor.PID_current_q.I = 100.0;

‎examples/hardware_specific_examples/DRV8302_driver/stm32_current_control_low_side/stm32_current_control_low_side.ino‎

Lines changed: 0 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -103,8 +103,6 @@ void setup() {
103103
motor.LPF_velocity.Tf = 0.02;
104104
// angle loop PID
105105
motor.P_angle.P = 20.0;
106-
// Low pass filtering time constant
107-
motor.LPF_angle.Tf = 0.0;
108106
// current q loop PID
109107
motor.PID_current_q.P = 3.0;
110108
motor.PID_current_q.I = 100.0;

‎examples/hardware_specific_examples/DRV8302_driver/teensy4_current_control_low_side/teensy4_current_control_low_side.ino‎

Lines changed: 0 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -104,8 +104,6 @@ void setup() {
104104
motor.LPF_velocity.Tf = 0.02;
105105
// angle loop PID
106106
motor.P_angle.P = 20.0;
107-
// Low pass filtering time constant
108-
motor.LPF_angle.Tf = 0.0;
109107
// current q loop PID
110108
motor.PID_current_q.P = 3.0;
111109
motor.PID_current_q.I = 100.0;

‎examples/motion_control/custom_motion_control/custom_motion_control.ino‎

Lines changed: 3 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -1,7 +1,6 @@
11

22
#include <Arduino.h>
33
#include <SimpleFOC.h>
4-
#include "current_sense/hardware_specific/stm32/stm32_mcu.h"
54

65

76
// BLDC motor & driver instance
@@ -25,9 +24,9 @@ void doMotor(char* cmd){ command.motor(&motor, cmd); }
2524
// P controller with gain of 1.0f, no integral or derivative gain
2625
PIDController custom_PID = PIDController(1.0f, 0, 0);
2726
// custom motion control method
28-
float positionPControl(FOCMotor* motor, float target){
27+
float positionPControl(FOCMotor* motor){
2928
// simple proportional position control
30-
float error = target - motor->shaft_angle;
29+
float error = motor->target - motor->shaft_angle;
3130
// set the PID output limit to the motor current limit
3231
custom_PID.limit = motor->current_limit;
3332
return custom_PID(error); // return current command based on the error
@@ -60,7 +59,7 @@ void setup() {
6059
motor.linkCustomMotionControl(positionPControl);
6160
// set control loop type to be used
6261
motor.controller = MotionControlType::custom;
63-
// set the torque control type to voltage control (default is voltage control)
62+
// set the torque control type to current control (default is voltage control)
6463
motor.torque_controller = TorqueControlType::foc_current;
6564

6665
// comment out if not needed

‎src/common/base_classes/FOCMotor.cpp‎

Lines changed: 8 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -65,7 +65,7 @@ void FOCMotor::linkCurrentSense(CurrentSense* _current_sense) {
6565
float FOCMotor::shaftAngle() {
6666
// if no sensor linked return previous value ( for open loop )
6767
if(!sensor) return shaft_angle;
68-
return sensor_direction*LPF_angle(sensor->getAngle()) - sensor_offset;
68+
return sensor_direction*sensor->getAngle() - sensor_offset;
6969
}
7070
// shaft velocity calculation
7171
float FOCMotor::shaftVelocity() {
@@ -644,12 +644,15 @@ void FOCMotor::loopFOC() {
644644
current.q = LPF_current_q(current.q);
645645
current.d = LPF_current_d(current.d);
646646
// calculate the phase voltages
647-
voltage.q = PID_current_q(current_sp - current.q) + feed_forward_voltage.q;
648-
voltage.d = PID_current_d(feed_forward_current.d - current.d) + feed_forward_voltage.d;
647+
voltage.q = PID_current_q(current_sp - current.q);
648+
voltage.d = PID_current_d(feed_forward_current.d - current.d);
649649
// d voltage - lag compensation
650650
if(_isset(axis_inductance.q)) voltage.d = _constrain( voltage.d - current_sp*shaft_velocity*pole_pairs*axis_inductance.q, -voltage_limit, voltage_limit);
651651
// q voltage - cross coupling compensation - TODO verify
652652
if(_isset(axis_inductance.d)) voltage.q = _constrain( voltage.q + current.d*shaft_velocity*pole_pairs*axis_inductance.d, -voltage_limit, voltage_limit);
653+
// add feed forward
654+
voltage.q += feed_forward_voltage.q;
655+
voltage.d += feed_forward_voltage.d;
653656
break;
654657
default:
655658
// no torque control selected
@@ -712,7 +715,7 @@ void FOCMotor::move(float new_target) {
712715
// angle set point
713716
shaft_angle_sp = target;
714717
// calculate the torque command - sensor precision: this calculation is ok, but based on bad value from previous calculation
715-
current_sp = P_angle(shaft_angle_sp - shaft_angle);
718+
current_sp = P_angle(shaft_angle_sp - LPF_angle(shaft_angle));
716719
break;
717720
case MotionControlType::angle:
718721
// TODO sensor precision: this calculation is not numerically precise. The target value cannot express precise positions when
@@ -721,7 +724,7 @@ void FOCMotor::move(float new_target) {
721724
// angle set point
722725
shaft_angle_sp = target;
723726
// calculate velocity set point
724-
shaft_velocity_sp = feed_forward_velocity + P_angle( shaft_angle_sp - shaft_angle );
727+
shaft_velocity_sp = feed_forward_velocity + P_angle( shaft_angle_sp - LPF_angle(shaft_angle) );
725728
shaft_velocity_sp = _constrain(shaft_velocity_sp, -velocity_limit, velocity_limit);
726729
// calculate the torque command - sensor precision: this calculation is ok, but based on bad value from previous calculation
727730
current_sp = PID_velocity(shaft_velocity_sp - shaft_velocity);

‎src/common/base_classes/FOCMotor.h‎

Lines changed: 9 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -11,17 +11,21 @@
1111
#include "../pid.h"
1212
#include "../lowpass_filter.h"
1313

14+
#define MOT_ERR "ERR-MOT:"
15+
#define MOT_WARN "WARN-MOT:"
16+
#define MOT_DEBUG "MOT:"
17+
1418
#ifndef SIMPLEFOC_DISABLE_DEBUG
1519
#define SIMPLEFOC_MOTOR_WARN(msg, ...) \
16-
SimpleFOCDebug::print("WARN-MOT: "); \
20+
SimpleFOCDebug::print(MOT_WARN); \
1721
SIMPLEFOC_DEBUG(msg, ##__VA_ARGS__)
1822

1923
#define SIMPLEFOC_MOTOR_ERROR(msg, ...) \
20-
SimpleFOCDebug::print("ERR-MOT: "); \
24+
SimpleFOCDebug::print(MOT_ERR); \
2125
SIMPLEFOC_DEBUG(msg, ##__VA_ARGS__)
2226

2327
#define SIMPLEFOC_MOTOR_DEBUG(msg, ...) \
24-
SimpleFOCDebug::print("MOT: "); \
28+
SimpleFOCDebug::print(MOT_DEBUG); \
2529
SIMPLEFOC_DEBUG(msg, ##__VA_ARGS__)
2630

2731
#else
@@ -371,10 +375,10 @@ class FOCMotor
371375

372376
/**
373377
* Function setting a custom motion control method defined by the user
374-
* @note the custom control method has to be defined by the user and should follow the signature: float controlMethod(FOCMotor* motor, float target)
378+
* @note the custom control method has to be defined by the user and should follow the signature: float controlMethod(FOCMotor* motor)
375379
* @param controlMethod - pointer to the custom control method function defined by the user
376380
*/
377-
void linkCustomMotionControl(float (*controlMethod)(FOCMotor* motor, float target)){
381+
void linkCustomMotionControl(float (*controlMethod)(FOCMotor* motor)){
378382
customMotionControlCallback = controlMethod;
379383
}
380384

‎src/common/defaults.h‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -6,7 +6,7 @@
66
#define DEF_PID_VEL_P 0.5f //!< default PID controller P value
77
#define DEF_PID_VEL_I 10.0f //!< default PID controller I value
88
#define DEF_PID_VEL_D 0.0f //!< default PID controller D value
9-
#define DEF_PID_VEL_RAMP 1000.0f //!< default PID controller voltage ramp value
9+
#define DEF_PID_VEL_RAMP NOT_SET //!< default PID controller voltage ramp value
1010
#define DEF_PID_VEL_LIMIT (DEF_POWER_SUPPLY) //!< default PID controller voltage limit
1111

1212
// current sensing PID values

‎src/communication/Commander.cpp‎

Lines changed: 29 additions & 25 deletions
Original file line numberDiff line numberDiff line change
@@ -281,16 +281,8 @@ void Commander::motor(FOCMotor* motor, char* user_command) {
281281
}
282282
break;
283283
case CMD_FOC_PARAMS:
284-
printVerbose(F("FOC | "));
284+
printVerbose(F("MOT | "));
285285
switch (sub_cmd){
286-
case SCMD_LOOPFOC_TIME: // loopFOC execution time
287-
printVerbose(F("loop time: "));
288-
println((int)motor->loopfoc_time_us);
289-
break;
290-
case SCMD_MOVE_TIME: // loopFOC execution time
291-
printVerbose(F("move time: "));
292-
println((int)motor->move_time_us);
293-
break;
294286
case SCMD_REINIT_FOC:
295287
printVerbose(F("Reinit!"));
296288
motor->sensor_direction = Direction::UNKNOWN; // reset sensor direction estimation
@@ -432,6 +424,10 @@ void Commander::motion(FOCMotor* motor, char* user_cmd, char* separator){
432424
case CMD_MOTION_TYPE:
433425
printVerbose(F("Motion:"));
434426
switch(sub_cmd){
427+
case SCMD_TIME:
428+
printVerbose(F(" time: "));
429+
println((int)motor->move_time_us);
430+
break;
435431
case SCMD_DOWNSAMPLE:
436432
printVerbose(F(" downsample: "));
437433
if(!GET) motor->motion_downsample = value;
@@ -470,24 +466,32 @@ void Commander::motion(FOCMotor* motor, char* user_cmd, char* separator){
470466
case CMD_TORQUE_TYPE:
471467
// change control type
472468
printVerbose(F("Torque: "));
473-
if(!GET && (int8_t)value >= 0 && (int8_t)value < 4) // if set command
474-
motor->updateTorqueControlType((TorqueControlType)value); // update torque control type
475-
switch(motor->torque_controller){
476-
case TorqueControlType::voltage:
477-
println(F("volt"));
478-
break;
479-
case TorqueControlType::dc_current:
480-
println(F("dc curr"));
481-
break;
482-
case TorqueControlType::foc_current:
483-
println(F("foc curr"));
484-
break;
485-
case TorqueControlType::estimated_current:
486-
println(F("est. curr"));
469+
switch(sub_cmd){
470+
case SCMD_TIME:
471+
printVerbose(F(" time: "));
472+
println((int)motor->move_time_us);
487473
break;
488474
default:
489-
printError();
490-
}
475+
if(!GET && (int8_t)value >= 0 && (int8_t)value < 4) // if set command
476+
motor->updateTorqueControlType((TorqueControlType)value); // update torque control type
477+
switch(motor->torque_controller){
478+
case TorqueControlType::voltage:
479+
println(F("volt"));
480+
break;
481+
case TorqueControlType::dc_current:
482+
println(F("dc curr"));
483+
break;
484+
case TorqueControlType::foc_current:
485+
println(F("foc curr"));
486+
break;
487+
case TorqueControlType::estimated_current:
488+
println(F("est. curr"));
489+
break;
490+
default:
491+
printError();
492+
}
493+
break;
494+
}
491495
break;
492496
case CMD_STATUS:
493497
// enable/disable

‎src/communication/commands.h‎

Lines changed: 55 additions & 53 deletions
Original file line numberDiff line numberDiff line change
@@ -4,58 +4,60 @@
44
// see docs.simplefoc.com for in depth explanation of the commands
55

66
// list of commands
7-
#define CMD_C_D_PID 'D' //!< current d PID & LPF
8-
#define CMD_C_Q_PID 'Q' //!< current d PID & LPF
9-
#define CMD_V_PID 'V' //!< velocity PID & LPF
10-
#define CMD_A_PID 'A' //!< angle PID & LPF
11-
#define CMD_STATUS 'E' //!< motor status enable/disable
12-
#define CMD_LIMITS 'L' //!< limits current/voltage/velocity
13-
#define CMD_MOTION_TYPE 'C' //!< motion control type
14-
#define CMD_TORQUE_TYPE 'T' //!< torque control type
15-
#define CMD_SENSOR 'S' //!< sensor offsets
16-
#define CMD_MONITOR 'M' //!< monitoring
17-
#define CMD_RESIST 'R' //!< motor phase resistance
18-
#define CMD_INDUCTANCE 'I' //!< motor phase inductance
19-
#define CMD_KV_RATING 'K' //!< motor kv rating
20-
#define CMD_PWMMOD 'W' //!< pwm modulation
21-
#define CMD_FOC_PARAMS 'F' //!< time parameters
22-
23-
// commander configuration
24-
#define CMD_SCAN '?' //!< command scaning the network - only for commander
25-
#define CMD_VERBOSE '@' //!< command setting output mode - only for commander
26-
#define CMD_DECIMAL '#' //!< command setting decimal places - only for commander
27-
28-
// subcomands
29-
//pid - lpf
30-
#define SCMD_PID_P 'P' //!< PID gain P
31-
#define SCMD_PID_I 'I' //!< PID gain I
32-
#define SCMD_PID_D 'D' //!< PID gain D
33-
#define SCMD_PID_RAMP 'R' //!< PID ramp
34-
#define SCMD_PID_LIM 'L' //!< PID limit
35-
#define SCMD_LPF_TF 'F' //!< LPF time constant
36-
// limits
37-
#define SCMD_LIM_CURR 'C' //!< Limit current
38-
#define SCMD_LIM_VOLT 'U' //!< Limit voltage
39-
#define SCMD_LIM_VEL 'V' //!< Limit velocity
40-
//sensor
41-
#define SCMD_SENS_MECH_OFFSET 'M' //!< Sensor offset
42-
#define SCMD_SENS_ELEC_OFFSET 'E' //!< Sensor electrical zero offset
43-
// monitoring
44-
#define SCMD_DOWNSAMPLE 'D' //!< Monitoring downsample value
45-
#define SCMD_CLEAR 'C' //!< Clear all monitored variables
46-
#define SCMD_GET 'G' //!< Get variable only one value
47-
#define SCMD_SET 'S' //!< Set variables to be monitored
48-
49-
#define SCMD_PWMMOD_TYPE 'T' //!<< Pwm modulation type
50-
#define SCMD_PWMMOD_CENTER 'C' //!<< Pwm modulation center flag
51-
52-
#define SCMD_LOOPFOC_TIME 'L' //!< loopFOC time between executions (filtered)
53-
#define SCMD_MOVE_TIME 'M' //!< move time between executions (filtered)
54-
#define SCMD_REINIT_FOC 'R' //!< reinitialize FOC
55-
#define SCMD_TUNE_CURR 'C' //!< tune current controller
56-
#define SCMD_MEAS_PARAMS 'P' //!< measure motor parameters (resistance and inductance)
57-
58-
#define SCMD_INDUCT_D 'D' //!< inductance d axis
59-
#define SCMD_INDUCT_Q 'Q' //!< inductance q axis
7+
#define CMD_C_D_PID 'D' //!< current d PID & LPF
8+
#define CMD_C_Q_PID 'Q' //!< current q PID & LPF
9+
#define CMD_V_PID 'V' //!< velocity PID & LPF
10+
#define CMD_A_PID 'A' //!< angle PID & LPF
11+
#define CMD_STATUS 'E' //!< motor status enable/disable
12+
#define CMD_LIMITS 'L' //!< limits current/voltage/velocity
13+
#define CMD_MOTION_TYPE 'C' //!< motion control type
14+
#define CMD_TORQUE_TYPE 'T' //!< torque control type
15+
#define CMD_SENSOR 'S' //!< sensor offsets
16+
#define CMD_MONITOR 'M' //!< monitoring
17+
#define CMD_RESIST 'R' //!< motor phase resistance
18+
#define CMD_INDUCTANCE 'I' //!< motor phase inductance
19+
#define CMD_KV_RATING 'K' //!< motor kv rating
20+
#define CMD_PWMMOD 'W' //!< pwm modulation
21+
#define CMD_FOC_PARAMS 'F' //!< time parameters
22+
23+
// commander configuration
24+
#define CMD_SCAN '?' //!< command scaning the network - only for commander
25+
#define CMD_VERBOSE '@' //!< command setting output mode - only for commander
26+
#define CMD_DECIMAL '#' //!< command setting decimal places - only for commander
27+
28+
// subcomands
29+
//pid - lpf
30+
#define SCMD_PID_P 'P' //!< PID gain P
31+
#define SCMD_PID_I 'I' //!< PID gain I
32+
#define SCMD_PID_D 'D' //!< PID gain D
33+
#define SCMD_PID_RAMP 'R' //!< PID ramp
34+
#define SCMD_PID_LIM 'L' //!< PID limit
35+
#define SCMD_LPF_TF 'F' //!< LPF time constant
36+
// limits
37+
#define SCMD_LIM_CURR 'C' //!< Limit current
38+
#define SCMD_LIM_VOLT 'U' //!< Limit voltage
39+
#define SCMD_LIM_VEL 'V' //!< Limit velocity
40+
//sensor
41+
#define SCMD_SENS_MECH_OFFSET 'M' //!< Sensor offset
42+
#define SCMD_SENS_ELEC_OFFSET 'E' //!< Sensor electrical zero offset
43+
// monitoring
44+
#define SCMD_DOWNSAMPLE 'D' //!< Monitoring downsample value
45+
#define SCMD_CLEAR 'C' //!< Clear all monitored variables
46+
#define SCMD_GET 'G' //!< Get variable only one value
47+
#define SCMD_SET 'S' //!< Set variables to be monitored
48+
#define SCMD_TIME 'T' //!< Time between executions (filtered)
49+
50+
// pwm modulation
51+
#define SCMD_PWMMOD_TYPE 'T' //!<< Pwm modulation type
52+
#define SCMD_PWMMOD_CENTER 'C' //!<< Pwm modulation center flag
53+
54+
// foc control parameters
55+
#define SCMD_REINIT_FOC 'R' //!< reinitialize FOC
56+
#define SCMD_TUNE_CURR 'C' //!< tune current controller
57+
#define SCMD_MEAS_PARAMS 'P' //!< measure motor parameters (resistance and inductance)
58+
59+
// subcommands for motor parameters measurement
60+
#define SCMD_INDUCT_D 'D' //!< inductance d axis
61+
#define SCMD_INDUCT_Q 'Q' //!< inductance q axis
6062

6163
#endif

0 commit comments

Comments
 (0)