From bd83388bd52f47701f7600b47384abe05ae888ab Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sat, 10 Feb 2024 03:04:32 +0100 Subject: [PATCH 1/3] add a angle_nocascade mode --- src/BLDCMotor.cpp | 22 ++++++++++++++++------ src/StepperMotor.cpp | 22 ++++++++++++++++------ src/common/base_classes/FOCMotor.h | 3 ++- 3 files changed, 34 insertions(+), 13 deletions(-) diff --git a/src/BLDCMotor.cpp b/src/BLDCMotor.cpp index 9dbc0a48..1dc2b0dd 100644 --- a/src/BLDCMotor.cpp +++ b/src/BLDCMotor.cpp @@ -86,11 +86,14 @@ void BLDCMotor::init() { if(_isset(phase_resistance) || torque_controller != TorqueControlType::voltage){ // velocity control loop controls current PID_velocity.limit = current_limit; + P_angle.limit = current_limit; // for MotionControlType::angle_nocascade }else{ // velocity control loop controls the voltage PID_velocity.limit = voltage_limit; + P_angle.limit = voltage_limit; // for MotionControlType::angle_nocascade } - P_angle.limit = velocity_limit; + if (controller!=MotionControlType::angle_nocascade) // if not MotionControlType::angle_nocascade angle PID output is limited by velocity 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 @@ -423,16 +426,23 @@ void BLDCMotor::move(float new_target) { } break; case MotionControlType::angle: + case MotionControlType::angle_nocascade: // 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 - 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 (controller == MotionControlType::angle) { + // 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 - 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 + } + else { + // no velocity control + current_sp = P_angle( shaft_angle_sp - shaft_angle ); + } // if torque controlled through voltage if(torque_controller == TorqueControlType::voltage){ // use voltage if phase-resistance not provided diff --git a/src/StepperMotor.cpp b/src/StepperMotor.cpp index 697ed277..e37db523 100644 --- a/src/StepperMotor.cpp +++ b/src/StepperMotor.cpp @@ -51,10 +51,13 @@ void StepperMotor::init() { if(_isset(phase_resistance)){ // velocity control loop controls current PID_velocity.limit = current_limit; + P_angle.limit = current_limit; // for MotionControlType::angle_nocascade }else{ PID_velocity.limit = voltage_limit; + P_angle.limit = voltage_limit; // for MotionControlType::angle_nocascade } - P_angle.limit = velocity_limit; + if (controller!=MotionControlType::angle_nocascade) // if not MotionControlType::angle_nocascade angle PID output is limited by velocity 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 @@ -308,13 +311,20 @@ void StepperMotor::move(float new_target) { else voltage.d = _constrain( -target*shaft_velocity*pole_pairs*phase_inductance, -voltage_limit, voltage_limit); break; case MotionControlType::angle: + case MotionControlType::angle_nocascade: // 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 - current_sp = PID_velocity(shaft_velocity_sp - shaft_velocity); // if voltage torque control + if (controller == MotionControlType::angle) { + // 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 + current_sp = PID_velocity(shaft_velocity_sp - shaft_velocity); // if voltage torque control + } + else { + // no velocity control + current_sp = P_angle( shaft_angle_sp - shaft_angle ); + } // if torque controlled through voltage // use voltage if phase-resistance not provided if(!_isset(phase_resistance)) voltage.q = current_sp; diff --git a/src/common/base_classes/FOCMotor.h b/src/common/base_classes/FOCMotor.h index b5b0a557..42cdbe15 100644 --- a/src/common/base_classes/FOCMotor.h +++ b/src/common/base_classes/FOCMotor.h @@ -29,7 +29,8 @@ enum MotionControlType : uint8_t { velocity = 0x01, //!< Velocity motion control angle = 0x02, //!< Position/angle motion control velocity_openloop = 0x03, - angle_openloop = 0x04 + angle_openloop = 0x04, + angle_nocascade = 0x05 //!< Position/angle motion control without velocity cascade }; /** From b0deb221c292c777cb2f520661c12734e80fbcb9 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sun, 18 Feb 2024 15:56:24 +0100 Subject: [PATCH 2/3] add nocascade case to commander --- src/communication/Commander.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/communication/Commander.cpp b/src/communication/Commander.cpp index d2822b04..dabfed61 100644 --- a/src/communication/Commander.cpp +++ b/src/communication/Commander.cpp @@ -505,6 +505,7 @@ void Commander::target(FOCMotor* motor, char* user_cmd, char* separator){ float pos, vel, torque; char* next_value; switch(motor->controller){ + case MotionControlType::angle_nocascade: case MotionControlType::torque: // setting torque target torque = atof(strtok (user_cmd, separator)); motor->target = torque; From 9f90d0a89d55f0376e3e15ce4f43500e25cd1c05 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sun, 18 Feb 2024 16:20:49 +0100 Subject: [PATCH 3/3] add nocascade to Commander --- src/communication/Commander.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/communication/Commander.cpp b/src/communication/Commander.cpp index dabfed61..51143b2c 100644 --- a/src/communication/Commander.cpp +++ b/src/communication/Commander.cpp @@ -377,7 +377,7 @@ void Commander::motion(FOCMotor* motor, char* user_cmd, char* separator){ break; default: // change control type - if(!GET && value >= 0 && (int)value < 5) // if set command + if(!GET && value >= 0 && (int)value <= 5) // if set command motor->controller = (MotionControlType)value; switch(motor->controller){ case MotionControlType::torque: @@ -395,6 +395,9 @@ void Commander::motion(FOCMotor* motor, char* user_cmd, char* separator){ case MotionControlType::angle_openloop: println(F("angle open")); break; + case MotionControlType::angle_nocascade: + println(F("angle nocascade")); + break; } break; }