Bryce Williams
Basic Demo for PID motor control
Revision 0:929eee0b13f2, committed 2018-08-14
- Comitter:
- electromotivated
- Date:
- Tue Aug 14 17:14:36 2018 +0000
- Commit message:
- Init;
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/PID.lib Tue Aug 14 17:14:36 2018 +0000 @@ -0,0 +1,1 @@ +http://developer.mbed.org/users/electromotivated/code/PID/#0cf2f6d13c71
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/QEI.lib Tue Aug 14 17:14:36 2018 +0000 @@ -0,0 +1,1 @@ +http://developer.mbed.org/users/electromotivated/code/QEI/#50aae578cb89
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Tue Aug 14 17:14:36 2018 +0000
@@ -0,0 +1,142 @@
+#include "mbed.h"
+#include "PID.h"
+#include "QEI.h"
+
+/*
+ Demo of setting motor speed using encoder in pid feedback.
+
+ Parts used for this demo:
+ HN-GH12-1634T Gear Motor
+ LMD18200 H-Bridge Breakout Board
+ E4P-100-079 Quadrature Encoder
+
+ TODO: Implement a "rolling"/"moving" average in callback
+ for speed feedback
+*/
+
+float clip(float value, float lower, float upper);
+
+static float setpoint, feedback, output;
+const float output_lower_limit = -1.0;
+const float output_upper_limit = 1.0;
+const float FEEDBACK_SCALE = 1.0/3000.0; // Scale feedback to 1rev/3000cnts
+
+const float kp = 0.01;
+const float ki = 0.01;
+const float kd = 0.0;
+const float Ts = 0.04; // 25Hz Sample Freq (40ms Sample Time);
+ // Used for PID Sample time and used
+ // to calculate callback() interrupt
+ // time
+
+PID pid(&setpoint, &feedback, &output, output_lower_limit, output_upper_limit,
+ kp, ki, kd, Ts);
+QEI encoder(p15, p16);
+PwmOut mtr_pwm(p25);
+DigitalOut mtr_dir(p24);
+void pid_callback(); // Updates encoder feedback and motor output
+Ticker motor;
+
+int main() {
+ // Wait for me to plug in motor battery
+ wait(5);
+
+ // Clear encoder count
+ encoder.reset();
+
+ // Init the motor
+ mtr_dir = 0; // CW
+ mtr_pwm = 0.0; // Quarter speed
+
+ // Update sensors and feedback twice as fast as PID sample time;
+ // this makes pid react in real-time avoiding errors due to
+ // missing counts etc.
+ motor.attach(&pid_callback, Ts/2.0);
+
+ // Init the pid
+ /*TODO: Implement PID Change Param Method in the PID class
+ and use it to init gains here*/
+ setpoint = 0.0;
+ feedback = encoder.read();
+ output = 0.0;
+ pid.start();
+
+ while(1){
+ int flag;
+ float userInput;
+ do{
+ printf("Enter Speed/RPM (-100.0 to 100.0)\r\n");
+ flag = scanf("%f", &userInput);
+ }while(flag == EOF);
+ setpoint = userInput;
+ do{
+ printf("Setpoint: %1.2f\t\tFeedback: %1.2f\t\tError: %1.2f\t\tOuput: %1.2f\r\n",
+ setpoint, feedback, pid.getError(), output);
+ wait(0.25);
+ }while(pid.getError() < -0.006 || 0.006 < pid.getError());
+ printf("Speed Reached!\r\n");
+ printf("Setpoint: %1.2f\t\tFeedback: %1.2f\t\tError: %1.2f\t\tOuput: %1.2f\r\n",
+ setpoint, feedback, pid.getError(), output);
+ }
+}
+
+/*
+ Updates feedback and output, interrupt driven so that paramaters
+ are updated in real-time, i.e. avoids update lag due to main
+ code overhead and printfs which can be slow.
+*/
+void pid_callback(){
+ // Update motor
+ if(setpoint >= 0.0) mtr_dir = 1; // Set motor direction based on setpoint
+ else mtr_dir = 0;
+ if(-0.001 < setpoint && setpoint < 0.001){
+ /* Setpoint = 0 is a special case, we allow output to control speed AND
+ direction to fight intertia and/or downhill roll. */
+ if(output >= 0.0) mtr_dir = 1;
+ else mtr_dir = 0;
+ mtr_pwm = abs(output);
+ }
+ else{
+ if(mtr_dir == 1){ // If CW then apply positive outputs
+ if(output >= 0.0) mtr_pwm = output;
+ else mtr_pwm = 0.0;
+ }
+ else{ // If CCW then apply negative outputs
+ if(output <= 0.0) mtr_pwm = abs(output);
+ else mtr_pwm = 0.0;
+ }
+ }
+// if(mtr_dir == 1){ // If CW then apply positive outputs
+// if(output >= 0.0) mtr_pwm = output;
+// else mtr_pwm = mtr_pwm.read() - abs(output); // Take negative output value out of full range
+// // helps avoid bumpiness in motor
+// }
+// else{ // If CCW then apply negative outputs
+// if(output <= 0.0) mtr_pwm = abs(output);
+// else mtr_pwm = mtr_pwm.read() - abs(output);
+// }
+//
+ // Running average
+ float k = Ts/2.0; // Discrete time, (Ts/2 because this callback is called
+ // at interval of Ts/2... or twice as fast as pid controller)
+
+ /* TODO: Implement a "rolling"/"moving" average */
+ static int last_count = 0;
+ int count = encoder.read();
+ float raw_speed = ((count - last_count)*FEEDBACK_SCALE) / k;
+ float rpm_speed = raw_speed * 60.0; // Convert speed to RPM
+
+ last_count = count; // Save last count
+ feedback = rpm_speed;
+}
+
+/*
+ Clips value to lower/ uppper
+ @param value The value to clip
+ @param lower The mininum allowable value
+ @param upper The maximum allowable value
+ @return The resulting clipped value
+*/
+float clip(float value, float lower, float upper){
+ return std::max(lower, std::min(value, upper));
+}
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Tue Aug 14 17:14:36 2018 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/9296ab0bfc11 \ No newline at end of file