The Dream Team / Mbed 2 deprecated first_test

Pop lock n drop it

Dependencies:   m3pi_ng mbed

main.cpp@9:493011bacdcd, 2014-05-26 (annotated)

Committer:
vsal
Date:
Mon May 26 09:50:52 2014 +0000
Revision:
9:493011bacdcd
Parent:
8:09f4495696d2
Child:
10:86cb40bf1c17
Final from Monday

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 7:d0c19729e7fd 35 val = (((1000*sensors[2] + 2000*sensors[3])/(sensors[1]+sensors[2]+sensors[3])));
vsal 7:d0c19729e7fd 36 return -1+(val/1000);
vsal 7:d0c19729e7fd 37
vsal 4:14b00a82d206 38 }
vsal 4:14b00a82d206 39
vsal 8:09f4495696d2 40 int rightCorner()
vsal 8:09f4495696d2 41 {
vsal 8:09f4495696d2 42 int sensors[5];
vsal 8:09f4495696d2 43 huey.calibrated_sensor(sensors);
vsal 8:09f4495696d2 44 if(sensors[4] > 900)
vsal 8:09f4495696d2 45 return 1;
vsal 8:09f4495696d2 46 else
vsal 8:09f4495696d2 47 return 0;
vsal 8:09f4495696d2 48 }
vsal 8:09f4495696d2 49
vsal 8:09f4495696d2 50 int leftCorner()
vsal 8:09f4495696d2 51 {
vsal 8:09f4495696d2 52 int sensors[5];
vsal 8:09f4495696d2 53 huey.calibrated_sensor(sensors);
vsal 8:09f4495696d2 54 if(sensors[0] > 900)
vsal 8:09f4495696d2 55 return 1;
vsal 8:09f4495696d2 56 else
vsal 8:09f4495696d2 57 return 0;
vsal 8:09f4495696d2 58 }
vsal 8:09f4495696d2 59
vsal 9:493011bacdcd 60 //direction varriable is between 0 and 1, 0 meaning left 1 meaning right
vsal 9:493011bacdcd 61 //returns 1 if it turned, 0 otherwise
vsal 9:493011bacdcd 62 /*int crossing(float speed, int direction)
vsal 9:493011bacdcd 63 {
vsal 9:493011bacdcd 64 if(rightCorner() == 1 && leftCorner() == 1)
vsal 9:493011bacdcd 65 {
vsal 9:493011bacdcd 66 if(direction == 0)
vsal 9:493011bacdcd 67 {
vsal 9:493011bacdcd 68 huey.left(speed*2);
vsal 9:493011bacdcd 69 return 1;
vsal 9:493011bacdcd 70 }
vsal 9:493011bacdcd 71 else
vsal 9:493011bacdcd 72 {
vsal 9:493011bacdcd 73 huey.right(speed*2);
vsal 9:493011bacdcd 74 return 1;
vsal 9:493011bacdcd 75 }
vsal 9:493011bacdcd 76 }
vsal 9:493011bacdcd 77 else
vsal 9:493011bacdcd 78 {
vsal 9:493011bacdcd 79 if(rightCorner() == 1)
vsal 9:493011bacdcd 80 {
vsal 9:493011bacdcd 81 huey.right(speed*2);
vsal 9:493011bacdcd 82 return 0;
vsal 9:493011bacdcd 83 }
vsal 9:493011bacdcd 84 else if(leftCorner() == 1)
vsal 9:493011bacdcd 85 {
vsal 9:493011bacdcd 86 huey.left(speed*2);
vsal 9:493011bacdcd 87 return 0;
vsal 9:493011bacdcd 88 }
vsal 9:493011bacdcd 89 else
vsal 9:493011bacdcd 90 return 0;
vsal 9:493011bacdcd 91 }
vsal 8:09f4495696d2 92
vsal 9:493011bacdcd 93 }
vsal 9:493011bacdcd 94 */
vsal 9:493011bacdcd 95 void smoothFollow(float position, float speed)//, int direction)
vsal 1:ac7fc0b5f878 96 {
vsal 4:14b00a82d206 97 float u = speed;
vsal 1:ac7fc0b5f878 98 u = u * position;
vsal 8:09f4495696d2 99
vsal 9:493011bacdcd 100 /*if(rightCorner() == 1 && leftCorner() == 1)
vsal 8:09f4495696d2 101 {
vsal 9:493011bacdcd 102 if(direction == 0)
vsal 9:493011bacdcd 103 {
vsal 9:493011bacdcd 104 huey.left(speed*2);
vsal 9:493011bacdcd 105 return 1;
vsal 9:493011bacdcd 106 }
vsal 9:493011bacdcd 107 else
vsal 9:493011bacdcd 108 {
vsal 9:493011bacdcd 109 huey.right(speed*2);
vsal 9:493011bacdcd 110 return 1;
vsal 9:493011bacdcd 111 }
vsal 1:ac7fc0b5f878 112 }
vsal 1:ac7fc0b5f878 113 else
vsal 9:493011bacdcd 114 {*/
vsal 9:493011bacdcd 115 if(rightCorner() == 1)
vsal 9:493011bacdcd 116 {
vsal 9:493011bacdcd 117 int sensors[5];
vsal 9:493011bacdcd 118 huey.calibrated_sensor(sensors);
vsal 9:493011bacdcd 119 while(sensors[4] > 100)
vsal 9:493011bacdcd 120 {
vsal 9:493011bacdcd 121 huey.forward(speed);
vsal 9:493011bacdcd 122 huey.calibrated_sensor(sensors);
vsal 9:493011bacdcd 123 }
vsal 9:493011bacdcd 124 //Stops and semi-properly turns at corners and t's but gets screwed up at + intersections because he turns until the middle sensors read black
vsal 9:493011bacdcd 125 // But at a + intersection the sensors are black already so he doesnt turn
vsal 9:493011bacdcd 126 huey.stop();
vsal 9:493011bacdcd 127 wait(0.1);
vsal 9:493011bacdcd 128 float pos = -1;
vsal 9:493011bacdcd 129 while(pos > 0.05 || pos < -0.05)
vsal 9:493011bacdcd 130 {
vsal 9:493011bacdcd 131 huey.right(speed);
vsal 9:493011bacdcd 132 pos = myLinePos();
vsal 9:493011bacdcd 133 }
vsal 9:493011bacdcd 134
vsal 9:493011bacdcd 135 //return 0;
vsal 9:493011bacdcd 136 }
vsal 9:493011bacdcd 137 else if(leftCorner() == 1)
vsal 9:493011bacdcd 138 {
vsal 9:493011bacdcd 139 huey.left(speed*2);
vsal 9:493011bacdcd 140 //return 0;
vsal 9:493011bacdcd 141 }
vsal 9:493011bacdcd 142 else if(speed+u > 1)
vsal 9:493011bacdcd 143 {
vsal 9:493011bacdcd 144 huey.stop();
vsal 9:493011bacdcd 145 huey.printf("Fast Er");
vsal 9:493011bacdcd 146 //return 0;
vsal 9:493011bacdcd 147 }
vsal 9:493011bacdcd 148 else
vsal 9:493011bacdcd 149 {
vsal 9:493011bacdcd 150 huey.right_motor(speed-u);
vsal 9:493011bacdcd 151 huey.left_motor(speed+u);
vsal 9:493011bacdcd 152 //return 0;
vsal 9:493011bacdcd 153 }
vsal 9:493011bacdcd 154 //}
vsal 0:80d9725a7342 155
vsal 1:ac7fc0b5f878 156 }
vsal 1:ac7fc0b5f878 157
vsal 9:493011bacdcd 158
vsal 9:493011bacdcd 159
vsal 9:493011bacdcd 160
vsal 3:6f773c2ba3a0 161 void slowStop(float speed, float waitTime, int steps)
vsal 1:ac7fc0b5f878 162 {
vsal 3:6f773c2ba3a0 163 float i;
vsal 3:6f773c2ba3a0 164 for(i = speed; i > 0; i = i - (speed/steps))
vsal 1:ac7fc0b5f878 165 {
vsal 9:493011bacdcd 166 //FIGURE THIS OUT KJALSHDFKJASHFLKJASHFLKJASHDLKJASHDLKJHASDLKJHASLKDJH
vsal 3:6f773c2ba3a0 167 smoothFollow(huey.line_position(),i);
vsal 3:6f773c2ba3a0 168 wait(waitTime);
vsal 1:ac7fc0b5f878 169 }
vsal 3:6f773c2ba3a0 170 huey.stop();
vsal 1:ac7fc0b5f878 171 }
vsal 1:ac7fc0b5f878 172
vsal 3:6f773c2ba3a0 173
vsal 3:6f773c2ba3a0 174 int pushToStart(void)
vsal 3:6f773c2ba3a0 175 {
vsal 3:6f773c2ba3a0 176 DigitalIn button(p21);
vsal 3:6f773c2ba3a0 177 int value = button.read();
vsal 3:6f773c2ba3a0 178 return value;
vsal 3:6f773c2ba3a0 179 }
vsal 3:6f773c2ba3a0 180
vsal 3:6f773c2ba3a0 181
vsal 8:09f4495696d2 182
vsal 3:6f773c2ba3a0 183 /////////////////////////////////////////////////////////////////
vsal 1:ac7fc0b5f878 184 int main() {
vsal 9:493011bacdcd 185 //time_t startt, endt;
vsal 9:493011bacdcd 186 //time(&startt);
vsal 9:493011bacdcd 187 //clock_t clock;
vsal 3:6f773c2ba3a0 188
vsal 8:09f4495696d2 189 //btbee trial
vsal 8:09f4495696d2 190 // initialization stuff ////////////////////////////////////////////////////////////////////////////////////////////////////
vsal 8:09f4495696d2 191 huey.locate(0,1);
vsal 8:09f4495696d2 192 btbee.reset();
vsal 8:09f4495696d2 193 for (int i = 0; i <4; i++) {
vsal 8:09f4495696d2 194 mbed_led[i] = 0;
vsal 8:09f4495696d2 195 }
vsal 8:09f4495696d2 196 for (int i = 0; i <8; i++) {
vsal 8:09f4495696d2 197 m3pi_led[i]=0;
vsal 8:09f4495696d2 198 }
vsal 8:09f4495696d2 199 m3pi_pb.mode(PullUp); // expected would be 1 when pb is pressed, 0 when not, opposite is the case
vsal 8:09f4495696d2 200
vsal 8:09f4495696d2 201 // end initialization stuff ////////////////////////////////////////////////////////////////////////////////////////////////
vsal 8:09f4495696d2 202
vsal 8:09f4495696d2 203 huey.locate(0,0);
vsal 8:09f4495696d2 204 huey.printf("B:%0.3f%V",huey.battery());
vsal 8:09f4495696d2 205 wait(0.3);
vsal 8:09f4495696d2 206
vsal 8:09f4495696d2 207 huey.locate(0,0);
vsal 8:09f4495696d2 208 huey.printf("%s","btTest");
vsal 8:09f4495696d2 209 huey.locate(0,1);
vsal 8:09f4495696d2 210 huey.printf("%s","PBonLNK");
vsal 8:09f4495696d2 211
vsal 8:09f4495696d2 212 // wait for the user to push P21, should be pressed when the bt link is established (green led "link")
vsal 8:09f4495696d2 213 while(m3pi_pb) {
vsal 8:09f4495696d2 214 m3pi_led[0]=!m3pi_led[0];
vsal 8:09f4495696d2 215 wait(0.1);
vsal 8:09f4495696d2 216 }
vsal 8:09f4495696d2 217
vsal 8:09f4495696d2 218 int iline=1;
vsal 8:09f4495696d2 219 char arr_read[30]; // this should be long enough to store any reply coming in over bt.
vsal 8:09f4495696d2 220 int chars_read; // number of chars read in a bt reply
vsal 8:09f4495696d2 221
vsal 9:493011bacdcd 222 while (iline < 3) {
vsal 8:09f4495696d2 223 // this writes "Line 001\n" to "Line 005\n" and then "end\n" to the btbee
vsal 8:09f4495696d2 224 if ( btbee.writeable() ) {
vsal 9:493011bacdcd 225 if (iline==2) {
vsal 9:493011bacdcd 226 //btbee.printf("end\n");
vsal 8:09f4495696d2 227 iline++;
vsal 8:09f4495696d2 228 }//if
vsal 8:09f4495696d2 229 else {
vsal 9:493011bacdcd 230 if (iline <2){
vsal 9:493011bacdcd 231 //time(&endt);
vsal 9:493011bacdcd 232 //btbee.printf("%f seconds - ", clock);
vsal 9:493011bacdcd 233 btbee.printf("Battery Voltage: %f\n", huey.battery());
vsal 8:09f4495696d2 234 m3pi_led[0]=0;
vsal 8:09f4495696d2 235 huey.locate(0,0);
vsal 8:09f4495696d2 236 huey.printf("Sent %0.3d",iline);
vsal 8:09f4495696d2 237 iline++;
vsal 8:09f4495696d2 238 }
vsal 8:09f4495696d2 239 }//else
vsal 8:09f4495696d2 240 }//if_write
vsal 8:09f4495696d2 241
vsal 8:09f4495696d2 242 // check for answers after each write /not write
vsal 8:09f4495696d2 243 while ( btbee.readable() ) {
vsal 8:09f4495696d2 244 m3pi_led[7]=1;
vsal 8:09f4495696d2 245 btbee.read_all(arr_read, 30, &chars_read );
vsal 8:09f4495696d2 246 m3pi_led[6]=1;
vsal 8:09f4495696d2 247 huey.locate(0,1);
vsal 8:09f4495696d2 248 huey.print(arr_read,chars_read);
vsal 8:09f4495696d2 249 m3pi_led[5]=1;
vsal 8:09f4495696d2 250 }//while_readable
vsal 8:09f4495696d2 251 wait(0.1);
vsal 8:09f4495696d2 252 }//while_true
vsal 9:493011bacdcd 253
vsal 8:09f4495696d2 254 //main
vsal 8:09f4495696d2 255 //end of btbee trial
vsal 8:09f4495696d2 256
vsal 8:09f4495696d2 257
vsal 7:d0c19729e7fd 258
vsal 3:6f773c2ba3a0 259 m3pi_IN[0].mode(PullUp);
vsal 3:6f773c2ba3a0 260 m3pi_IN[1].mode(PullUp);
vsal 3:6f773c2ba3a0 261
vsal 3:6f773c2ba3a0 262 int start;
vsal 3:6f773c2ba3a0 263 do
vsal 3:6f773c2ba3a0 264 {
vsal 3:6f773c2ba3a0 265 start = pushToStart();
vsal 3:6f773c2ba3a0 266 }while(start == 1);
vsal 3:6f773c2ba3a0 267
vsal 3:6f773c2ba3a0 268 wait(1);
vsal 1:ac7fc0b5f878 269 //calibrates sensors
vsal 1:ac7fc0b5f878 270 huey.sensor_auto_calibrate();
vsal 1:ac7fc0b5f878 271
vsal 3:6f773c2ba3a0 272 printBattery();
vsal 0:80d9725a7342 273
vsal 0:80d9725a7342 274
vsal 2:e513ada0ecf1 275 float speed = 0.25;
vsal 1:ac7fc0b5f878 276 float pos;
vsal 1:ac7fc0b5f878 277 int z=1;
vsal 9:493011bacdcd 278 int turns[3];
vsal 9:493011bacdcd 279 turns[0] = 0;
vsal 9:493011bacdcd 280 turns[1] = 1;
vsal 9:493011bacdcd 281 turns[2] = 0;
vsal 9:493011bacdcd 282
vsal 9:493011bacdcd 283
vsal 1:ac7fc0b5f878 284 while(z==1)
vsal 1:ac7fc0b5f878 285 {
vsal 9:493011bacdcd 286
vsal 9:493011bacdcd 287 //Trying to get it to follow a sequence of turns
vsal 9:493011bacdcd 288 /*for(int q = 0; q < sizeof(turns); q++)
vsal 9:493011bacdcd 289 {
vsal 9:493011bacdcd 290 int change = 0;
vsal 9:493011bacdcd 291 while(change == 0)
vsal 9:493011bacdcd 292 {
vsal 9:493011bacdcd 293 //change = crossing(speed, turns[q]);
vsal 9:493011bacdcd 294 pos = myLinePos();
vsal 9:493011bacdcd 295 change = smoothFollow(pos, speed, turns[q]);
vsal 9:493011bacdcd 296
vsal 9:493011bacdcd 297 }
vsal 9:493011bacdcd 298 }*/
vsal 9:493011bacdcd 299
vsal 9:493011bacdcd 300
vsal 9:493011bacdcd 301
vsal 9:493011bacdcd 302 pos = myLinePos();
vsal 9:493011bacdcd 303
vsal 1:ac7fc0b5f878 304 smoothFollow(pos, speed);
vsal 9:493011bacdcd 305
vsal 3:6f773c2ba3a0 306 if(m3pi_IN[0]==0)
vsal 3:6f773c2ba3a0 307 {
vsal 3:6f773c2ba3a0 308 slowStop(speed, 0.05, 3);
vsal 3:6f773c2ba3a0 309 huey.stop();
vsal 9:493011bacdcd 310 btbee.printf("Object in Front");
vsal 3:6f773c2ba3a0 311 while(m3pi_IN[0]==0)
vsal 3:6f773c2ba3a0 312 {
vsal 9:493011bacdcd 313 huey.stop();
vsal 3:6f773c2ba3a0 314 }
vsal 3:6f773c2ba3a0 315 }
vsal 9:493011bacdcd 316 if(pushToStart() == 0)
vsal 9:493011bacdcd 317 {
vsal 9:493011bacdcd 318 btbee.printf("end\n");
vsal 9:493011bacdcd 319 break;
vsal 9:493011bacdcd 320 }
vsal 1:ac7fc0b5f878 321 }
vsal 1:ac7fc0b5f878 322
vsal 1:ac7fc0b5f878 323
vsal 0:80d9725a7342 324
vsal 0:80d9725a7342 325
vsal 0:80d9725a7342 326
vsal 0:80d9725a7342 327
vsal 0:80d9725a7342 328 while(1)
vsal 0:80d9725a7342 329 {
vsal 0:80d9725a7342 330 myled = 1;
vsal 0:80d9725a7342 331 wait(0.2);
vsal 0:80d9725a7342 332 myled = 0;
vsal 0:80d9725a7342 333 wait(0.2);
vsal 0:80d9725a7342 334 }
vsal 0:80d9725a7342 335 }
vsal 1:ac7fc0b5f878 336