Micromouse 18 / Mbed 2 deprecated encoder_v2_12-3

Scaled IR readings by 10^5 so also changed thresholds. Implemented and tested LED multiplexing Working on a new turning scheme involving IR readings

Dependencies:   QEI RemoteIR mbed

Fork of encoder_v2 by Micromouse 18

PID_control.cpp@18:14bf07a27998, 2017-12-09 (annotated)

Committer:
kensterino
Date:
Sat Dec 09 03:37:08 2017 +0000
Revision:
18:14bf07a27998
Parent:
17:2b519a746a1e 
works but make sure the fucking battery is charged

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
kensterino 16:d8428077ebc4 17 DigitalOut led1(LED1); //for debugging, on-board led. turn on by
kensterino 16:d8428077ebc4 18 //led1 = true;
Joshua_Cheung 2:aa961ba3199e 19 QEI encoder_Right(PB_3, PA_15, NC, 360, QEI::X4_ENCODING);
kensterino 4:90303483fd5f 20 QEI encoder_Left(PA_1, PC_4, NC, 360, QEI::X4_ENCODING);
Joshua_Cheung 2:aa961ba3199e 21 PwmOut m_Right_F(PB_10);
Joshua_Cheung 2:aa961ba3199e 22 PwmOut m_Right_B(PC_7);
kensterino 4:90303483fd5f 23 PwmOut m_Left_F(PA_7);
kensterino 4:90303483fd5f 24 PwmOut m_Left_B(PB_6);
kensterino 13:a208f8b052ad 25 //IRR = IR Reciver
kensterino 13:a208f8b052ad 26 AnalogIn RS_IRR(PA_0); //Right Side
kensterino 13:a208f8b052ad 27 AnalogIn RF_IRR(PA_4); //Right Front
kensterino 13:a208f8b052ad 28 AnalogIn LF_IRR(PC_1); //Left Front
kensterino 13:a208f8b052ad 29 AnalogIn LS_IRR(PC_0); //Left Side
kensterino 13:a208f8b052ad 30 //IRE = IR Emitter
kensterino 13:a208f8b052ad 31 DigitalOut RS_IRE(PC_10); //Right Side
kensterino 13:a208f8b052ad 32 DigitalOut RF_IRE(PC_11); //Right Front
kensterino 13:a208f8b052ad 33 DigitalOut LF_IRE(PB_0); //Left Front
kensterino 13:a208f8b052ad 34 DigitalOut LS_IRE(PB_7); //Left Side
kensterino 13:a208f8b052ad 35
kensterino 13:a208f8b052ad 36 Serial pc (PA_2, PA_3); //serial comm enabled on pins pa_2 and pa_3
kensterino 13:a208f8b052ad 37 Timer timer;
kensterino 13:a208f8b052ad 38
kensterino 13:a208f8b052ad 39 double Kp = 0.27;//.005;
kensterino 13:a208f8b052ad 40 double Ki = 0.001;//0.0000001;
kensterino 13:a208f8b052ad 41 double Kd = 0.001;
Joshua_Cheung 7:e10dc3cb9212 42 double i_speed = 0.15;
Joshua_Cheung 12:e44dd0d4f1ee 43 double C_speed = 0;
Joshua_Cheung 2:aa961ba3199e 44 int integrator = 0;
Joshua_Cheung 2:aa961ba3199e 45 int decayFactor = 1;
kensterino 4:90303483fd5f 46 double Error = 0;
kensterino 15:9b9bdbf4748a 47 float IRError = 0; //added
kensterino 4:90303483fd5f 48 double prevError = 0;
kensterino 4:90303483fd5f 49 int counter = 0;
kensterino 13:a208f8b052ad 50 float threshold = 50;
kensterino 15:9b9bdbf4748a 51 float turnThreshold = 25;
kensterino 13:a208f8b052ad 52 float LS_reading = 0;
kensterino 13:a208f8b052ad 53 float RS_reading = 0;
kensterino 13:a208f8b052ad 54 float LF_reading = 0;
kensterino 13:a208f8b052ad 55 float RF_reading = 0;
kensterino 13:a208f8b052ad 56 int LEDSelector = 0;
kensterino 15:9b9bdbf4748a 57 float sum = 0;
kensterino 15:9b9bdbf4748a 58 float size = 0;
Joshua_Cheung 17:2b519a746a1e 59 double IR_Kp = 0.001;
kensterino 15:9b9bdbf4748a 60 double IR_correction = 0;
kensterino 15:9b9bdbf4748a 61
kensterino 15:9b9bdbf4748a 62 void IRP_controller() { //+ means need to turn left, - means turn right
kensterino 15:9b9bdbf4748a 63 IRError = RS_reading - LS_reading;
kensterino 15:9b9bdbf4748a 64 IR_correction = (IR_Kp*IRError);
kensterino 15:9b9bdbf4748a 65 }
Joshua_Cheung 10:707e542688dc 66
Joshua_Cheung 17:2b519a746a1e 67 void resetEncoders(){
Joshua_Cheung 17:2b519a746a1e 68 encoder_Right.reset();
Joshua_Cheung 17:2b519a746a1e 69 encoder_Left.reset();
Joshua_Cheung 17:2b519a746a1e 70
Joshua_Cheung 17:2b519a746a1e 71 }
Joshua_Cheung 10:707e542688dc 72
Joshua_Cheung 2:aa961ba3199e 73
kensterino 4:90303483fd5f 74 double P_controller(int error)
kensterino 4:90303483fd5f 75 {
Joshua_Cheung 2:aa961ba3199e 76 double correction = (Kp*error);
Joshua_Cheung 2:aa961ba3199e 77 return correction;
Joshua_Cheung 2:aa961ba3199e 78 }
Joshua_Cheung 2:aa961ba3199e 79
kensterino 4:90303483fd5f 80 double I_controller(int error)
kensterino 4:90303483fd5f 81 {
Joshua_Cheung 2:aa961ba3199e 82 integrator += error;
Joshua_Cheung 2:aa961ba3199e 83 double correction = Ki*integrator;
Joshua_Cheung 2:aa961ba3199e 84 integrator /= decayFactor;
Joshua_Cheung 2:aa961ba3199e 85 return correction;
Joshua_Cheung 2:aa961ba3199e 86 }
Joshua_Cheung 2:aa961ba3199e 87
kensterino 4:90303483fd5f 88 double D_controller(int error)
kensterino 4:90303483fd5f 89 {
Joshua_Cheung 2:aa961ba3199e 90 int dError = error - prevError;
Joshua_Cheung 2:aa961ba3199e 91 int dt = timer.read_us();
Joshua_Cheung 2:aa961ba3199e 92 timer.reset();
Joshua_Cheung 2:aa961ba3199e 93 prevError = error;
kensterino 5:a49a77ddf4e3 94 double correction;
kensterino 5:a49a77ddf4e3 95 if (dt==0)
kensterino 5:a49a77ddf4e3 96 correction=0;
kensterino 5:a49a77ddf4e3 97 else
kensterino 5:a49a77ddf4e3 98 correction = Kd*dError/dt;
Joshua_Cheung 2:aa961ba3199e 99 return correction;
Joshua_Cheung 2:aa961ba3199e 100 }
Joshua_Cheung 2:aa961ba3199e 101
Joshua_Cheung 2:aa961ba3199e 102 Ticker systicker;
Joshua_Cheung 2:aa961ba3199e 103 //speed = speed + P_Controller(error) + I_Controller(error) + D_Controller(error);
kensterino 4:90303483fd5f 104 void systick()
kensterino 4:90303483fd5f 105 {
kensterino 15:9b9bdbf4748a 106 IRP_controller();
kensterino 4:90303483fd5f 107 double R_en_count = encoder_Right.getPulses()/100;
kensterino 4:90303483fd5f 108 double L_en_count = encoder_Left.getPulses()/100;
kensterino 4:90303483fd5f 109 Error = R_en_count - L_en_count;
kensterino 4:90303483fd5f 110 double ex = D_controller(Error);
kensterino 4:90303483fd5f 111 C_speed = P_controller(Error) + I_controller(Error) + ex;
kensterino 5:a49a77ddf4e3 112 if (C_speed < 0)
kensterino 5:a49a77ddf4e3 113 C_speed = C_speed*-1;
kensterino 13:a208f8b052ad 114
kensterino 13:a208f8b052ad 115
kensterino 13:a208f8b052ad 116 switch(LEDSelector) { //multiplexes IRLEDS
kensterino 13:a208f8b052ad 117 case 0:
kensterino 13:a208f8b052ad 118 LS_IRE.write(1);
kensterino 13:a208f8b052ad 119 if (LS_IRR.read() == 0) {
kensterino 13:a208f8b052ad 120 LS_IRE.write(0);
kensterino 13:a208f8b052ad 121 break;
kensterino 13:a208f8b052ad 122 }
kensterino 13:a208f8b052ad 123 LS_reading = LS_IRR.read() * 33000;
kensterino 13:a208f8b052ad 124 LS_IRE.write(0);
kensterino 13:a208f8b052ad 125 LEDSelector++;
kensterino 13:a208f8b052ad 126 break;
kensterino 13:a208f8b052ad 127 case 1:
kensterino 13:a208f8b052ad 128 RS_IRE.write(1);
kensterino 13:a208f8b052ad 129 if (RS_IRR.read() == 0) {
kensterino 13:a208f8b052ad 130 RS_IRE.write(0);
kensterino 13:a208f8b052ad 131 break;
kensterino 13:a208f8b052ad 132 }
kensterino 15:9b9bdbf4748a 133 RS_reading = RS_IRR.read() * 36332.5;//scale from 49450
kensterino 13:a208f8b052ad 134 RS_IRE.write(0);
kensterino 13:a208f8b052ad 135 LEDSelector++;
kensterino 13:a208f8b052ad 136 break;
kensterino 13:a208f8b052ad 137 case 2:
kensterino 13:a208f8b052ad 138 LF_IRE.write(1);
kensterino 13:a208f8b052ad 139 if (LF_IRR.read() == 0) {
kensterino 13:a208f8b052ad 140 LF_IRE.write(0);
kensterino 13:a208f8b052ad 141 break;
kensterino 13:a208f8b052ad 142 }
kensterino 13:a208f8b052ad 143 LF_reading = LF_IRR.read() * 50000;//scale from 33000
kensterino 13:a208f8b052ad 144 LF_IRE.write(0);
kensterino 13:a208f8b052ad 145 LEDSelector++;
kensterino 13:a208f8b052ad 146 break;
kensterino 13:a208f8b052ad 147 case 3:
kensterino 13:a208f8b052ad 148 RF_IRE.write(1);
kensterino 13:a208f8b052ad 149 if (RF_IRR.read() == 0) {
kensterino 13:a208f8b052ad 150 RF_IRE.write(0);
kensterino 13:a208f8b052ad 151 break;
kensterino 13:a208f8b052ad 152 }
kensterino 13:a208f8b052ad 153 RF_reading = RF_IRR.read() * 33000;
kensterino 13:a208f8b052ad 154 RF_IRE.write(0);
kensterino 13:a208f8b052ad 155 LEDSelector = 0;
kensterino 13:a208f8b052ad 156 break;
kensterino 13:a208f8b052ad 157 }
Joshua_Cheung 2:aa961ba3199e 158 }
Joshua_Cheung 2:aa961ba3199e 159
kensterino 18:14bf07a27998 160 bool corner() {
kensterino 18:14bf07a27998 161 if (RS_reading > 1078000000 && LS_reading > 1078000000 && RF_reading > 1080000000 && LF_reading > 1080000000)
kensterino 18:14bf07a27998 162 return true;
kensterino 18:14bf07a27998 163 else return false;
kensterino 18:14bf07a27998 164 }
Joshua_Cheung 7:e10dc3cb9212 165
kensterino 4:90303483fd5f 166 void forward()
kensterino 4:90303483fd5f 167 {
kensterino 4:90303483fd5f 168 double f1_speed = i_speed + C_speed;
kensterino 4:90303483fd5f 169 double f2_speed = i_speed - C_speed;
kensterino 5:a49a77ddf4e3 170
kensterino 5:a49a77ddf4e3 171 /*pc.printf("C: %f", C_speed);
kensterino 5:a49a77ddf4e3 172 if (C_speed < 0)
kensterino 5:a49a77ddf4e3 173 pc.printf("-");
kensterino 5:a49a77ddf4e3 174 if (C_speed > 0)
kensterino 5:a49a77ddf4e3 175 pc.printf("+");
Joshua_Cheung 7:e10dc3cb9212 176 */
Joshua_Cheung 7:e10dc3cb9212 177
kensterino 5:a49a77ddf4e3 178
kensterino 5:a49a77ddf4e3 179 if(f1_speed >= 0.7) { //upper bound, can not go faster
kensterino 4:90303483fd5f 180 f1_speed = 0.7;
kensterino 4:90303483fd5f 181 }
kensterino 5:a49a77ddf4e3 182 if (f1_speed <= 0.05)
kensterino 5:a49a77ddf4e3 183 f1_speed = 0.05;
kensterino 5:a49a77ddf4e3 184
kensterino 5:a49a77ddf4e3 185 if(f2_speed <= 0.05) { //lower bound, should not be slower than this
kensterino 5:a49a77ddf4e3 186 f2_speed = 0.05;
kensterino 4:90303483fd5f 187 }
kensterino 5:a49a77ddf4e3 188
kensterino 15:9b9bdbf4748a 189 //pc.printf(" f1: %f", f1_speed);
kensterino 15:9b9bdbf4748a 190 //pc.printf(" f2: %f", f2_speed);
kensterino 5:a49a77ddf4e3 191
kensterino 4:90303483fd5f 192 //problems when left wheel is held for the + case
kensterino 4:90303483fd5f 193 if (Error > 0) { //right wheel is turning more
kensterino 4:90303483fd5f 194 m_Left_F.write(f1_speed);
kensterino 4:90303483fd5f 195 m_Right_F.write(f2_speed); //f2_speed
kensterino 4:90303483fd5f 196 }
kensterino 4:90303483fd5f 197 if (Error < 0) { //left wheel is turning more
kensterino 4:90303483fd5f 198 m_Right_F.write(f1_speed);
kensterino 4:90303483fd5f 199 m_Left_F.write(f2_speed); //f2_speed
kensterino 4:90303483fd5f 200 }
kensterino 5:a49a77ddf4e3 201 if (Error == 0) {
kensterino 4:90303483fd5f 202 m_Right_F.write(i_speed);
kensterino 5:a49a77ddf4e3 203 m_Left_F.write(i_speed);
Joshua_Cheung 2:aa961ba3199e 204 }
kensterino 4:90303483fd5f 205 }
kensterino 4:90303483fd5f 206
kensterino 15:9b9bdbf4748a 207 void IRForward() {
kensterino 15:9b9bdbf4748a 208 printf(" IRError: %f\n " , IRError);
kensterino 15:9b9bdbf4748a 209 sum += IRError;
kensterino 15:9b9bdbf4748a 210 size++;
kensterino 15:9b9bdbf4748a 211 float avg = sum/size;
kensterino 15:9b9bdbf4748a 212 //printf(" avg : %f\n " ,avg);
kensterino 15:9b9bdbf4748a 213 double high_threshold = 0.25;
kensterino 15:9b9bdbf4748a 214 double low_threshold = 0.05;
kensterino 15:9b9bdbf4748a 215
kensterino 15:9b9bdbf4748a 216 if (IRError > 30) {
kensterino 15:9b9bdbf4748a 217 double high_speed = i_speed + IR_correction;
kensterino 15:9b9bdbf4748a 218 double low_speed = i_speed - IR_correction;
kensterino 15:9b9bdbf4748a 219 if (high_speed > high_threshold) //upper bound
kensterino 15:9b9bdbf4748a 220 high_speed = high_threshold;
kensterino 15:9b9bdbf4748a 221 if (high_speed < low_threshold)
kensterino 15:9b9bdbf4748a 222 high_speed = low_threshold;
kensterino 15:9b9bdbf4748a 223 if (low_speed < low_threshold)
kensterino 15:9b9bdbf4748a 224 low_speed = low_threshold;
kensterino 15:9b9bdbf4748a 225 if (low_speed > high_threshold)
kensterino 15:9b9bdbf4748a 226 low_speed = high_threshold;
kensterino 15:9b9bdbf4748a 227
kensterino 15:9b9bdbf4748a 228 m_Left_F.write(low_speed);
kensterino 15:9b9bdbf4748a 229 m_Right_F.write(high_speed);
kensterino 15:9b9bdbf4748a 230 printf(" + left: %f\n " , low_speed);
kensterino 15:9b9bdbf4748a 231 printf(" + right: %f\n " , high_speed);
kensterino 15:9b9bdbf4748a 232 printf("\n");
kensterino 15:9b9bdbf4748a 233 //wait(2);
kensterino 18:14bf07a27998 234 resetEncoders();
kensterino 15:9b9bdbf4748a 235 }
kensterino 15:9b9bdbf4748a 236 if (IRError < -30) {
kensterino 15:9b9bdbf4748a 237 double high_speed = i_speed - IR_correction;
kensterino 15:9b9bdbf4748a 238 double low_speed = i_speed + IR_correction;
kensterino 15:9b9bdbf4748a 239 if (high_speed > high_threshold) //upper bound
kensterino 15:9b9bdbf4748a 240 high_speed = high_threshold;
kensterino 15:9b9bdbf4748a 241 if (high_speed < low_threshold)
kensterino 15:9b9bdbf4748a 242 high_speed = low_threshold;
kensterino 15:9b9bdbf4748a 243 if (low_speed < low_threshold)
kensterino 15:9b9bdbf4748a 244 low_speed = low_threshold;
kensterino 15:9b9bdbf4748a 245 if (low_speed > high_threshold)
kensterino 15:9b9bdbf4748a 246 low_speed = high_threshold;
kensterino 15:9b9bdbf4748a 247 m_Left_F.write(high_speed);
kensterino 15:9b9bdbf4748a 248 m_Right_F.write(low_speed);
kensterino 15:9b9bdbf4748a 249 printf(" - left: %f\n " , high_speed);
kensterino 15:9b9bdbf4748a 250 printf(" - right: %f\n " , low_speed);
kensterino 15:9b9bdbf4748a 251 printf("\n");
kensterino 18:14bf07a27998 252 resetEncoders();
kensterino 15:9b9bdbf4748a 253 //wait(2);
kensterino 15:9b9bdbf4748a 254 }
kensterino 18:14bf07a27998 255 if (IRError <= 30 && IRError >= -30) {
kensterino 15:9b9bdbf4748a 256 forward();
kensterino 15:9b9bdbf4748a 257 printf("forward\n");
kensterino 15:9b9bdbf4748a 258 printf("\n");
kensterino 15:9b9bdbf4748a 259 }
kensterino 15:9b9bdbf4748a 260 }
kensterino 15:9b9bdbf4748a 261
mbshark 11:e647b14ff4ef 262 void forwardSlow()
mbshark 11:e647b14ff4ef 263 {
mbshark 11:e647b14ff4ef 264 float iS_speed = 0.05;
mbshark 11:e647b14ff4ef 265 double f1_speed = iS_speed + C_speed;
mbshark 11:e647b14ff4ef 266 double f2_speed = iS_speed - C_speed;
mbshark 11:e647b14ff4ef 267
mbshark 11:e647b14ff4ef 268 /*pc.printf("C: %f", C_speed);
mbshark 11:e647b14ff4ef 269 if (C_speed < 0)
mbshark 11:e647b14ff4ef 270 pc.printf("-");
mbshark 11:e647b14ff4ef 271 if (C_speed > 0)
mbshark 11:e647b14ff4ef 272 pc.printf("+");
mbshark 11:e647b14ff4ef 273 */
mbshark 11:e647b14ff4ef 274
mbshark 11:e647b14ff4ef 275
kensterino 15:9b9bdbf4748a 276 if(f1_speed >= 0.2) { //upper bound, can not go faster
kensterino 15:9b9bdbf4748a 277 f1_speed = 0.2;
mbshark 11:e647b14ff4ef 278 }
mbshark 11:e647b14ff4ef 279 if (f1_speed <= 0.05)
mbshark 11:e647b14ff4ef 280 f1_speed = 0.05;
mbshark 11:e647b14ff4ef 281
mbshark 11:e647b14ff4ef 282 if(f2_speed <= 0.05) { //lower bound, should not be slower than this
mbshark 11:e647b14ff4ef 283 f2_speed = 0.05;
mbshark 11:e647b14ff4ef 284 }
mbshark 11:e647b14ff4ef 285
kensterino 15:9b9bdbf4748a 286 //pc.printf(" f1: %f", f1_speed);
kensterino 15:9b9bdbf4748a 287 //pc.printf(" f2: %f", f2_speed);
mbshark 11:e647b14ff4ef 288
mbshark 11:e647b14ff4ef 289 //problems when left wheel is held for the + case
mbshark 11:e647b14ff4ef 290 if (Error > 0) { //right wheel is turning more
mbshark 11:e647b14ff4ef 291 m_Left_F.write(f1_speed);
mbshark 11:e647b14ff4ef 292 m_Right_F.write(f2_speed); //f2_speed
mbshark 11:e647b14ff4ef 293 }
mbshark 11:e647b14ff4ef 294 if (Error < 0) { //left wheel is turning more
mbshark 11:e647b14ff4ef 295 m_Right_F.write(f1_speed);
mbshark 11:e647b14ff4ef 296 m_Left_F.write(f2_speed); //f2_speed
mbshark 11:e647b14ff4ef 297 }
mbshark 11:e647b14ff4ef 298 if (Error == 0) {
mbshark 11:e647b14ff4ef 299 m_Right_F.write(iS_speed);
mbshark 11:e647b14ff4ef 300 m_Left_F.write(iS_speed);
mbshark 11:e647b14ff4ef 301 }
mbshark 11:e647b14ff4ef 302 }
kensterino 13:a208f8b052ad 303 void stop()
kensterino 13:a208f8b052ad 304 {
kensterino 13:a208f8b052ad 305 m_Right_F.write(0);
kensterino 13:a208f8b052ad 306 m_Right_B.write(0);
kensterino 13:a208f8b052ad 307 m_Left_F.write(0);
kensterino 13:a208f8b052ad 308 m_Left_B.write(0);
kensterino 13:a208f8b052ad 309 }
kensterino 13:a208f8b052ad 310
mbshark 11:e647b14ff4ef 311
Joshua_Cheung 7:e10dc3cb9212 312 void backUp()
Joshua_Cheung 7:e10dc3cb9212 313 {
Joshua_Cheung 7:e10dc3cb9212 314 m_Left_F.write(0);
Joshua_Cheung 7:e10dc3cb9212 315 m_Right_F.write(0);
Joshua_Cheung 7:e10dc3cb9212 316 m_Left_B.write(i_speed);
Joshua_Cheung 7:e10dc3cb9212 317 m_Right_B.write(i_speed);
Joshua_Cheung 7:e10dc3cb9212 318 wait(.15);
Joshua_Cheung 7:e10dc3cb9212 319 }
Joshua_Cheung 7:e10dc3cb9212 320
kensterino 15:9b9bdbf4748a 321 void backUpForTurn()
kensterino 4:90303483fd5f 322 {
kensterino 15:9b9bdbf4748a 323 stop();
kensterino 15:9b9bdbf4748a 324 while (RF_reading > 50 && LF_reading > 50) {
kensterino 15:9b9bdbf4748a 325 m_Left_B.write(i_speed);
kensterino 15:9b9bdbf4748a 326 m_Right_B.write(i_speed);
kensterino 15:9b9bdbf4748a 327 }
kensterino 15:9b9bdbf4748a 328 stop();
kensterino 15:9b9bdbf4748a 329 }
kensterino 4:90303483fd5f 330
kensterino 13:a208f8b052ad 331 void turnRight() {
kensterino 18:14bf07a27998 332 //double scale = RF_reading / LF_reading;
kensterino 15:9b9bdbf4748a 333 m_Left_F.write(0.5);
kensterino 15:9b9bdbf4748a 334 m_Right_B.write(0.5);
Joshua_Cheung 17:2b519a746a1e 335 resetEncoders();
kensterino 18:14bf07a27998 336 wait(0.1); //*scale);
Joshua_Cheung 17:2b519a746a1e 337
kensterino 13:a208f8b052ad 338 }
kensterino 9:20b8b64ed259 339
kensterino 15:9b9bdbf4748a 340 /*void turnRight() {
kensterino 15:9b9bdbf4748a 341 double turnSpeed = 0.1;
kensterino 15:9b9bdbf4748a 342 printf("Turning Error: %f\n " , IRError );
kensterino 15:9b9bdbf4748a 343 m_Left_F.write(turnSpeed+0.1);
kensterino 15:9b9bdbf4748a 344 m_Right_B.write(turnSpeed/2);
kensterino 15:9b9bdbf4748a 345 //while (IRError > 0.05 || IRError < -0.05)
kensterino 15:9b9bdbf4748a 346 while((LF_reading - RF_reading) > -24)
kensterino 15:9b9bdbf4748a 347 {printf("LF-RF: %f\n" , LF_reading-RF_reading); }
kensterino 15:9b9bdbf4748a 348 printf("Final Error: %f\n " , IRError);
kensterino 15:9b9bdbf4748a 349 stop();
kensterino 15:9b9bdbf4748a 350 }*/
kensterino 15:9b9bdbf4748a 351
kensterino 4:90303483fd5f 352 void turnLeft()
kensterino 4:90303483fd5f 353 {
Joshua_Cheung 17:2b519a746a1e 354 m_Right_F.write(0.5);
Joshua_Cheung 17:2b519a746a1e 355 m_Left_B.write(0.5);
Joshua_Cheung 17:2b519a746a1e 356 resetEncoders();
Joshua_Cheung 17:2b519a746a1e 357 wait(0.1); //this is dependent on i_speed, can we write a function that varies with i_speed?
kensterino 9:20b8b64ed259 358 }
kensterino 9:20b8b64ed259 359
kensterino 9:20b8b64ed259 360 void quarterTurnLeft() { //rev1, for ladders
kensterino 9:20b8b64ed259 361 m_Left_F.write(0);
kensterino 9:20b8b64ed259 362 m_Right_B.write(0);
kensterino 9:20b8b64ed259 363 m_Right_F.write(i_speed);
kensterino 9:20b8b64ed259 364 m_Left_B.write(i_speed);
kensterino 9:20b8b64ed259 365 wait(.1); //time needs testing
kensterino 4:90303483fd5f 366 }
kensterino 4:90303483fd5f 367
kensterino 4:90303483fd5f 368 void turnAround()
kensterino 4:90303483fd5f 369 {
Joshua_Cheung 7:e10dc3cb9212 370 m_Left_B.write(0);
Joshua_Cheung 7:e10dc3cb9212 371 m_Right_F.write(0);
Joshua_Cheung 7:e10dc3cb9212 372 m_Left_F.write(i_speed);
Joshua_Cheung 7:e10dc3cb9212 373 m_Right_B.write(i_speed);
Joshua_Cheung 17:2b519a746a1e 374 resetEncoders();
Joshua_Cheung 17:2b519a746a1e 375 wait(.6);
Joshua_Cheung 7:e10dc3cb9212 376 }
kensterino 4:90303483fd5f 377
kensterino 4:90303483fd5f 378 void debugEncoder()
kensterino 4:90303483fd5f 379 {
kensterino 4:90303483fd5f 380 while(1) {
kensterino 4:90303483fd5f 381 wait(1);
kensterino 4:90303483fd5f 382 pc.printf("Right: %i", encoder_Right.getPulses());
kensterino 4:90303483fd5f 383 pc.printf(" Left: %i", encoder_Left.getPulses(), "\n");
kensterino 4:90303483fd5f 384 pc.printf("\n");
kensterino 4:90303483fd5f 385 }
kensterino 4:90303483fd5f 386 }
kensterino 4:90303483fd5f 387
kensterino 4:90303483fd5f 388 void debugError()
kensterino 4:90303483fd5f 389 {
kensterino 4:90303483fd5f 390 while(1) {
kensterino 4:90303483fd5f 391 pc.printf("Error: %i\n", Error);
kensterino 4:90303483fd5f 392 }
kensterino 4:90303483fd5f 393 }
kensterino 4:90303483fd5f 394
Joshua_Cheung 7:e10dc3cb9212 395
Joshua_Cheung 10:707e542688dc 396 /*void main1() {
Joshua_Cheung 7:e10dc3cb9212 397 printf("\nAnalogIn example\n");
Joshua_Cheung 7:e10dc3cb9212 398 LF_IRE.write(1);
Joshua_Cheung 7:e10dc3cb9212 399 RF_IRE.write(1);
Joshua_Cheung 7:e10dc3cb9212 400 while (1){
kensterino 9:20b8b64ed259 401 while (LF_IRR.read() < threshold && RF_IRR.read() < threshold){
kensterino 9:20b8b64ed259 402 forward();
kensterino 9:20b8b64ed259 403 float value1 = LF_IRR.read();
kensterino 9:20b8b64ed259 404 float value2 = RF_IRR.read();
kensterino 9:20b8b64ed259 405 printf("LF Led: %f\n", value1);
kensterino 9:20b8b64ed259 406 wait(0.5);
kensterino 9:20b8b64ed259 407 printf("RF Led: %f\n", value2);
kensterino 9:20b8b64ed259 408 }
kensterino 9:20b8b64ed259 409
Joshua_Cheung 7:e10dc3cb9212 410 backUp();
Joshua_Cheung 8:6b2f7886768d 411 LS_IRE.write(1);
Joshua_Cheung 8:6b2f7886768d 412 RS_IRE.write(1);
kensterino 9:20b8b64ed259 413
kensterino 9:20b8b64ed259 414 if (LS_IRR.read() > turnThreshold) {
Joshua_Cheung 7:e10dc3cb9212 415 if (RS_IRR.read() < turnThreshold)
Joshua_Cheung 7:e10dc3cb9212 416 turnRight();
Joshua_Cheung 7:e10dc3cb9212 417 else
kensterino 9:20b8b64ed259 418 turnAround();
kensterino 9:20b8b64ed259 419 }
kensterino 9:20b8b64ed259 420
kensterino 9:20b8b64ed259 421 else if (RS_IRR.read() > turnThreshold) {
Joshua_Cheung 7:e10dc3cb9212 422 if (LS_IRR.read() < turnThreshold)
Joshua_Cheung 7:e10dc3cb9212 423 turnRight();
Joshua_Cheung 7:e10dc3cb9212 424 else
Joshua_Cheung 7:e10dc3cb9212 425 turnAround();
kensterino 9:20b8b64ed259 426 }
Joshua_Cheung 7:e10dc3cb9212 427 else
Joshua_Cheung 7:e10dc3cb9212 428 turnAround();
kensterino 9:20b8b64ed259 429
Joshua_Cheung 8:6b2f7886768d 430 LS_IRE.write(0);
Joshua_Cheung 8:6b2f7886768d 431 RS_IRE.write(0);
kensterino 9:20b8b64ed259 432 }
Joshua_Cheung 7:e10dc3cb9212 433 stop();
kensterino 9:20b8b64ed259 434 }
Joshua_Cheung 10:707e542688dc 435 */
mbshark 11:e647b14ff4ef 436
mbshark 11:e647b14ff4ef 437
kensterino 15:9b9bdbf4748a 438 bool testStop() //rev1
kensterino 9:20b8b64ed259 439 {
kensterino 15:9b9bdbf4748a 440 //bool stopOut=false;
kensterino 15:9b9bdbf4748a 441 //while(!stopOut) { //loop
kensterino 15:9b9bdbf4748a 442 //forward();
Joshua_Cheung 12:e44dd0d4f1ee 443
kensterino 15:9b9bdbf4748a 444 if (RF_reading > turnThreshold && LF_reading > turnThreshold && RS_reading > turnThreshold && LS_reading > turnThreshold ) //IR Receivers will read higher analog values than Threshold if wall is near
Joshua_Cheung 10:707e542688dc 445 {
kensterino 15:9b9bdbf4748a 446 printf(" 1 stop\n");
Joshua_Cheung 12:e44dd0d4f1ee 447 stop(); //stop the rat
kensterino 18:14bf07a27998 448 //wait(1); //delay optional
kensterino 15:9b9bdbf4748a 449 while(1){
kensterino 18:14bf07a27998 450 //forwardSlow();
kensterino 15:9b9bdbf4748a 451 if (RF_reading > threshold && LF_reading > threshold)
kensterino 15:9b9bdbf4748a 452 {
kensterino 15:9b9bdbf4748a 453 printf(" 2 stop\n");
kensterino 15:9b9bdbf4748a 454 stop();
kensterino 15:9b9bdbf4748a 455 return true;
kensterino 15:9b9bdbf4748a 456 break;
kensterino 15:9b9bdbf4748a 457 }
mbshark 11:e647b14ff4ef 458 }
Joshua_Cheung 12:e44dd0d4f1ee 459
kensterino 9:20b8b64ed259 460 }
kensterino 15:9b9bdbf4748a 461 return false;
Joshua_Cheung 10:707e542688dc 462 }
kensterino 15:9b9bdbf4748a 463 //printf(" end");
kensterino 15:9b9bdbf4748a 464 //}
kensterino 9:20b8b64ed259 465
mbshark 11:e647b14ff4ef 466
kensterino 9:20b8b64ed259 467 void testTurnAround() //rev1
kensterino 9:20b8b64ed259 468 {
kensterino 9:20b8b64ed259 469
kensterino 9:20b8b64ed259 470 }
kensterino 9:20b8b64ed259 471
kensterino 13:a208f8b052ad 472 void testRightTurn() {
kensterino 13:a208f8b052ad 473 forward();
kensterino 13:a208f8b052ad 474 while(1) {
kensterino 13:a208f8b052ad 475 if (LF_reading > threshold && RF_reading > threshold)
kensterino 13:a208f8b052ad 476 break;
kensterino 13:a208f8b052ad 477 }
kensterino 13:a208f8b052ad 478 float leftCount = encoder_Left.getPulses();
kensterino 13:a208f8b052ad 479 float rightCount = encoder_Right.getPulses();
kensterino 13:a208f8b052ad 480 while (encoder_Left.getPulses() < (leftCount + 180) && encoder_Right.getPulses() > rightCount + 180) {
kensterino 13:a208f8b052ad 481 m_Left_F.write(0.1);
kensterino 13:a208f8b052ad 482 m_Right_B.write(0.1);
kensterino 13:a208f8b052ad 483 }
kensterino 13:a208f8b052ad 484 stop();
kensterino 13:a208f8b052ad 485 }
kensterino 13:a208f8b052ad 486
kensterino 15:9b9bdbf4748a 487 /*void testTurns()
mbshark 11:e647b14ff4ef 488 {
mbshark 11:e647b14ff4ef 489 LF_IRE.write(1);
mbshark 11:e647b14ff4ef 490 RF_IRE.write(1);
mbshark 11:e647b14ff4ef 491 forward();
mbshark 11:e647b14ff4ef 492 while(1)
mbshark 11:e647b14ff4ef 493 {
mbshark 11:e647b14ff4ef 494 if (RF_IRR.read() > turnThreshold && LF_IRR.read() > turnThreshold)
mbshark 11:e647b14ff4ef 495 {
mbshark 11:e647b14ff4ef 496 stop();
mbshark 11:e647b14ff4ef 497 forwardSlow();
mbshark 11:e647b14ff4ef 498 if (RF_IRR.read() > threshold && LF_IRR.read() > threshold)
mbshark 11:e647b14ff4ef 499 {
mbshark 11:e647b14ff4ef 500 stop();
mbshark 11:e647b14ff4ef 501 if (LS_IRR.read() > turnThreshold) {
mbshark 11:e647b14ff4ef 502 if (RS_IRR.read() < turnThreshold)
mbshark 11:e647b14ff4ef 503 turnRight();
mbshark 11:e647b14ff4ef 504 else
mbshark 11:e647b14ff4ef 505 turnAround();
mbshark 11:e647b14ff4ef 506 }
mbshark 11:e647b14ff4ef 507
mbshark 11:e647b14ff4ef 508 else if (RS_IRR.read() > turnThreshold) {
mbshark 11:e647b14ff4ef 509 if (LS_IRR.read() < turnThreshold)
mbshark 11:e647b14ff4ef 510 turnLeft();
mbshark 11:e647b14ff4ef 511 else
mbshark 11:e647b14ff4ef 512 turnAround();
mbshark 11:e647b14ff4ef 513 }
mbshark 11:e647b14ff4ef 514 else
mbshark 11:e647b14ff4ef 515 turnAround();
mbshark 11:e647b14ff4ef 516
mbshark 11:e647b14ff4ef 517 LS_IRE.write(0);
mbshark 11:e647b14ff4ef 518 RS_IRE.write(0);
mbshark 11:e647b14ff4ef 519 }
mbshark 11:e647b14ff4ef 520 //stop();
mbshark 11:e647b14ff4ef 521 break;
mbshark 11:e647b14ff4ef 522 }
mbshark 11:e647b14ff4ef 523 }
mbshark 11:e647b14ff4ef 524 float value1 = LF_IRR.read();
mbshark 11:e647b14ff4ef 525 float value2 = RF_IRR.read();
mbshark 11:e647b14ff4ef 526 printf("LF Led: %f\n", value1);
mbshark 11:e647b14ff4ef 527 wait(0.25);
mbshark 11:e647b14ff4ef 528 printf("RF Led: %f\n\r", value2);
kensterino 15:9b9bdbf4748a 529 }*/
mbshark 11:e647b14ff4ef 530
kensterino 13:a208f8b052ad 531 void debugIR() {
kensterino 13:a208f8b052ad 532 while(1) {
kensterino 13:a208f8b052ad 533 printf("LS %f\n ", LS_reading);
kensterino 13:a208f8b052ad 534 wait(0.1);
kensterino 13:a208f8b052ad 535 printf("LF %f\n ", LF_reading);
kensterino 13:a208f8b052ad 536 wait(0.1);
kensterino 13:a208f8b052ad 537 printf("RS %f\n ", RS_reading);
kensterino 13:a208f8b052ad 538 wait(0.1);
kensterino 13:a208f8b052ad 539 printf("RF %f\n ", RF_reading);
kensterino 13:a208f8b052ad 540 wait(0.1);
kensterino 13:a208f8b052ad 541 printf("\n");
kensterino 13:a208f8b052ad 542 }
kensterino 13:a208f8b052ad 543 }
kensterino 15:9b9bdbf4748a 544
kensterino 15:9b9bdbf4748a 545 void afterTurnForward() {
kensterino 18:14bf07a27998 546 resetEncoders();
Joshua_Cheung 17:2b519a746a1e 547 Timer turn;
Joshua_Cheung 17:2b519a746a1e 548 turn.start();
Joshua_Cheung 17:2b519a746a1e 549 int current = turn.read_ms();
kensterino 18:14bf07a27998 550 while ((turn.read_ms() - current) < 500)
kensterino 18:14bf07a27998 551 { forward();
Joshua_Cheung 17:2b519a746a1e 552 printf("%f\n", turn.read_ms());
Joshua_Cheung 17:2b519a746a1e 553 }
Joshua_Cheung 17:2b519a746a1e 554 printf("out of after\n");
Joshua_Cheung 17:2b519a746a1e 555 stop();
kensterino 18:14bf07a27998 556 //resetEncoders();
kensterino 18:14bf07a27998 557 wait(1);
kensterino 15:9b9bdbf4748a 558 }
Joshua_Cheung 17:2b519a746a1e 559
Joshua_Cheung 17:2b519a746a1e 560 void LeftOrRight() {
Joshua_Cheung 17:2b519a746a1e 561 while(1){
Joshua_Cheung 17:2b519a746a1e 562 if(IRError > 30){
Joshua_Cheung 17:2b519a746a1e 563 turnLeft();
Joshua_Cheung 17:2b519a746a1e 564 stop();
Joshua_Cheung 17:2b519a746a1e 565 break;
Joshua_Cheung 17:2b519a746a1e 566 }
Joshua_Cheung 17:2b519a746a1e 567 else if(IRError < -30){
Joshua_Cheung 17:2b519a746a1e 568 turnRight();
Joshua_Cheung 17:2b519a746a1e 569 stop();
Joshua_Cheung 17:2b519a746a1e 570 break;
Joshua_Cheung 17:2b519a746a1e 571 }
Joshua_Cheung 17:2b519a746a1e 572 else{
Joshua_Cheung 17:2b519a746a1e 573 stop();
Joshua_Cheung 17:2b519a746a1e 574 //break;
Joshua_Cheung 17:2b519a746a1e 575 }
Joshua_Cheung 17:2b519a746a1e 576 }
Joshua_Cheung 17:2b519a746a1e 577 }
kensterino 9:20b8b64ed259 578
kensterino 9:20b8b64ed259 579 int main()
kensterino 9:20b8b64ed259 580 {
kensterino 9:20b8b64ed259 581 systicker.attach_us(&systick, 1000); //rev1
kensterino 15:9b9bdbf4748a 582 wait(1); //DONT DELETE INITIAL WAIT
kensterino 15:9b9bdbf4748a 583 //testStop();
Joshua_Cheung 17:2b519a746a1e 584 //turnAround();
Joshua_Cheung 17:2b519a746a1e 585
kensterino 18:14bf07a27998 586 while (1) {
kensterino 18:14bf07a27998 587
kensterino 15:9b9bdbf4748a 588 IRForward();
kensterino 15:9b9bdbf4748a 589 if (testStop()) {
kensterino 15:9b9bdbf4748a 590 backUpForTurn();
Joshua_Cheung 17:2b519a746a1e 591 LeftOrRight();
kensterino 15:9b9bdbf4748a 592 stop();
kensterino 18:14bf07a27998 593 wait(0.05);
kensterino 15:9b9bdbf4748a 594 afterTurnForward();
kensterino 18:14bf07a27998 595 //wait(0.5);
Joshua_Cheung 17:2b519a746a1e 596 stop();
kensterino 15:9b9bdbf4748a 597 }
kensterino 18:14bf07a27998 598
kensterino 18:14bf07a27998 599 /*if (corner) { //TEST CORNER() test
kensterino 18:14bf07a27998 600 turnAround();
kensterino 18:14bf07a27998 601 stop();
kensterino 18:14bf07a27998 602 break;
kensterino 18:14bf07a27998 603 }
kensterino 18:14bf07a27998 604 */
kensterino 18:14bf07a27998 605 //printf("%d\n", RF_reading);
kensterino 15:9b9bdbf4748a 606 }
kensterino 18:14bf07a27998 607
kensterino 9:20b8b64ed259 608 }