Micromouse 18 / Mbed 2 deprecated encoder_v2

Dependencies:   QEI RemoteIR mbed

Fork of encoder_v2 by Joshua Cheung

PID_control.cpp@11:e647b14ff4ef, 2017-12-01 (annotated)

Committer:
mbshark
Date:
Fri Dec 01 03:25:53 2017 +0000
Revision:
11:e647b14ff4ef
Parent:
10:707e542688dc
Child:
12:e44dd0d4f1ee
edited testStop();; started a testTurns();

Who changed what in which revision?

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