The Dream Team / Mbed 2 deprecated first_test

Pop lock n drop it

Dependencies:   m3pi_ng mbed

main.cpp@6:36c597e4d07a, 2014-05-22 (annotated)

Committer:
rachelka
Date:
Thu May 22 12:52:43 2014 +0000
Revision:
6:36c597e4d07a
Parent:
5:f3805a1f9047 
blue bishh;

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