Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Adjust phase when close to target, allow read_position to work while in closed loop mode #41

Open
wants to merge 2 commits into
base: master
Choose a base branch
from
Open

Adjust phase when close to target, allow read_position to work while in closed loop mode #41

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
a41ce03
Add phase adjust / deadband for PID controller
PetteriAimonen Jan 12, 2020
3c4c7a7
Allow 'read position' command to work while in closed loop mode.
PetteriAimonen Jan 12, 2020
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
15 changes: 12 additions & 3 deletions Mechaduino/Mechaduino/Controller.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Copy link

@mmwolbrink mmwolbrink Jan 12, 2020 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This is a really nice idea, I was thinking of implementing like this myself, but It looks like you beat me to it.
It might be a good idea calculate inverse of deadband once where its defined, and multiply by this inversion. I think doing an expensive division every interrupt is not good practice (Armv6 doesn't have division instruction).

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Possibly true, but it is a float division so a float multiplication is only slightly faster.

Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Sorry you are correct, I forgot about the lack of floating point hardware. Out of curiosity I will try both approaches and time the results.

Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Sorry you are correct, I forgot about the lack of floating point hardware. Out of curiosity I will try both approaches and time the results.

I just had some time to test, your approach took 20uS, the inverted multiplication 12uS. The code seemed to work fine.

}

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
}
Expand Down
2 changes: 2 additions & 0 deletions Mechaduino/Mechaduino/Parameters.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
3 changes: 2 additions & 1 deletion Mechaduino/Mechaduino/Parameters.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -36,11 +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[];
Expand Down
17 changes: 13 additions & 4 deletions Mechaduino/Mechaduino/Utils.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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();
}

Expand Down