The Dream Team / Mbed 2 deprecated first_test

Pop lock n drop it

Dependencies:   m3pi_ng mbed

main.cpp@10:86cb40bf1c17, 2014-06-02 (annotated)

Committer:
vsal
Date:
Mon Jun 02 08:44:08 2014 +0000
Revision:
10:86cb40bf1c17
Parent:
9:493011bacdcd
Child:
11:af3d70d153bb
Final before trying to turn on map

Who changed what in which revision?

UserRevisionLine numberNew contents of line
vsal 0:80d9725a7342 1 #include "mbed.h"
vsal 0:80d9725a7342 2 #include "m3pi_ng.h"
vsal 3:6f773c2ba3a0 3 #include "DigitalIn.h"
vsal 8:09f4495696d2 4 #include "btbee.h"
vsal 9:493011bacdcd 5 #include "time.h"
vsal 8:09f4495696d2 6
vsal 0:80d9725a7342 7
vsal 0:80d9725a7342 8 DigitalOut myled(LED1);
vsal 0:80d9725a7342 9 m3pi huey;
vsal 8:09f4495696d2 10 btbee btbee;
vsal 8:09f4495696d2 11
vsal 3:6f773c2ba3a0 12 DigitalIn m3pi_IN[]={(p12),(p21)}; // IR-Sensor
vsal 8:09f4495696d2 13 DigitalOut mbed_led[] = {(LED1), (LED2),(LED3), (LED4)};
vsal 8:09f4495696d2 14 DigitalOut m3pi_led[] = {(p13), (p14), (p15), (p16), (p17), (p18), (p19), (p20)};
vsal 8:09f4495696d2 15 DigitalIn m3pi_pb(p21);
vsal 1:ac7fc0b5f878 16
vsal 1:ac7fc0b5f878 17 void printBattery()
vsal 1:ac7fc0b5f878 18 {
vsal 0:80d9725a7342 19 //prints battery voltage on led screen
vsal 0:80d9725a7342 20 float bat = huey.battery();
vsal 1:ac7fc0b5f878 21 huey.printf("Bat volt");
vsal 1:ac7fc0b5f878 22 huey.locate(0,1);
vsal 1:ac7fc0b5f878 23 huey.printf("%f",bat);
vsal 1:ac7fc0b5f878 24 }
vsal 1:ac7fc0b5f878 25
vsal 4:14b00a82d206 26 float myLinePos(void)
vsal 4:14b00a82d206 27 {
vsal 4:14b00a82d206 28 int sensors[5];
vsal 4:14b00a82d206 29 huey.calibrated_sensor(sensors);
vsal 7:d0c19729e7fd 30 //oat leftSen, rightSen, midSen;
vsal 7:d0c19729e7fd 31 //ftSen = sensors[1] / 1000;
vsal 7:d0c19729e7fd 32 //ghtSen = sensors[3] / 1000;
vsal 7:d0c19729e7fd 33 //dSen = sensors[2] / 1000;
vsal 7:d0c19729e7fd 34 float val;
vsal 10:86cb40bf1c17 35 if(sensors[1] < 50 && sensors[2] < 50 && sensors[3] < 50)
vsal 10:86cb40bf1c17 36 return -1;
vsal 10:86cb40bf1c17 37 else
vsal 10:86cb40bf1c17 38 {
vsal 10:86cb40bf1c17 39 val = (((1000*sensors[2] + 2000*sensors[3])/(sensors[1]+sensors[2]+sensors[3])));
vsal 10:86cb40bf1c17 40 return (-1)+(val/1000);
vsal 10:86cb40bf1c17 41 }
vsal 4:14b00a82d206 42 }
vsal 4:14b00a82d206 43
vsal 8:09f4495696d2 44 int rightCorner()
vsal 8:09f4495696d2 45 {
vsal 8:09f4495696d2 46 int sensors[5];
vsal 8:09f4495696d2 47 huey.calibrated_sensor(sensors);
vsal 10:86cb40bf1c17 48 if(sensors[4] > 950)
vsal 8:09f4495696d2 49 return 1;
vsal 8:09f4495696d2 50 else
vsal 8:09f4495696d2 51 return 0;
vsal 8:09f4495696d2 52 }
vsal 8:09f4495696d2 53
vsal 8:09f4495696d2 54 int leftCorner()
vsal 8:09f4495696d2 55 {
vsal 8:09f4495696d2 56 int sensors[5];
vsal 8:09f4495696d2 57 huey.calibrated_sensor(sensors);
vsal 10:86cb40bf1c17 58 if(sensors[0] > 950)
vsal 8:09f4495696d2 59 return 1;
vsal 8:09f4495696d2 60 else
vsal 8:09f4495696d2 61 return 0;
vsal 8:09f4495696d2 62 }
vsal 8:09f4495696d2 63
vsal 10:86cb40bf1c17 64 //returns 0 if side sensors aren't light, 1 if light
vsal 10:86cb40bf1c17 65 int sideSensorsLight(void)
vsal 10:86cb40bf1c17 66 {
vsal 10:86cb40bf1c17 67 int sensors[5];
vsal 10:86cb40bf1c17 68 huey.calibrated_sensor(sensors);
vsal 10:86cb40bf1c17 69 if(sensors[4] > 50 && sensors[0] > 50)
vsal 10:86cb40bf1c17 70 return 0;
vsal 10:86cb40bf1c17 71 else
vsal 10:86cb40bf1c17 72 return 1;
vsal 10:86cb40bf1c17 73 }
vsal 10:86cb40bf1c17 74
vsal 10:86cb40bf1c17 75 //returns 0 if side sensors aren't dark, 1 if dark
vsal 10:86cb40bf1c17 76 int sideSensorsDark(void)
vsal 9:493011bacdcd 77 {
vsal 9:493011bacdcd 78 if(rightCorner() == 1 && leftCorner() == 1)
vsal 10:86cb40bf1c17 79 return 1;
vsal 10:86cb40bf1c17 80 else
vsal 10:86cb40bf1c17 81 return 0;
vsal 10:86cb40bf1c17 82 }
vsal 10:86cb40bf1c17 83
vsal 10:86cb40bf1c17 84 void rightTurn(float speed)
vsal 10:86cb40bf1c17 85 {
vsal 10:86cb40bf1c17 86 int side = sideSensorsLight();
vsal 10:86cb40bf1c17 87 huey.forward(speed/2);
vsal 10:86cb40bf1c17 88 while(side == 0)
vsal 9:493011bacdcd 89 {
vsal 10:86cb40bf1c17 90 side = sideSensorsLight();
vsal 9:493011bacdcd 91 }
vsal 10:86cb40bf1c17 92 wait(0.20);
vsal 10:86cb40bf1c17 93 float pos = myLinePos();
vsal 10:86cb40bf1c17 94 huey.right(speed/2);
vsal 10:86cb40bf1c17 95 while(pos > -0.1 && pos < 0.1)
vsal 9:493011bacdcd 96 {
vsal 10:86cb40bf1c17 97 pos = myLinePos();
vsal 9:493011bacdcd 98 }
vsal 10:86cb40bf1c17 99 pos = myLinePos();
vsal 10:86cb40bf1c17 100 while(pos > 0.05 || pos < -0.05)
vsal 10:86cb40bf1c17 101 {
vsal 10:86cb40bf1c17 102 pos = myLinePos();
vsal 10:86cb40bf1c17 103 }
vsal 9:493011bacdcd 104 }
vsal 10:86cb40bf1c17 105
vsal 10:86cb40bf1c17 106 void leftTurn(float speed)
vsal 10:86cb40bf1c17 107 {
vsal 10:86cb40bf1c17 108 int side = sideSensorsLight();
vsal 10:86cb40bf1c17 109 huey.forward(speed/2);
vsal 10:86cb40bf1c17 110 while(side == 0)
vsal 10:86cb40bf1c17 111 {
vsal 10:86cb40bf1c17 112 side = sideSensorsLight();
vsal 10:86cb40bf1c17 113 }
vsal 10:86cb40bf1c17 114 wait(0.20);
vsal 10:86cb40bf1c17 115
vsal 10:86cb40bf1c17 116 float pos = myLinePos();
vsal 10:86cb40bf1c17 117 huey.left(speed/2);
vsal 10:86cb40bf1c17 118 while(pos > -0.1 && pos < 0.1)
vsal 10:86cb40bf1c17 119 {
vsal 10:86cb40bf1c17 120 pos = myLinePos();
vsal 10:86cb40bf1c17 121 }
vsal 10:86cb40bf1c17 122 pos = myLinePos();
vsal 10:86cb40bf1c17 123 while(pos > 0.05 || pos < -0.05)
vsal 10:86cb40bf1c17 124 {
vsal 10:86cb40bf1c17 125 pos = myLinePos();
vsal 10:86cb40bf1c17 126 }
vsal 10:86cb40bf1c17 127 }
vsal 10:86cb40bf1c17 128
vsal 9:493011bacdcd 129 void smoothFollow(float position, float speed)//, int direction)
vsal 1:ac7fc0b5f878 130 {
vsal 4:14b00a82d206 131 float u = speed;
vsal 1:ac7fc0b5f878 132 u = u * position;
vsal 10:86cb40bf1c17 133
vsal 9:493011bacdcd 134 if(rightCorner() == 1)
vsal 10:86cb40bf1c17 135 {
vsal 10:86cb40bf1c17 136 rightTurn(speed);
vsal 10:86cb40bf1c17 137 /*int side = sideSensorsLight();
vsal 10:86cb40bf1c17 138 huey.forward(speed/2);
vsal 10:86cb40bf1c17 139 while(side == 0)
vsal 9:493011bacdcd 140 {
vsal 10:86cb40bf1c17 141 side = sideSensorsLight();
vsal 9:493011bacdcd 142 }
vsal 10:86cb40bf1c17 143 wait(0.20);
vsal 10:86cb40bf1c17 144 float pos = myLinePos();
vsal 10:86cb40bf1c17 145 huey.right(speed/2);
vsal 10:86cb40bf1c17 146 while(pos > -0.1 && pos < 0.1)
vsal 10:86cb40bf1c17 147 {
vsal 10:86cb40bf1c17 148 pos = myLinePos();
vsal 10:86cb40bf1c17 149 }
vsal 10:86cb40bf1c17 150 pos = myLinePos();
vsal 9:493011bacdcd 151 while(pos > 0.05 || pos < -0.05)
vsal 9:493011bacdcd 152 {
vsal 9:493011bacdcd 153 pos = myLinePos();
vsal 10:86cb40bf1c17 154 }*/
vsal 9:493011bacdcd 155 }
vsal 9:493011bacdcd 156 else if(leftCorner() == 1)
vsal 9:493011bacdcd 157 {
vsal 10:86cb40bf1c17 158 leftTurn(speed);
vsal 10:86cb40bf1c17 159 /*
vsal 10:86cb40bf1c17 160 int side = sideSensorsLight();
vsal 10:86cb40bf1c17 161 huey.forward(speed/2);
vsal 10:86cb40bf1c17 162 while(side == 0)
vsal 10:86cb40bf1c17 163 {
vsal 10:86cb40bf1c17 164 side = sideSensorsLight();
vsal 10:86cb40bf1c17 165 }
vsal 10:86cb40bf1c17 166 wait(0.20);
vsal 10:86cb40bf1c17 167
vsal 10:86cb40bf1c17 168 float pos = myLinePos();
vsal 10:86cb40bf1c17 169 huey.left(speed/2);
vsal 10:86cb40bf1c17 170 while(pos > -0.1 && pos < 0.1)
vsal 10:86cb40bf1c17 171 {
vsal 10:86cb40bf1c17 172 pos = myLinePos();
vsal 10:86cb40bf1c17 173 }
vsal 10:86cb40bf1c17 174 pos = myLinePos();
vsal 10:86cb40bf1c17 175 while(pos > 0.05 || pos < -0.05)
vsal 10:86cb40bf1c17 176 {
vsal 10:86cb40bf1c17 177 pos = myLinePos();
vsal 10:86cb40bf1c17 178 }*/
vsal 9:493011bacdcd 179 }
vsal 9:493011bacdcd 180 else if(speed+u > 1)
vsal 9:493011bacdcd 181 {
vsal 9:493011bacdcd 182 huey.stop();
vsal 9:493011bacdcd 183 huey.printf("Fast Er");
vsal 9:493011bacdcd 184 //return 0;
vsal 9:493011bacdcd 185 }
vsal 9:493011bacdcd 186 else
vsal 9:493011bacdcd 187 {
vsal 9:493011bacdcd 188 huey.right_motor(speed-u);
vsal 9:493011bacdcd 189 huey.left_motor(speed+u);
vsal 9:493011bacdcd 190 }
vsal 1:ac7fc0b5f878 191 }
vsal 1:ac7fc0b5f878 192
vsal 9:493011bacdcd 193
vsal 9:493011bacdcd 194
vsal 9:493011bacdcd 195
vsal 3:6f773c2ba3a0 196 void slowStop(float speed, float waitTime, int steps)
vsal 1:ac7fc0b5f878 197 {
vsal 3:6f773c2ba3a0 198 float i;
vsal 3:6f773c2ba3a0 199 for(i = speed; i > 0; i = i - (speed/steps))
vsal 1:ac7fc0b5f878 200 {
vsal 10:86cb40bf1c17 201
vsal 3:6f773c2ba3a0 202 smoothFollow(huey.line_position(),i);
vsal 3:6f773c2ba3a0 203 wait(waitTime);
vsal 1:ac7fc0b5f878 204 }
vsal 3:6f773c2ba3a0 205 huey.stop();
vsal 1:ac7fc0b5f878 206 }
vsal 1:ac7fc0b5f878 207
vsal 3:6f773c2ba3a0 208
vsal 3:6f773c2ba3a0 209 int pushToStart(void)
vsal 3:6f773c2ba3a0 210 {
vsal 3:6f773c2ba3a0 211 DigitalIn button(p21);
vsal 3:6f773c2ba3a0 212 int value = button.read();
vsal 3:6f773c2ba3a0 213 return value;
vsal 3:6f773c2ba3a0 214 }
vsal 3:6f773c2ba3a0 215
vsal 3:6f773c2ba3a0 216
vsal 8:09f4495696d2 217
vsal 3:6f773c2ba3a0 218 /////////////////////////////////////////////////////////////////
vsal 1:ac7fc0b5f878 219 int main() {
vsal 10:86cb40bf1c17 220
vsal 10:86cb40bf1c17 221
vsal 10:86cb40bf1c17 222 //Matrix of paths from one point to another: Right = R, Left = L, Straight = S
vsal 10:86cb40bf1c17 223 char* map[6][6] =
vsal 10:86cb40bf1c17 224 {
vsal 10:86cb40bf1c17 225 {"", "LRLLR", "LRLSS", "LRLSRLL", "LRRLSLRRRL", "LRRLR"},
vsal 10:86cb40bf1c17 226 {"LRRLR", "", "LLS", "LLRLL", "LSRLRRL", "LRSLR"},
vsal 10:86cb40bf1c17 227 {"SSRLR", "SRR", "", "LLL", "LLRSRL", "SSSLR"},
vsal 10:86cb40bf1c17 228 {"RLSRLR", "RLRR", "RR", "", "SSRL", "SRLRL"},
vsal 10:86cb40bf1c17 229 {"RLLLRSRLLR", "RLLRLSR", "RLSLR", "RLSS", "", "RLLLRL"},
vsal 10:86cb40bf1c17 230 {"LRLLR", "LRSLR", "LRSSS", "RLRLS", "RLRRRL", ""}
vsal 10:86cb40bf1c17 231 };
vsal 10:86cb40bf1c17 232
vsal 10:86cb40bf1c17 233
vsal 10:86cb40bf1c17 234
vsal 10:86cb40bf1c17 235
vsal 10:86cb40bf1c17 236
vsal 9:493011bacdcd 237 //time_t startt, endt;
vsal 9:493011bacdcd 238 //time(&startt);
vsal 9:493011bacdcd 239 //clock_t clock;
vsal 3:6f773c2ba3a0 240
vsal 8:09f4495696d2 241 //btbee trial
vsal 8:09f4495696d2 242 // initialization stuff ////////////////////////////////////////////////////////////////////////////////////////////////////
vsal 8:09f4495696d2 243 huey.locate(0,1);
vsal 8:09f4495696d2 244 btbee.reset();
vsal 8:09f4495696d2 245 for (int i = 0; i <4; i++) {
vsal 8:09f4495696d2 246 mbed_led[i] = 0;
vsal 8:09f4495696d2 247 }
vsal 8:09f4495696d2 248 for (int i = 0; i <8; i++) {
vsal 8:09f4495696d2 249 m3pi_led[i]=0;
vsal 8:09f4495696d2 250 }
vsal 8:09f4495696d2 251 m3pi_pb.mode(PullUp); // expected would be 1 when pb is pressed, 0 when not, opposite is the case
vsal 8:09f4495696d2 252
vsal 8:09f4495696d2 253 // end initialization stuff ////////////////////////////////////////////////////////////////////////////////////////////////
vsal 8:09f4495696d2 254
vsal 8:09f4495696d2 255 huey.locate(0,0);
vsal 8:09f4495696d2 256 huey.printf("B:%0.3f%V",huey.battery());
vsal 8:09f4495696d2 257 wait(0.3);
vsal 8:09f4495696d2 258
vsal 8:09f4495696d2 259 huey.locate(0,0);
vsal 8:09f4495696d2 260 huey.printf("%s","btTest");
vsal 8:09f4495696d2 261 huey.locate(0,1);
vsal 8:09f4495696d2 262 huey.printf("%s","PBonLNK");
vsal 8:09f4495696d2 263
vsal 8:09f4495696d2 264 // wait for the user to push P21, should be pressed when the bt link is established (green led "link")
vsal 8:09f4495696d2 265 while(m3pi_pb) {
vsal 8:09f4495696d2 266 m3pi_led[0]=!m3pi_led[0];
vsal 8:09f4495696d2 267 wait(0.1);
vsal 8:09f4495696d2 268 }
vsal 8:09f4495696d2 269
vsal 8:09f4495696d2 270 int iline=1;
vsal 8:09f4495696d2 271 char arr_read[30]; // this should be long enough to store any reply coming in over bt.
vsal 8:09f4495696d2 272 int chars_read; // number of chars read in a bt reply
vsal 8:09f4495696d2 273
vsal 9:493011bacdcd 274 while (iline < 3) {
vsal 8:09f4495696d2 275 // this writes "Line 001\n" to "Line 005\n" and then "end\n" to the btbee
vsal 8:09f4495696d2 276 if ( btbee.writeable() ) {
vsal 9:493011bacdcd 277 if (iline==2) {
vsal 9:493011bacdcd 278 //btbee.printf("end\n");
vsal 8:09f4495696d2 279 iline++;
vsal 8:09f4495696d2 280 }//if
vsal 8:09f4495696d2 281 else {
vsal 9:493011bacdcd 282 if (iline <2){
vsal 9:493011bacdcd 283 //time(&endt);
vsal 9:493011bacdcd 284 //btbee.printf("%f seconds - ", clock);
vsal 10:86cb40bf1c17 285 //btbee.printf("Battery Voltage: %f\n", huey.battery());
vsal 8:09f4495696d2 286 m3pi_led[0]=0;
vsal 8:09f4495696d2 287 huey.locate(0,0);
vsal 8:09f4495696d2 288 huey.printf("Sent %0.3d",iline);
vsal 8:09f4495696d2 289 iline++;
vsal 8:09f4495696d2 290 }
vsal 8:09f4495696d2 291 }//else
vsal 8:09f4495696d2 292 }//if_write
vsal 8:09f4495696d2 293
vsal 8:09f4495696d2 294 // check for answers after each write /not write
vsal 8:09f4495696d2 295 while ( btbee.readable() ) {
vsal 8:09f4495696d2 296 m3pi_led[7]=1;
vsal 8:09f4495696d2 297 btbee.read_all(arr_read, 30, &chars_read );
vsal 8:09f4495696d2 298 m3pi_led[6]=1;
vsal 8:09f4495696d2 299 huey.locate(0,1);
vsal 8:09f4495696d2 300 huey.print(arr_read,chars_read);
vsal 8:09f4495696d2 301 m3pi_led[5]=1;
vsal 8:09f4495696d2 302 }//while_readable
vsal 8:09f4495696d2 303 wait(0.1);
vsal 8:09f4495696d2 304 }//while_true
vsal 9:493011bacdcd 305
vsal 8:09f4495696d2 306 //main
vsal 8:09f4495696d2 307 //end of btbee trial
vsal 8:09f4495696d2 308
vsal 8:09f4495696d2 309
vsal 7:d0c19729e7fd 310
vsal 3:6f773c2ba3a0 311 m3pi_IN[0].mode(PullUp);
vsal 3:6f773c2ba3a0 312 m3pi_IN[1].mode(PullUp);
vsal 3:6f773c2ba3a0 313
vsal 3:6f773c2ba3a0 314 int start;
vsal 3:6f773c2ba3a0 315 do
vsal 3:6f773c2ba3a0 316 {
vsal 3:6f773c2ba3a0 317 start = pushToStart();
vsal 3:6f773c2ba3a0 318 }while(start == 1);
vsal 3:6f773c2ba3a0 319
vsal 3:6f773c2ba3a0 320 wait(1);
vsal 1:ac7fc0b5f878 321 //calibrates sensors
vsal 1:ac7fc0b5f878 322 huey.sensor_auto_calibrate();
vsal 1:ac7fc0b5f878 323
vsal 3:6f773c2ba3a0 324 printBattery();
vsal 0:80d9725a7342 325
vsal 0:80d9725a7342 326
vsal 2:e513ada0ecf1 327 float speed = 0.25;
vsal 1:ac7fc0b5f878 328 float pos;
vsal 1:ac7fc0b5f878 329 int z=1;
vsal 10:86cb40bf1c17 330
vsal 9:493011bacdcd 331
vsal 1:ac7fc0b5f878 332 while(z==1)
vsal 1:ac7fc0b5f878 333 {
vsal 10:86cb40bf1c17 334
vsal 9:493011bacdcd 335
vsal 9:493011bacdcd 336
vsal 9:493011bacdcd 337 pos = myLinePos();
vsal 9:493011bacdcd 338
vsal 1:ac7fc0b5f878 339 smoothFollow(pos, speed);
vsal 9:493011bacdcd 340
vsal 3:6f773c2ba3a0 341 if(m3pi_IN[0]==0)
vsal 3:6f773c2ba3a0 342 {
vsal 3:6f773c2ba3a0 343 slowStop(speed, 0.05, 3);
vsal 3:6f773c2ba3a0 344 huey.stop();
vsal 9:493011bacdcd 345 btbee.printf("Object in Front");
vsal 3:6f773c2ba3a0 346 while(m3pi_IN[0]==0)
vsal 3:6f773c2ba3a0 347 {
vsal 9:493011bacdcd 348 huey.stop();
vsal 3:6f773c2ba3a0 349 }
vsal 3:6f773c2ba3a0 350 }
vsal 9:493011bacdcd 351 if(pushToStart() == 0)
vsal 9:493011bacdcd 352 {
vsal 9:493011bacdcd 353 btbee.printf("end\n");
vsal 9:493011bacdcd 354 break;
vsal 9:493011bacdcd 355 }
vsal 1:ac7fc0b5f878 356 }
vsal 1:ac7fc0b5f878 357
vsal 1:ac7fc0b5f878 358
vsal 0:80d9725a7342 359
vsal 0:80d9725a7342 360
vsal 0:80d9725a7342 361
vsal 0:80d9725a7342 362
vsal 0:80d9725a7342 363 while(1)
vsal 0:80d9725a7342 364 {
vsal 0:80d9725a7342 365 myled = 1;
vsal 0:80d9725a7342 366 wait(0.2);
vsal 0:80d9725a7342 367 myled = 0;
vsal 0:80d9725a7342 368 wait(0.2);
vsal 0:80d9725a7342 369 }
vsal 0:80d9725a7342 370 }
vsal 1:ac7fc0b5f878 371