Micromouse 18 / Mbed 2 deprecated encoder_v2

Dependencies:   QEI RemoteIR mbed

Fork of encoder_v2 by Joshua Cheung

PID_control.cpp@10:707e542688dc, 2017-12-01 (annotated)

Committer:
Joshua_Cheung
Date:
Fri Dec 01 02:00:02 2017 +0000
Revision:
10:707e542688dc
Parent:
9:20b8b64ed259
Child:
11:e647b14ff4ef
testStop not working

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