The Dream Team / Mbed 2 deprecated first_test

Pop lock n drop it

Dependencies:   m3pi_ng mbed

main.cpp@5:f3805a1f9047, 2014-05-22 (annotated)

Committer:
katesleavitt
Date:
Thu May 22 11:54:41 2014 +0000
Revision:
5:f3805a1f9047
Parent:
4:14b00a82d206
Child:
6:36c597e4d07a
with bluetooth

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
vsal 3:6f773c2ba3a0 77 m3pi_IN[0].mode(PullUp);
vsal 3:6f773c2ba3a0 78 m3pi_IN[1].mode(PullUp);
vsal 3:6f773c2ba3a0 79
vsal 3:6f773c2ba3a0 80 int start;
vsal 3:6f773c2ba3a0 81 do
vsal 3:6f773c2ba3a0 82 {
vsal 3:6f773c2ba3a0 83 start = pushToStart();
vsal 3:6f773c2ba3a0 84 }while(start == 1);
vsal 3:6f773c2ba3a0 85
vsal 3:6f773c2ba3a0 86 wait(1);
vsal 1:ac7fc0b5f878 87 //calibrates sensors
vsal 1:ac7fc0b5f878 88 huey.sensor_auto_calibrate();
vsal 1:ac7fc0b5f878 89
vsal 3:6f773c2ba3a0 90 printBattery();
vsal 0:80d9725a7342 91
vsal 2:e513ada0ecf1 92 float speed = 0.25;
vsal 1:ac7fc0b5f878 93 float pos;
vsal 1:ac7fc0b5f878 94 int z=1;
vsal 1:ac7fc0b5f878 95 while(z==1)
vsal 1:ac7fc0b5f878 96 {
vsal 1:ac7fc0b5f878 97 //huey.right_motor(speed);
vsal 4:14b00a82d206 98 pos = myLinePos();//huey.line_position();
vsal 1:ac7fc0b5f878 99 smoothFollow(pos, speed);
vsal 3:6f773c2ba3a0 100 if(m3pi_IN[0]==0)
vsal 3:6f773c2ba3a0 101 {
vsal 3:6f773c2ba3a0 102 slowStop(speed, 0.05, 3);
vsal 3:6f773c2ba3a0 103 huey.stop();
vsal 3:6f773c2ba3a0 104 while(m3pi_IN[0]==0)
vsal 3:6f773c2ba3a0 105 {
vsal 3:6f773c2ba3a0 106 huey.printf("Stuck");
vsal 3:6f773c2ba3a0 107 }
vsal 3:6f773c2ba3a0 108 }
vsal 1:ac7fc0b5f878 109
vsal 1:ac7fc0b5f878 110 }
vsal 1:ac7fc0b5f878 111
vsal 1:ac7fc0b5f878 112
vsal 0:80d9725a7342 113
vsal 0:80d9725a7342 114
vsal 0:80d9725a7342 115
vsal 0:80d9725a7342 116
vsal 0:80d9725a7342 117 while(1)
vsal 0:80d9725a7342 118 {
vsal 0:80d9725a7342 119 myled = 1;
vsal 0:80d9725a7342 120 wait(0.2);
vsal 0:80d9725a7342 121 myled = 0;
vsal 0:80d9725a7342 122 wait(0.2);
vsal 0:80d9725a7342 123 }
katesleavitt 5:f3805a1f9047 124 //btbee trial
katesleavitt 5:f3805a1f9047 125 // initialization stuff ////////////////////////////////////////////////////////////////////////////////////////////////////
katesleavitt 5:f3805a1f9047 126 huey.locate(0,1);
katesleavitt 5:f3805a1f9047 127 btbee.reset();
katesleavitt 5:f3805a1f9047 128 for (int i = 0; i <4; i++) {
katesleavitt 5:f3805a1f9047 129 mbed_led[i] = 0;
katesleavitt 5:f3805a1f9047 130 }
katesleavitt 5:f3805a1f9047 131 for (int i = 0; i <8; i++) {
katesleavitt 5:f3805a1f9047 132 m3pi_led[i]=0;
katesleavitt 5:f3805a1f9047 133 }
katesleavitt 5:f3805a1f9047 134 m3pi_pb.mode(PullUp); // expected would be 1 when pb is pressed, 0 when not, opposite is the case
katesleavitt 5:f3805a1f9047 135
katesleavitt 5:f3805a1f9047 136 // end initialization stuff ////////////////////////////////////////////////////////////////////////////////////////////////
katesleavitt 5:f3805a1f9047 137
katesleavitt 5:f3805a1f9047 138 huey.locate(0,0);
katesleavitt 5:f3805a1f9047 139 huey.printf("B:%0.3f%V",huey.battery());
katesleavitt 5:f3805a1f9047 140 wait(0.3);
katesleavitt 5:f3805a1f9047 141
katesleavitt 5:f3805a1f9047 142 huey.locate(0,0);
katesleavitt 5:f3805a1f9047 143 huey.printf("%s","btTest");
katesleavitt 5:f3805a1f9047 144 huey.locate(0,1);
katesleavitt 5:f3805a1f9047 145 huey.printf("%s","PBonLNK");
katesleavitt 5:f3805a1f9047 146
katesleavitt 5:f3805a1f9047 147 // wait for the user to push P21, should be pressed when the bt link is established (green led "link")
katesleavitt 5:f3805a1f9047 148 while(m3pi_pb) {
katesleavitt 5:f3805a1f9047 149 m3pi_led[0]=!m3pi_led[0];
katesleavitt 5:f3805a1f9047 150 wait(0.1);
katesleavitt 5:f3805a1f9047 151 }
vsal 1:ac7fc0b5f878 152
katesleavitt 5:f3805a1f9047 153 int iline=1;
katesleavitt 5:f3805a1f9047 154 char arr_read[30]; // this should be long enough to store any reply coming in over bt.
katesleavitt 5:f3805a1f9047 155 int chars_read; // number of chars read in a bt reply
katesleavitt 5:f3805a1f9047 156
katesleavitt 5:f3805a1f9047 157 while (true) {
katesleavitt 5:f3805a1f9047 158 // this writes "Line 001\n" to "Line 005\n" and then "end\n" to the btbee
katesleavitt 5:f3805a1f9047 159 if ( btbee.writeable() ) {
katesleavitt 5:f3805a1f9047 160 if (iline==6) {
katesleavitt 5:f3805a1f9047 161 btbee.printf("end\n");
katesleavitt 5:f3805a1f9047 162 iline++;
katesleavitt 5:f3805a1f9047 163 }//if
katesleavitt 5:f3805a1f9047 164 else {
katesleavitt 5:f3805a1f9047 165 if (iline <6){
katesleavitt 5:f3805a1f9047 166 btbee.printf("Line %0.3d \n",iline);
katesleavitt 5:f3805a1f9047 167 m3pi_led[0]=0;
katesleavitt 5:f3805a1f9047 168 huey.locate(0,0);
katesleavitt 5:f3805a1f9047 169 huey.printf("Sent %0.3d",iline);
katesleavitt 5:f3805a1f9047 170 iline++;
katesleavitt 5:f3805a1f9047 171 }
katesleavitt 5:f3805a1f9047 172 }//else
katesleavitt 5:f3805a1f9047 173 }//if_write
katesleavitt 5:f3805a1f9047 174
katesleavitt 5:f3805a1f9047 175 // check for answers after each write /not write
katesleavitt 5:f3805a1f9047 176 while ( btbee.readable() ) {
katesleavitt 5:f3805a1f9047 177 m3pi_led[7]=1;
katesleavitt 5:f3805a1f9047 178 btbee.read_all(arr_read, 30, &chars_read );
katesleavitt 5:f3805a1f9047 179 m3pi_led[6]=1;
katesleavitt 5:f3805a1f9047 180 huey.locate(0,1);
katesleavitt 5:f3805a1f9047 181 huey.print(arr_read,chars_read);
katesleavitt 5:f3805a1f9047 182 m3pi_led[5]=1;
katesleavitt 5:f3805a1f9047 183 }//while_readable
katesleavitt 5:f3805a1f9047 184 wait(0.1);
katesleavitt 5:f3805a1f9047 185 }//while_true
katesleavitt 5:f3805a1f9047 186
katesleavitt 5:f3805a1f9047 187 }//main
katesleavitt 5:f3805a1f9047 188 //end of btbee trial
katesleavitt 5:f3805a1f9047 189
katesleavitt 5:f3805a1f9047 190