The Dream Team / Mbed 2 deprecated first_test

Pop lock n drop it

Dependencies:   m3pi_ng mbed

main.cpp@8:09f4495696d2, 2014-05-23 (annotated)

Committer:
vsal
Date:
Fri May 23 06:53:45 2014 +0000
Revision:
8:09f4495696d2
Parent:
7:d0c19729e7fd
Child:
9:493011bacdcd
Final From Thurs

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 8:09f4495696d2 5
vsal 0:80d9725a7342 6
vsal 0:80d9725a7342 7 DigitalOut myled(LED1);
vsal 0:80d9725a7342 8 m3pi huey;
vsal 8:09f4495696d2 9 btbee btbee;
vsal 8:09f4495696d2 10
vsal 3:6f773c2ba3a0 11 DigitalIn m3pi_IN[]={(p12),(p21)}; // IR-Sensor
vsal 8:09f4495696d2 12 DigitalOut mbed_led[] = {(LED1), (LED2),(LED3), (LED4)};
vsal 8:09f4495696d2 13 DigitalOut m3pi_led[] = {(p13), (p14), (p15), (p16), (p17), (p18), (p19), (p20)};
vsal 8:09f4495696d2 14 DigitalIn m3pi_pb(p21);
vsal 1:ac7fc0b5f878 15
vsal 1:ac7fc0b5f878 16 void printBattery()
vsal 1:ac7fc0b5f878 17 {
vsal 0:80d9725a7342 18 //prints battery voltage on led screen
vsal 0:80d9725a7342 19 float bat = huey.battery();
vsal 1:ac7fc0b5f878 20 huey.printf("Bat volt");
vsal 1:ac7fc0b5f878 21 huey.locate(0,1);
vsal 1:ac7fc0b5f878 22 huey.printf("%f",bat);
vsal 1:ac7fc0b5f878 23 }
vsal 1:ac7fc0b5f878 24
vsal 4:14b00a82d206 25 float myLinePos(void)
vsal 4:14b00a82d206 26 {
vsal 4:14b00a82d206 27 int sensors[5];
vsal 4:14b00a82d206 28 huey.calibrated_sensor(sensors);
vsal 7:d0c19729e7fd 29 //oat leftSen, rightSen, midSen;
vsal 7:d0c19729e7fd 30 //ftSen = sensors[1] / 1000;
vsal 7:d0c19729e7fd 31 //ghtSen = sensors[3] / 1000;
vsal 7:d0c19729e7fd 32 //dSen = sensors[2] / 1000;
vsal 7:d0c19729e7fd 33 float val;
vsal 7:d0c19729e7fd 34 val = (((1000*sensors[2] + 2000*sensors[3])/(sensors[1]+sensors[2]+sensors[3])));
vsal 7:d0c19729e7fd 35 return -1+(val/1000);
vsal 7:d0c19729e7fd 36
vsal 4:14b00a82d206 37 }
vsal 4:14b00a82d206 38
vsal 8:09f4495696d2 39 int rightCorner()
vsal 8:09f4495696d2 40 {
vsal 8:09f4495696d2 41 int sensors[5];
vsal 8:09f4495696d2 42 huey.calibrated_sensor(sensors);
vsal 8:09f4495696d2 43 if(sensors[4] > 900)
vsal 8:09f4495696d2 44 return 1;
vsal 8:09f4495696d2 45 else
vsal 8:09f4495696d2 46 return 0;
vsal 8:09f4495696d2 47 }
vsal 8:09f4495696d2 48
vsal 8:09f4495696d2 49 int leftCorner()
vsal 8:09f4495696d2 50 {
vsal 8:09f4495696d2 51 int sensors[5];
vsal 8:09f4495696d2 52 huey.calibrated_sensor(sensors);
vsal 8:09f4495696d2 53 if(sensors[0] > 900)
vsal 8:09f4495696d2 54 return 1;
vsal 8:09f4495696d2 55 else
vsal 8:09f4495696d2 56 return 0;
vsal 8:09f4495696d2 57 }
vsal 8:09f4495696d2 58
vsal 8:09f4495696d2 59
vsal 1:ac7fc0b5f878 60 void smoothFollow(float position, float speed)
vsal 1:ac7fc0b5f878 61 {
vsal 4:14b00a82d206 62 float u = speed;
vsal 1:ac7fc0b5f878 63 u = u * position;
vsal 8:09f4495696d2 64
vsal 8:09f4495696d2 65 if(rightCorner() == 1)
vsal 8:09f4495696d2 66 {
vsal 8:09f4495696d2 67 huey.right(speed*2);
vsal 8:09f4495696d2 68 //wait(0.2);
vsal 8:09f4495696d2 69 }
vsal 8:09f4495696d2 70 else if(leftCorner() == 1)
vsal 8:09f4495696d2 71 {
vsal 8:09f4495696d2 72 huey.left(speed*2);
vsal 8:09f4495696d2 73 //wait(0.2);
vsal 8:09f4495696d2 74 }
vsal 8:09f4495696d2 75 else if(speed+u > 1)
vsal 1:ac7fc0b5f878 76 {
vsal 1:ac7fc0b5f878 77 huey.stop();
vsal 1:ac7fc0b5f878 78 huey.printf("Fast Er");
vsal 1:ac7fc0b5f878 79 }
vsal 1:ac7fc0b5f878 80 else
vsal 1:ac7fc0b5f878 81 {
vsal 2:e513ada0ecf1 82 huey.right_motor(speed-u);
vsal 2:e513ada0ecf1 83 huey.left_motor(speed+u);
vsal 1:ac7fc0b5f878 84 }
vsal 0:80d9725a7342 85
vsal 1:ac7fc0b5f878 86 }
vsal 1:ac7fc0b5f878 87
vsal 3:6f773c2ba3a0 88 void slowStop(float speed, float waitTime, int steps)
vsal 1:ac7fc0b5f878 89 {
vsal 3:6f773c2ba3a0 90 float i;
vsal 3:6f773c2ba3a0 91 for(i = speed; i > 0; i = i - (speed/steps))
vsal 1:ac7fc0b5f878 92 {
vsal 3:6f773c2ba3a0 93 smoothFollow(huey.line_position(),i);
vsal 3:6f773c2ba3a0 94 wait(waitTime);
vsal 1:ac7fc0b5f878 95 }
vsal 3:6f773c2ba3a0 96 huey.stop();
vsal 1:ac7fc0b5f878 97 }
vsal 1:ac7fc0b5f878 98
vsal 3:6f773c2ba3a0 99
vsal 3:6f773c2ba3a0 100 int pushToStart(void)
vsal 3:6f773c2ba3a0 101 {
vsal 3:6f773c2ba3a0 102 DigitalIn button(p21);
vsal 3:6f773c2ba3a0 103 int value = button.read();
vsal 3:6f773c2ba3a0 104 return value;
vsal 3:6f773c2ba3a0 105 }
vsal 3:6f773c2ba3a0 106
vsal 3:6f773c2ba3a0 107
vsal 8:09f4495696d2 108
vsal 3:6f773c2ba3a0 109 /////////////////////////////////////////////////////////////////
vsal 1:ac7fc0b5f878 110 int main() {
vsal 3:6f773c2ba3a0 111
vsal 8:09f4495696d2 112 /*
vsal 8:09f4495696d2 113 //btbee trial
vsal 8:09f4495696d2 114 // initialization stuff ////////////////////////////////////////////////////////////////////////////////////////////////////
vsal 8:09f4495696d2 115 huey.locate(0,1);
vsal 8:09f4495696d2 116 btbee.reset();
vsal 8:09f4495696d2 117 for (int i = 0; i <4; i++) {
vsal 8:09f4495696d2 118 mbed_led[i] = 0;
vsal 8:09f4495696d2 119 }
vsal 8:09f4495696d2 120 for (int i = 0; i <8; i++) {
vsal 8:09f4495696d2 121 m3pi_led[i]=0;
vsal 8:09f4495696d2 122 }
vsal 8:09f4495696d2 123 m3pi_pb.mode(PullUp); // expected would be 1 when pb is pressed, 0 when not, opposite is the case
vsal 8:09f4495696d2 124
vsal 8:09f4495696d2 125 // end initialization stuff ////////////////////////////////////////////////////////////////////////////////////////////////
vsal 8:09f4495696d2 126
vsal 8:09f4495696d2 127 huey.locate(0,0);
vsal 8:09f4495696d2 128 huey.printf("B:%0.3f%V",huey.battery());
vsal 8:09f4495696d2 129 wait(0.3);
vsal 8:09f4495696d2 130
vsal 8:09f4495696d2 131 huey.locate(0,0);
vsal 8:09f4495696d2 132 huey.printf("%s","btTest");
vsal 8:09f4495696d2 133 huey.locate(0,1);
vsal 8:09f4495696d2 134 huey.printf("%s","PBonLNK");
vsal 8:09f4495696d2 135
vsal 8:09f4495696d2 136 // wait for the user to push P21, should be pressed when the bt link is established (green led "link")
vsal 8:09f4495696d2 137 while(m3pi_pb) {
vsal 8:09f4495696d2 138 m3pi_led[0]=!m3pi_led[0];
vsal 8:09f4495696d2 139 wait(0.1);
vsal 8:09f4495696d2 140 }
vsal 8:09f4495696d2 141
vsal 8:09f4495696d2 142 int iline=1;
vsal 8:09f4495696d2 143 char arr_read[30]; // this should be long enough to store any reply coming in over bt.
vsal 8:09f4495696d2 144 int chars_read; // number of chars read in a bt reply
vsal 8:09f4495696d2 145
vsal 8:09f4495696d2 146 while (true) {
vsal 8:09f4495696d2 147 // this writes "Line 001\n" to "Line 005\n" and then "end\n" to the btbee
vsal 8:09f4495696d2 148 if ( btbee.writeable() ) {
vsal 8:09f4495696d2 149 if (iline==6) {
vsal 8:09f4495696d2 150 btbee.printf("end\n");
vsal 8:09f4495696d2 151 iline++;
vsal 8:09f4495696d2 152 }//if
vsal 8:09f4495696d2 153 else {
vsal 8:09f4495696d2 154 if (iline <6){
vsal 8:09f4495696d2 155 btbee.printf("Line %0.3d \n",iline);
vsal 8:09f4495696d2 156 m3pi_led[0]=0;
vsal 8:09f4495696d2 157 huey.locate(0,0);
vsal 8:09f4495696d2 158 huey.printf("Sent %0.3d",iline);
vsal 8:09f4495696d2 159 iline++;
vsal 8:09f4495696d2 160 }
vsal 8:09f4495696d2 161 }//else
vsal 8:09f4495696d2 162 }//if_write
vsal 8:09f4495696d2 163
vsal 8:09f4495696d2 164 // check for answers after each write /not write
vsal 8:09f4495696d2 165 while ( btbee.readable() ) {
vsal 8:09f4495696d2 166 m3pi_led[7]=1;
vsal 8:09f4495696d2 167 btbee.read_all(arr_read, 30, &chars_read );
vsal 8:09f4495696d2 168 m3pi_led[6]=1;
vsal 8:09f4495696d2 169 huey.locate(0,1);
vsal 8:09f4495696d2 170 huey.print(arr_read,chars_read);
vsal 8:09f4495696d2 171 m3pi_led[5]=1;
vsal 8:09f4495696d2 172 }//while_readable
vsal 8:09f4495696d2 173 wait(0.1);
vsal 8:09f4495696d2 174 }//while_true
vsal 8:09f4495696d2 175 */
vsal 8:09f4495696d2 176 //main
vsal 8:09f4495696d2 177 //end of btbee trial
vsal 8:09f4495696d2 178
vsal 8:09f4495696d2 179
vsal 7:d0c19729e7fd 180
vsal 3:6f773c2ba3a0 181 m3pi_IN[0].mode(PullUp);
vsal 3:6f773c2ba3a0 182 m3pi_IN[1].mode(PullUp);
vsal 3:6f773c2ba3a0 183
vsal 3:6f773c2ba3a0 184 int start;
vsal 3:6f773c2ba3a0 185 do
vsal 3:6f773c2ba3a0 186 {
vsal 3:6f773c2ba3a0 187 start = pushToStart();
vsal 3:6f773c2ba3a0 188 }while(start == 1);
vsal 3:6f773c2ba3a0 189
vsal 3:6f773c2ba3a0 190 wait(1);
vsal 1:ac7fc0b5f878 191 //calibrates sensors
vsal 1:ac7fc0b5f878 192 huey.sensor_auto_calibrate();
vsal 1:ac7fc0b5f878 193
vsal 3:6f773c2ba3a0 194 printBattery();
vsal 0:80d9725a7342 195
vsal 0:80d9725a7342 196
vsal 2:e513ada0ecf1 197 float speed = 0.25;
vsal 1:ac7fc0b5f878 198 float pos;
vsal 1:ac7fc0b5f878 199 int z=1;
vsal 1:ac7fc0b5f878 200 while(z==1)
vsal 1:ac7fc0b5f878 201 {
vsal 1:ac7fc0b5f878 202 //huey.right_motor(speed);
vsal 4:14b00a82d206 203 pos = myLinePos();//huey.line_position();
vsal 1:ac7fc0b5f878 204 smoothFollow(pos, speed);
vsal 3:6f773c2ba3a0 205 if(m3pi_IN[0]==0)
vsal 3:6f773c2ba3a0 206 {
vsal 3:6f773c2ba3a0 207 slowStop(speed, 0.05, 3);
vsal 3:6f773c2ba3a0 208 huey.stop();
vsal 3:6f773c2ba3a0 209 while(m3pi_IN[0]==0)
vsal 3:6f773c2ba3a0 210 {
vsal 3:6f773c2ba3a0 211 huey.printf("Stuck");
vsal 3:6f773c2ba3a0 212 }
vsal 3:6f773c2ba3a0 213 }
vsal 1:ac7fc0b5f878 214
vsal 1:ac7fc0b5f878 215 }
vsal 1:ac7fc0b5f878 216
vsal 1:ac7fc0b5f878 217
vsal 0:80d9725a7342 218
vsal 0:80d9725a7342 219
vsal 0:80d9725a7342 220
vsal 0:80d9725a7342 221
vsal 0:80d9725a7342 222 while(1)
vsal 0:80d9725a7342 223 {
vsal 0:80d9725a7342 224 myled = 1;
vsal 0:80d9725a7342 225 wait(0.2);
vsal 0:80d9725a7342 226 myled = 0;
vsal 0:80d9725a7342 227 wait(0.2);
vsal 0:80d9725a7342 228 }
vsal 0:80d9725a7342 229 }
vsal 1:ac7fc0b5f878 230