Bryce Williams / Mbed 2 deprecated pid_encoder_speed_demo

Basic Demo for PID motor control

Dependencies:   PID QEI mbed

main.cpp@0:929eee0b13f2, 2018-08-14 (annotated)

Committer:
electromotivated
Date:
Tue Aug 14 17:14:36 2018 +0000
Revision:
0:929eee0b13f2
Init;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
electromotivated 0:929eee0b13f2 1 #include "mbed.h"
electromotivated 0:929eee0b13f2 2 #include "PID.h"
electromotivated 0:929eee0b13f2 3 #include "QEI.h"
electromotivated 0:929eee0b13f2 4
electromotivated 0:929eee0b13f2 5 /*
electromotivated 0:929eee0b13f2 6 Demo of setting motor speed using encoder in pid feedback.
electromotivated 0:929eee0b13f2 7
electromotivated 0:929eee0b13f2 8 Parts used for this demo:
electromotivated 0:929eee0b13f2 9 HN-GH12-1634T Gear Motor
electromotivated 0:929eee0b13f2 10 LMD18200 H-Bridge Breakout Board
electromotivated 0:929eee0b13f2 11 E4P-100-079 Quadrature Encoder
electromotivated 0:929eee0b13f2 12
electromotivated 0:929eee0b13f2 13 TODO: Implement a "rolling"/"moving" average in callback
electromotivated 0:929eee0b13f2 14 for speed feedback
electromotivated 0:929eee0b13f2 15 */
electromotivated 0:929eee0b13f2 16
electromotivated 0:929eee0b13f2 17 float clip(float value, float lower, float upper);
electromotivated 0:929eee0b13f2 18
electromotivated 0:929eee0b13f2 19 static float setpoint, feedback, output;
electromotivated 0:929eee0b13f2 20 const float output_lower_limit = -1.0;
electromotivated 0:929eee0b13f2 21 const float output_upper_limit = 1.0;
electromotivated 0:929eee0b13f2 22 const float FEEDBACK_SCALE = 1.0/3000.0; // Scale feedback to 1rev/3000cnts
electromotivated 0:929eee0b13f2 23
electromotivated 0:929eee0b13f2 24 const float kp = 0.01;
electromotivated 0:929eee0b13f2 25 const float ki = 0.01;
electromotivated 0:929eee0b13f2 26 const float kd = 0.0;
electromotivated 0:929eee0b13f2 27 const float Ts = 0.04; // 25Hz Sample Freq (40ms Sample Time);
electromotivated 0:929eee0b13f2 28 // Used for PID Sample time and used
electromotivated 0:929eee0b13f2 29 // to calculate callback() interrupt
electromotivated 0:929eee0b13f2 30 // time
electromotivated 0:929eee0b13f2 31
electromotivated 0:929eee0b13f2 32 PID pid(&setpoint, &feedback, &output, output_lower_limit, output_upper_limit,
electromotivated 0:929eee0b13f2 33 kp, ki, kd, Ts);
electromotivated 0:929eee0b13f2 34 QEI encoder(p15, p16);
electromotivated 0:929eee0b13f2 35 PwmOut mtr_pwm(p25);
electromotivated 0:929eee0b13f2 36 DigitalOut mtr_dir(p24);
electromotivated 0:929eee0b13f2 37 void pid_callback(); // Updates encoder feedback and motor output
electromotivated 0:929eee0b13f2 38 Ticker motor;
electromotivated 0:929eee0b13f2 39
electromotivated 0:929eee0b13f2 40 int main() {
electromotivated 0:929eee0b13f2 41 // Wait for me to plug in motor battery
electromotivated 0:929eee0b13f2 42 wait(5);
electromotivated 0:929eee0b13f2 43
electromotivated 0:929eee0b13f2 44 // Clear encoder count
electromotivated 0:929eee0b13f2 45 encoder.reset();
electromotivated 0:929eee0b13f2 46
electromotivated 0:929eee0b13f2 47 // Init the motor
electromotivated 0:929eee0b13f2 48 mtr_dir = 0; // CW
electromotivated 0:929eee0b13f2 49 mtr_pwm = 0.0; // Quarter speed
electromotivated 0:929eee0b13f2 50
electromotivated 0:929eee0b13f2 51 // Update sensors and feedback twice as fast as PID sample time;
electromotivated 0:929eee0b13f2 52 // this makes pid react in real-time avoiding errors due to
electromotivated 0:929eee0b13f2 53 // missing counts etc.
electromotivated 0:929eee0b13f2 54 motor.attach(&pid_callback, Ts/2.0);
electromotivated 0:929eee0b13f2 55
electromotivated 0:929eee0b13f2 56 // Init the pid
electromotivated 0:929eee0b13f2 57 /*TODO: Implement PID Change Param Method in the PID class
electromotivated 0:929eee0b13f2 58 and use it to init gains here*/
electromotivated 0:929eee0b13f2 59 setpoint = 0.0;
electromotivated 0:929eee0b13f2 60 feedback = encoder.read();
electromotivated 0:929eee0b13f2 61 output = 0.0;
electromotivated 0:929eee0b13f2 62 pid.start();
electromotivated 0:929eee0b13f2 63
electromotivated 0:929eee0b13f2 64 while(1){
electromotivated 0:929eee0b13f2 65 int flag;
electromotivated 0:929eee0b13f2 66 float userInput;
electromotivated 0:929eee0b13f2 67 do{
electromotivated 0:929eee0b13f2 68 printf("Enter Speed/RPM (-100.0 to 100.0)\r\n");
electromotivated 0:929eee0b13f2 69 flag = scanf("%f", &userInput);
electromotivated 0:929eee0b13f2 70 }while(flag == EOF);
electromotivated 0:929eee0b13f2 71 setpoint = userInput;
electromotivated 0:929eee0b13f2 72 do{
electromotivated 0:929eee0b13f2 73 printf("Setpoint: %1.2f\t\tFeedback: %1.2f\t\tError: %1.2f\t\tOuput: %1.2f\r\n",
electromotivated 0:929eee0b13f2 74 setpoint, feedback, pid.getError(), output);
electromotivated 0:929eee0b13f2 75 wait(0.25);
electromotivated 0:929eee0b13f2 76 }while(pid.getError() < -0.006 || 0.006 < pid.getError());
electromotivated 0:929eee0b13f2 77 printf("Speed Reached!\r\n");
electromotivated 0:929eee0b13f2 78 printf("Setpoint: %1.2f\t\tFeedback: %1.2f\t\tError: %1.2f\t\tOuput: %1.2f\r\n",
electromotivated 0:929eee0b13f2 79 setpoint, feedback, pid.getError(), output);
electromotivated 0:929eee0b13f2 80 }
electromotivated 0:929eee0b13f2 81 }
electromotivated 0:929eee0b13f2 82
electromotivated 0:929eee0b13f2 83 /*
electromotivated 0:929eee0b13f2 84 Updates feedback and output, interrupt driven so that paramaters
electromotivated 0:929eee0b13f2 85 are updated in real-time, i.e. avoids update lag due to main
electromotivated 0:929eee0b13f2 86 code overhead and printfs which can be slow.
electromotivated 0:929eee0b13f2 87 */
electromotivated 0:929eee0b13f2 88 void pid_callback(){
electromotivated 0:929eee0b13f2 89 // Update motor
electromotivated 0:929eee0b13f2 90 if(setpoint >= 0.0) mtr_dir = 1; // Set motor direction based on setpoint
electromotivated 0:929eee0b13f2 91 else mtr_dir = 0;
electromotivated 0:929eee0b13f2 92 if(-0.001 < setpoint && setpoint < 0.001){
electromotivated 0:929eee0b13f2 93 /* Setpoint = 0 is a special case, we allow output to control speed AND
electromotivated 0:929eee0b13f2 94 direction to fight intertia and/or downhill roll. */
electromotivated 0:929eee0b13f2 95 if(output >= 0.0) mtr_dir = 1;
electromotivated 0:929eee0b13f2 96 else mtr_dir = 0;
electromotivated 0:929eee0b13f2 97 mtr_pwm = abs(output);
electromotivated 0:929eee0b13f2 98 }
electromotivated 0:929eee0b13f2 99 else{
electromotivated 0:929eee0b13f2 100 if(mtr_dir == 1){ // If CW then apply positive outputs
electromotivated 0:929eee0b13f2 101 if(output >= 0.0) mtr_pwm = output;
electromotivated 0:929eee0b13f2 102 else mtr_pwm = 0.0;
electromotivated 0:929eee0b13f2 103 }
electromotivated 0:929eee0b13f2 104 else{ // If CCW then apply negative outputs
electromotivated 0:929eee0b13f2 105 if(output <= 0.0) mtr_pwm = abs(output);
electromotivated 0:929eee0b13f2 106 else mtr_pwm = 0.0;
electromotivated 0:929eee0b13f2 107 }
electromotivated 0:929eee0b13f2 108 }
electromotivated 0:929eee0b13f2 109 // if(mtr_dir == 1){ // If CW then apply positive outputs
electromotivated 0:929eee0b13f2 110 // if(output >= 0.0) mtr_pwm = output;
electromotivated 0:929eee0b13f2 111 // else mtr_pwm = mtr_pwm.read() - abs(output); // Take negative output value out of full range
electromotivated 0:929eee0b13f2 112 // // helps avoid bumpiness in motor
electromotivated 0:929eee0b13f2 113 // }
electromotivated 0:929eee0b13f2 114 // else{ // If CCW then apply negative outputs
electromotivated 0:929eee0b13f2 115 // if(output <= 0.0) mtr_pwm = abs(output);
electromotivated 0:929eee0b13f2 116 // else mtr_pwm = mtr_pwm.read() - abs(output);
electromotivated 0:929eee0b13f2 117 // }
electromotivated 0:929eee0b13f2 118 //
electromotivated 0:929eee0b13f2 119 // Running average
electromotivated 0:929eee0b13f2 120 float k = Ts/2.0; // Discrete time, (Ts/2 because this callback is called
electromotivated 0:929eee0b13f2 121 // at interval of Ts/2... or twice as fast as pid controller)
electromotivated 0:929eee0b13f2 122
electromotivated 0:929eee0b13f2 123 /* TODO: Implement a "rolling"/"moving" average */
electromotivated 0:929eee0b13f2 124 static int last_count = 0;
electromotivated 0:929eee0b13f2 125 int count = encoder.read();
electromotivated 0:929eee0b13f2 126 float raw_speed = ((count - last_count)*FEEDBACK_SCALE) / k;
electromotivated 0:929eee0b13f2 127 float rpm_speed = raw_speed * 60.0; // Convert speed to RPM
electromotivated 0:929eee0b13f2 128
electromotivated 0:929eee0b13f2 129 last_count = count; // Save last count
electromotivated 0:929eee0b13f2 130 feedback = rpm_speed;
electromotivated 0:929eee0b13f2 131 }
electromotivated 0:929eee0b13f2 132
electromotivated 0:929eee0b13f2 133 /*
electromotivated 0:929eee0b13f2 134 Clips value to lower/ uppper
electromotivated 0:929eee0b13f2 135 @param value The value to clip
electromotivated 0:929eee0b13f2 136 @param lower The mininum allowable value
electromotivated 0:929eee0b13f2 137 @param upper The maximum allowable value
electromotivated 0:929eee0b13f2 138 @return The resulting clipped value
electromotivated 0:929eee0b13f2 139 */
electromotivated 0:929eee0b13f2 140 float clip(float value, float lower, float upper){
electromotivated 0:929eee0b13f2 141 return std::max(lower, std::min(value, upper));
electromotivated 0:929eee0b13f2 142 }