From a41ce030f09571906a701afa7895ebfe07300dbe Mon Sep 17 00:00:00 2001 From: Petteri Aimonen Date: Sun, 12 Jan 2020 17:46:49 +0200 Subject: [PATCH 1/2] Add phase adjust / deadband for PID controller Currently the PID loop always uses either + or - maximum phase advance. This means that when the PID output value is 0, there is no torque applied. This causes the motor to zoom past the setpoint if there is no external friction to stop it, which in turn causes a lot of oscillations. This commit adjusts the phase angle when PID output is close to zero, which acts to brake the motor and allows it to stop accurately between steps also. --- Mechaduino/Mechaduino/Controller.cpp | 15 ++++++++++++--- Mechaduino/Mechaduino/Parameters.cpp | 2 ++ Mechaduino/Mechaduino/Parameters.h | 4 ++-- 3 files changed, 16 insertions(+), 5 deletions(-) diff --git a/Mechaduino/Mechaduino/Controller.cpp b/Mechaduino/Mechaduino/Controller.cpp index f88ac02..366f784 100644 --- a/Mechaduino/Mechaduino/Controller.cpp +++ b/Mechaduino/Mechaduino/Controller.cpp @@ -67,16 +67,25 @@ void TC5_Handler() { // gets called with FPID frequency y_1 = y; //copy current value of y to previous value (y_1) for next control cycle before PA angle added + // Depending on direction we want to apply torque, add or subtract a phase angle of PA for max effective torque. + // PA should be equal to one full step angle: if the excitation angle is the same as the current position, we would not move! + // If we are close to target (u is close to 0), phase angle is adjusted to intermediate value to reduce oscillations. + float pa = PA; + + if (u > -deadband && u < deadband) + { + pa = PA * abs(u) / deadband; + } - if (u > 0) //Depending on direction we want to apply torque, add or subtract a phase angle of PA for max effective torque. PA should be equal to one full step angle: if the excitation angle is the same as the current position, we would not move! + if (u > 0) { //You can experiment with "Phase Advance" by increasing PA when operating at high speeds - y += PA; //update phase excitation angle + y += pa; //update phase excitation angle if (u > uMAX) // limit control effort u = uMAX; //saturation limits max current command } else { - y -= PA; //update phase excitation angle + y -= pa; //update phase excitation angle if (u < -uMAX) // limit control effort u = -uMAX; //saturation limits max current command } diff --git a/Mechaduino/Mechaduino/Parameters.cpp b/Mechaduino/Mechaduino/Parameters.cpp index cd4666f..1436493 100644 --- a/Mechaduino/Mechaduino/Parameters.cpp +++ b/Mechaduino/Mechaduino/Parameters.cpp @@ -44,6 +44,8 @@ const float iMAX = 1.0; // Be careful adjusting this. While the A49 const float rSense = 0.150; volatile int uMAX = (255/3.3)*(iMAX*10*rSense); // 255 for 8-bit pwm, 1023 for 10 bit, must also edit analogFastWrite +volatile int deadband = uMAX / 16; + // A sine lookup table is faster than using the built in sin() function // for motor commutation... shifted by 0 degrees (this appears to allow // for a less noisy cal routine) and multiplied by 1024 diff --git a/Mechaduino/Mechaduino/Parameters.h b/Mechaduino/Mechaduino/Parameters.h index e38e065..4bc0f68 100644 --- a/Mechaduino/Mechaduino/Parameters.h +++ b/Mechaduino/Mechaduino/Parameters.h @@ -36,12 +36,12 @@ extern const float aps; // angle per step extern int cpr; //counts per rev extern const float stepangle; -extern volatile float PA; // +extern volatile float PA; // Phase advance extern const float iMAX; extern const float rSense; extern volatile int uMAX; - +extern volatile int deadband; // Deadband for PID loop, in this range phase is adjusted between -PA and +PA extern const int sin_1[]; From 3c4c7a7af0690b80187bafb9cc9cc4edef5477e8 Mon Sep 17 00:00:00 2001 From: Petteri Aimonen Date: Sun, 12 Jan 2020 17:56:14 +0200 Subject: [PATCH 2/2] Allow 'read position' command to work while in closed loop mode. --- Mechaduino/Mechaduino/Parameters.h | 1 + Mechaduino/Mechaduino/Utils.cpp | 17 +++++++++++++---- 2 files changed, 14 insertions(+), 4 deletions(-) diff --git a/Mechaduino/Mechaduino/Parameters.h b/Mechaduino/Mechaduino/Parameters.h index 4bc0f68..8f7178f 100644 --- a/Mechaduino/Mechaduino/Parameters.h +++ b/Mechaduino/Mechaduino/Parameters.h @@ -43,6 +43,7 @@ extern const float rSense; extern volatile int uMAX; extern volatile int deadband; // Deadband for PID loop, in this range phase is adjusted between -PA and +PA + extern const int sin_1[]; //Defines for pins: diff --git a/Mechaduino/Mechaduino/Utils.cpp b/Mechaduino/Mechaduino/Utils.cpp index e583020..400e652 100644 --- a/Mechaduino/Mechaduino/Utils.cpp +++ b/Mechaduino/Mechaduino/Utils.cpp @@ -693,10 +693,19 @@ void print_angle() /////////////////////////////////// PRIN SerialUSB.print(" , "); // SerialUSB.print(stepNumber * aps, DEC); // SerialUSB.print(" , "); - SerialUSB.print("Angle: "); - SerialUSB.print(read_angle(), 2); - SerialUSB.print(", raw encoder: "); - SerialUSB.print(readEncoder()); + if (TC5->COUNT16.CTRLA.reg & TC_CTRLA_ENABLE) + { + // Print realtime data when in closed loop mode + SerialUSB.print("Angle: "); + SerialUSB.print(y_1, 2); + } + else + { + SerialUSB.print("Angle: "); + SerialUSB.print(read_angle(), 2); + SerialUSB.print(", raw encoder: "); + SerialUSB.print(readEncoder()); + } SerialUSB.println(); }