Micromouse 18 / Mbed 2 deprecated encoder_v2

Dependencies:   QEI RemoteIR mbed

Fork of encoder_v2 by Joshua Cheung

PID_control.cpp@9:20b8b64ed259, 2017-12-01 (annotated)

Committer:
kensterino
Date:
Fri Dec 01 01:19:35 2017 +0000
Revision:
9:20b8b64ed259
Parent:
8:6b2f7886768d
Child:
10:707e542688dc
changes in PID_Control.cpp;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kensterino 9:20b8b64ed259 1 //rev1
kensterino 9:20b8b64ed259 2 //added routines to main for easier testing of movement functions
kensterino 9:20b8b64ed259 3 //wrote testStop();
kensterino 9:20b8b64ed259 4 //TODO: test quarterTurnLeft(); , quarterTurnRight(); for delay constants
kensterino 9:20b8b64ed259 5 //TODO: finetune PID , Multiplexing for IR LEDS ,
kensterino 9:20b8b64ed259 6 //SUGGESTIONS: implement turn functions using encoders. i.e. each pulse represents 1 degree, need difference of 90 pulses for right turn
kensterino 9:20b8b64ed259 7
kensterino 9:20b8b64ed259 8
kensterino 9:20b8b64ed259 9 //Kevin Lee Nov 30, 2017
kensterino 9:20b8b64ed259 10
Joshua_Cheung 7:e10dc3cb9212 11
Joshua_Cheung 2:aa961ba3199e 12 #include "mbed.h"
Joshua_Cheung 2:aa961ba3199e 13 #include "QEI.h"
Joshua_Cheung 2:aa961ba3199e 14
Joshua_Cheung 2:aa961ba3199e 15 QEI encoder_Right(PB_3, PA_15, NC, 360, QEI::X4_ENCODING);
kensterino 4:90303483fd5f 16 QEI encoder_Left(PA_1, PC_4, NC, 360, QEI::X4_ENCODING);
Joshua_Cheung 7:e10dc3cb9212 17 double Kp = 0.27;//.005;
kensterino 6:71829ae2ee07 18 double Ki = 0.001;//0.0000001;
kensterino 4:90303483fd5f 19 double Kd = 0.001;
Joshua_Cheung 2:aa961ba3199e 20 PwmOut m_Right_F(PB_10);
Joshua_Cheung 2:aa961ba3199e 21 PwmOut m_Right_B(PC_7);
kensterino 4:90303483fd5f 22 PwmOut m_Left_F(PA_7);
kensterino 4:90303483fd5f 23 PwmOut m_Left_B(PB_6);
Joshua_Cheung 7:e10dc3cb9212 24 double i_speed = 0.15;
kensterino 4:90303483fd5f 25 double C_speed(0);
Joshua_Cheung 2:aa961ba3199e 26 int integrator = 0;
Joshua_Cheung 2:aa961ba3199e 27 int decayFactor = 1;
kensterino 4:90303483fd5f 28 double Error = 0;
kensterino 9:20b8b64ed259 29 double IRError = 0; //added
kensterino 4:90303483fd5f 30 double prevError = 0;
Joshua_Cheung 2:aa961ba3199e 31 Serial pc (PA_2, PA_3); //serial comm enabled on pins pa_2 and pa_3
kensterino 4:90303483fd5f 32 Timer timer;
kensterino 4:90303483fd5f 33 int counter = 0;
Joshua_Cheung 2:aa961ba3199e 34
kensterino 4:90303483fd5f 35 double P_controller(int error)
kensterino 4:90303483fd5f 36 {
Joshua_Cheung 2:aa961ba3199e 37 double correction = (Kp*error);
Joshua_Cheung 2:aa961ba3199e 38 return correction;
Joshua_Cheung 2:aa961ba3199e 39 }
Joshua_Cheung 2:aa961ba3199e 40
kensterino 4:90303483fd5f 41 double I_controller(int error)
kensterino 4:90303483fd5f 42 {
Joshua_Cheung 2:aa961ba3199e 43 integrator += error;
Joshua_Cheung 2:aa961ba3199e 44 double correction = Ki*integrator;
Joshua_Cheung 2:aa961ba3199e 45 integrator /= decayFactor;
Joshua_Cheung 2:aa961ba3199e 46 return correction;
Joshua_Cheung 2:aa961ba3199e 47 }
Joshua_Cheung 2:aa961ba3199e 48
kensterino 4:90303483fd5f 49 double D_controller(int error)
kensterino 4:90303483fd5f 50 {
Joshua_Cheung 2:aa961ba3199e 51 int dError = error - prevError;
Joshua_Cheung 2:aa961ba3199e 52 int dt = timer.read_us();
Joshua_Cheung 2:aa961ba3199e 53 timer.reset();
Joshua_Cheung 2:aa961ba3199e 54 prevError = error;
kensterino 5:a49a77ddf4e3 55 double correction;
kensterino 5:a49a77ddf4e3 56 if (dt==0)
kensterino 5:a49a77ddf4e3 57 correction=0;
kensterino 5:a49a77ddf4e3 58 else
kensterino 5:a49a77ddf4e3 59 correction = Kd*dError/dt;
Joshua_Cheung 2:aa961ba3199e 60 return correction;
Joshua_Cheung 2:aa961ba3199e 61 }
Joshua_Cheung 2:aa961ba3199e 62
Joshua_Cheung 2:aa961ba3199e 63 Ticker systicker;
Joshua_Cheung 2:aa961ba3199e 64 //speed = speed + P_Controller(error) + I_Controller(error) + D_Controller(error);
kensterino 4:90303483fd5f 65 void systick()
kensterino 4:90303483fd5f 66 {
kensterino 4:90303483fd5f 67 double R_en_count = encoder_Right.getPulses()/100;
kensterino 4:90303483fd5f 68 double L_en_count = encoder_Left.getPulses()/100;
kensterino 4:90303483fd5f 69 Error = R_en_count - L_en_count;
kensterino 4:90303483fd5f 70 double ex = D_controller(Error);
kensterino 4:90303483fd5f 71 C_speed = P_controller(Error) + I_controller(Error) + ex;
kensterino 5:a49a77ddf4e3 72 if (C_speed < 0)
kensterino 5:a49a77ddf4e3 73 C_speed = C_speed*-1;
kensterino 9:20b8b64ed259 74 Front_IRError = RF_IRR.read() + LF_IRR.read(); //rev1
kensterino 9:20b8b64ed259 75 Side_IRError = RS_IRR.read() - LS_IRR.read(); //rev1,define side_error as positive if closer to right
Joshua_Cheung 2:aa961ba3199e 76 }
Joshua_Cheung 2:aa961ba3199e 77
Joshua_Cheung 7:e10dc3cb9212 78
kensterino 4:90303483fd5f 79 void forward()
kensterino 4:90303483fd5f 80 {
kensterino 4:90303483fd5f 81 double f1_speed = i_speed + C_speed;
kensterino 4:90303483fd5f 82 double f2_speed = i_speed - C_speed;
kensterino 5:a49a77ddf4e3 83
kensterino 5:a49a77ddf4e3 84 /*pc.printf("C: %f", C_speed);
kensterino 5:a49a77ddf4e3 85 if (C_speed < 0)
kensterino 5:a49a77ddf4e3 86 pc.printf("-");
kensterino 5:a49a77ddf4e3 87 if (C_speed > 0)
kensterino 5:a49a77ddf4e3 88 pc.printf("+");
Joshua_Cheung 7:e10dc3cb9212 89 */
Joshua_Cheung 7:e10dc3cb9212 90
kensterino 5:a49a77ddf4e3 91
kensterino 5:a49a77ddf4e3 92 if(f1_speed >= 0.7) { //upper bound, can not go faster
kensterino 4:90303483fd5f 93 f1_speed = 0.7;
kensterino 4:90303483fd5f 94 }
kensterino 5:a49a77ddf4e3 95 if (f1_speed <= 0.05)
kensterino 5:a49a77ddf4e3 96 f1_speed = 0.05;
kensterino 5:a49a77ddf4e3 97
kensterino 5:a49a77ddf4e3 98 if(f2_speed <= 0.05) { //lower bound, should not be slower than this
kensterino 5:a49a77ddf4e3 99 f2_speed = 0.05;
kensterino 4:90303483fd5f 100 }
kensterino 5:a49a77ddf4e3 101
kensterino 5:a49a77ddf4e3 102 pc.printf(" f1: %f", f1_speed);
kensterino 5:a49a77ddf4e3 103 pc.printf(" f2: %f", f2_speed);
kensterino 5:a49a77ddf4e3 104
kensterino 4:90303483fd5f 105 //problems when left wheel is held for the + case
kensterino 4:90303483fd5f 106 if (Error > 0) { //right wheel is turning more
kensterino 4:90303483fd5f 107 m_Left_F.write(f1_speed);
kensterino 4:90303483fd5f 108 m_Right_F.write(f2_speed); //f2_speed
kensterino 4:90303483fd5f 109 }
kensterino 4:90303483fd5f 110 if (Error < 0) { //left wheel is turning more
kensterino 4:90303483fd5f 111 m_Right_F.write(f1_speed);
kensterino 4:90303483fd5f 112 m_Left_F.write(f2_speed); //f2_speed
kensterino 4:90303483fd5f 113 }
kensterino 5:a49a77ddf4e3 114 if (Error == 0) {
kensterino 4:90303483fd5f 115 m_Right_F.write(i_speed);
kensterino 5:a49a77ddf4e3 116 m_Left_F.write(i_speed);
Joshua_Cheung 2:aa961ba3199e 117 }
kensterino 4:90303483fd5f 118 }
kensterino 4:90303483fd5f 119
Joshua_Cheung 7:e10dc3cb9212 120 void backUp()
Joshua_Cheung 7:e10dc3cb9212 121 {
Joshua_Cheung 7:e10dc3cb9212 122 m_Left_F.write(0);
Joshua_Cheung 7:e10dc3cb9212 123 m_Right_F.write(0);
Joshua_Cheung 7:e10dc3cb9212 124 m_Left_B.write(i_speed);
Joshua_Cheung 7:e10dc3cb9212 125 m_Right_B.write(i_speed);
Joshua_Cheung 7:e10dc3cb9212 126 wait(.15);
Joshua_Cheung 7:e10dc3cb9212 127 }
Joshua_Cheung 7:e10dc3cb9212 128
kensterino 4:90303483fd5f 129 void turnRight()
kensterino 4:90303483fd5f 130 {
Joshua_Cheung 7:e10dc3cb9212 131 m_Left_B.write(0);
Joshua_Cheung 7:e10dc3cb9212 132 m_Right_F.write(0);
Joshua_Cheung 7:e10dc3cb9212 133 m_Left_F.write(i_speed);
Joshua_Cheung 7:e10dc3cb9212 134 m_Right_B.write(i_speed);
Joshua_Cheung 7:e10dc3cb9212 135 wait(.2);
kensterino 4:90303483fd5f 136 }
kensterino 4:90303483fd5f 137
kensterino 9:20b8b64ed259 138 void quarterTurnRight() //rev1, for ladders
kensterino 9:20b8b64ed259 139 {
kensterino 9:20b8b64ed259 140 m_Left_B.write(0);
kensterino 9:20b8b64ed259 141 m_Right_F.write(0);
kensterino 9:20b8b64ed259 142 m_Left_F.write(i_speed);
kensterino 9:20b8b64ed259 143 m_Right_B.write(i_speed);
kensterino 9:20b8b64ed259 144 wait(.1); //delay needs testing
kensterino 9:20b8b64ed259 145 }
kensterino 9:20b8b64ed259 146
kensterino 4:90303483fd5f 147 void turnLeft()
kensterino 4:90303483fd5f 148 {
Joshua_Cheung 7:e10dc3cb9212 149 m_Left_F.write(0);
Joshua_Cheung 7:e10dc3cb9212 150 m_Right_B.write(0);
Joshua_Cheung 7:e10dc3cb9212 151 m_Right_F.write(i_speed);
Joshua_Cheung 7:e10dc3cb9212 152 m_Left_B.write(i_speed);
kensterino 9:20b8b64ed259 153 wait(.2); //this is dependent on i_speed, can we write a function that varies with i_speed?
kensterino 9:20b8b64ed259 154 }
kensterino 9:20b8b64ed259 155
kensterino 9:20b8b64ed259 156 void quarterTurnLeft() { //rev1, for ladders
kensterino 9:20b8b64ed259 157 m_Left_F.write(0);
kensterino 9:20b8b64ed259 158 m_Right_B.write(0);
kensterino 9:20b8b64ed259 159 m_Right_F.write(i_speed);
kensterino 9:20b8b64ed259 160 m_Left_B.write(i_speed);
kensterino 9:20b8b64ed259 161 wait(.1); //time needs testing
kensterino 4:90303483fd5f 162 }
kensterino 4:90303483fd5f 163
kensterino 4:90303483fd5f 164 void turnAround()
kensterino 4:90303483fd5f 165 {
Joshua_Cheung 7:e10dc3cb9212 166 m_Left_B.write(0);
Joshua_Cheung 7:e10dc3cb9212 167 m_Right_F.write(0);
Joshua_Cheung 7:e10dc3cb9212 168 m_Left_F.write(i_speed);
Joshua_Cheung 7:e10dc3cb9212 169 m_Right_B.write(i_speed);
Joshua_Cheung 7:e10dc3cb9212 170 wait(.4);
Joshua_Cheung 7:e10dc3cb9212 171 }
kensterino 4:90303483fd5f 172
Joshua_Cheung 7:e10dc3cb9212 173 void stop()
Joshua_Cheung 7:e10dc3cb9212 174 {
Joshua_Cheung 7:e10dc3cb9212 175 m_Right_F.write(0);
Joshua_Cheung 7:e10dc3cb9212 176 m_Right_B.write(0);
Joshua_Cheung 7:e10dc3cb9212 177 m_Left_F.write(0);
Joshua_Cheung 7:e10dc3cb9212 178 m_Left_B.write(0);
Joshua_Cheung 2:aa961ba3199e 179 }
kensterino 4:90303483fd5f 180
kensterino 4:90303483fd5f 181 void debugEncoder()
kensterino 4:90303483fd5f 182 {
kensterino 4:90303483fd5f 183 while(1) {
kensterino 4:90303483fd5f 184 wait(1);
kensterino 4:90303483fd5f 185 pc.printf("Right: %i", encoder_Right.getPulses());
kensterino 4:90303483fd5f 186 pc.printf(" Left: %i", encoder_Left.getPulses(), "\n");
kensterino 4:90303483fd5f 187 pc.printf("\n");
kensterino 4:90303483fd5f 188 }
kensterino 4:90303483fd5f 189 }
kensterino 4:90303483fd5f 190
kensterino 4:90303483fd5f 191 void debugError()
kensterino 4:90303483fd5f 192 {
kensterino 4:90303483fd5f 193 while(1) {
kensterino 4:90303483fd5f 194 pc.printf("Error: %i\n", Error);
kensterino 4:90303483fd5f 195 }
kensterino 4:90303483fd5f 196 }
kensterino 4:90303483fd5f 197
Joshua_Cheung 7:e10dc3cb9212 198 AnalogIn RS_IRR(PA_0);
Joshua_Cheung 7:e10dc3cb9212 199 AnalogIn RF_IRR(PA_4); //Right Front
Joshua_Cheung 7:e10dc3cb9212 200 AnalogIn LF_IRR(PC_1); //Left Front
Joshua_Cheung 7:e10dc3cb9212 201 AnalogIn LS_IRR(PC_0); //Left Side
Joshua_Cheung 7:e10dc3cb9212 202
Joshua_Cheung 7:e10dc3cb9212 203 DigitalOut RS_IRE(PC_10);
Joshua_Cheung 7:e10dc3cb9212 204 DigitalOut RF_IRE(PC_11); //Right Front
Joshua_Cheung 7:e10dc3cb9212 205 DigitalOut LF_IRE(PB_0); //Left Front
Joshua_Cheung 7:e10dc3cb9212 206 DigitalOut LS_IRE(PB_7); //Left Side
Joshua_Cheung 7:e10dc3cb9212 207
kensterino 9:20b8b64ed259 208 void main1() {
Joshua_Cheung 7:e10dc3cb9212 209 printf("\nAnalogIn example\n");
Joshua_Cheung 7:e10dc3cb9212 210 LF_IRE.write(1);
Joshua_Cheung 7:e10dc3cb9212 211 RF_IRE.write(1);
Joshua_Cheung 7:e10dc3cb9212 212 while (1){
kensterino 9:20b8b64ed259 213 while (LF_IRR.read() < threshold && RF_IRR.read() < threshold){
kensterino 9:20b8b64ed259 214 forward();
kensterino 9:20b8b64ed259 215 float value1 = LF_IRR.read();
kensterino 9:20b8b64ed259 216 float value2 = RF_IRR.read();
kensterino 9:20b8b64ed259 217 printf("LF Led: %f\n", value1);
kensterino 9:20b8b64ed259 218 wait(0.5);
kensterino 9:20b8b64ed259 219 printf("RF Led: %f\n", value2);
kensterino 9:20b8b64ed259 220 }
kensterino 9:20b8b64ed259 221
Joshua_Cheung 7:e10dc3cb9212 222 backUp();
Joshua_Cheung 8:6b2f7886768d 223 LS_IRE.write(1);
Joshua_Cheung 8:6b2f7886768d 224 RS_IRE.write(1);
kensterino 9:20b8b64ed259 225
kensterino 9:20b8b64ed259 226 if (LS_IRR.read() > turnThreshold) {
Joshua_Cheung 7:e10dc3cb9212 227 if (RS_IRR.read() < turnThreshold)
Joshua_Cheung 7:e10dc3cb9212 228 turnRight();
Joshua_Cheung 7:e10dc3cb9212 229 else
kensterino 9:20b8b64ed259 230 turnAround();
kensterino 9:20b8b64ed259 231 }
kensterino 9:20b8b64ed259 232
kensterino 9:20b8b64ed259 233 else if (RS_IRR.read() > turnThreshold) {
Joshua_Cheung 7:e10dc3cb9212 234 if (LS_IRR.read() < turnThreshold)
Joshua_Cheung 7:e10dc3cb9212 235 turnRight();
Joshua_Cheung 7:e10dc3cb9212 236 else
Joshua_Cheung 7:e10dc3cb9212 237 turnAround();
kensterino 9:20b8b64ed259 238 }
Joshua_Cheung 7:e10dc3cb9212 239 else
Joshua_Cheung 7:e10dc3cb9212 240 turnAround();
kensterino 9:20b8b64ed259 241
Joshua_Cheung 8:6b2f7886768d 242 LS_IRE.write(0);
Joshua_Cheung 8:6b2f7886768d 243 RS_IRE.write(0);
kensterino 9:20b8b64ed259 244 }
Joshua_Cheung 7:e10dc3cb9212 245 stop();
kensterino 9:20b8b64ed259 246 }
kensterino 9:20b8b64ed259 247
kensterino 9:20b8b64ed259 248 void testStop() //rev1
kensterino 9:20b8b64ed259 249 {
kensterino 9:20b8b64ed259 250 //printf("\nAnalogIn example\n");
kensterino 9:20b8b64ed259 251 LF_IRE.write(1);
kensterino 9:20b8b64ed259 252 RF_IRE.write(1);
kensterino 9:20b8b64ed259 253 forward();
kensterino 9:20b8b64ed259 254 while(1) {
kensterino 9:20b8b64ed259 255 if (RF_IRR.read() < threshold && LF_IRR.read() < threshold)
kensterino 9:20b8b64ed259 256 stop();
kensterino 9:20b8b64ed259 257 }
Joshua_Cheung 7:e10dc3cb9212 258 }
kensterino 9:20b8b64ed259 259
kensterino 9:20b8b64ed259 260 void testTurnAround() //rev1
kensterino 9:20b8b64ed259 261 {
kensterino 9:20b8b64ed259 262
kensterino 9:20b8b64ed259 263 }
kensterino 9:20b8b64ed259 264
kensterino 9:20b8b64ed259 265
kensterino 9:20b8b64ed259 266 int main()
kensterino 9:20b8b64ed259 267 {
kensterino 9:20b8b64ed259 268 systicker.attach_us(&systick, 1000); //rev1
kensterino 9:20b8b64ed259 269 float threshold = 0.001;
kensterino 9:20b8b64ed259 270 float turnThreshold = 0.001;
kensterino 9:20b8b64ed259 271
kensterino 9:20b8b64ed259 272 testStop();
kensterino 9:20b8b64ed259 273 }
kensterino 9:20b8b64ed259 274
kensterino 9:20b8b64ed259 275
kensterino 9:20b8b64ed259 276
Joshua_Cheung 7:e10dc3cb9212 277 /*while(RF_IRR.read() * 100000 < 175 && LF_IRR.read() * 100000 < 175) {
Joshua_Cheung 7:e10dc3cb9212 278
Joshua_Cheung 7:e10dc3cb9212 279 /*meas = LS_IRR.read(); // Converts and read the analog input value (value from 0.0 to 1.0)
Joshua_Cheung 7:e10dc3cb9212 280 meas = meas * 100000; // Change the value to be in the 0 to 3300 range
Joshua_Cheung 7:e10dc3cb9212 281 printf("measure = %.0f mV\n", meas);
Joshua_Cheung 7:e10dc3cb9212 282 */
Joshua_Cheung 7:e10dc3cb9212 283 /*if (meas > 2000) { // If the value is greater than 2V then switch the LED on
Joshua_Cheung 7:e10dc3cb9212 284 LS_IRE = 1;
Joshua_Cheung 7:e10dc3cb9212 285 }
Joshua_Cheung 7:e10dc3cb9212 286 else {
Joshua_Cheung 7:e10dc3cb9212 287 LS_IRE = 0;
Joshua_Cheung 7:e10dc3cb9212 288 }
Joshua_Cheung 7:e10dc3cb9212 289 */
Joshua_Cheung 7:e10dc3cb9212 290 /*
Joshua_Cheung 7:e10dc3cb9212 291 forward();
Joshua_Cheung 7:e10dc3cb9212 292 //wait(0.2); // 200 ms
Joshua_Cheung 7:e10dc3cb9212 293 }
Joshua_Cheung 7:e10dc3cb9212 294 stop();
Joshua_Cheung 7:e10dc3cb9212 295 */
Joshua_Cheung 7:e10dc3cb9212 296
Joshua_Cheung 7:e10dc3cb9212 297
Joshua_Cheung 7:e10dc3cb9212 298
Joshua_Cheung 7:e10dc3cb9212 299
Joshua_Cheung 7:e10dc3cb9212 300 /*int main() //only runs once
kensterino 4:90303483fd5f 301 {
kensterino 4:90303483fd5f 302 systicker.attach_us(&systick, 1000); //enable interrupt
kensterino 5:a49a77ddf4e3 303 while (1) {
kensterino 5:a49a77ddf4e3 304 forward();
kensterino 5:a49a77ddf4e3 305 }
kensterino 4:90303483fd5f 306 //
kensterino 5:a49a77ddf4e3 307 //debugEncoder();
kensterino 4:90303483fd5f 308 }
Joshua_Cheung 7:e10dc3cb9212 309 */