der Roboter / Mbed 2 deprecated BlueToothStuff

WORK IN PROGRESS

Dependencies:   m3pi_ng mbed

Fork of ThursWork by der Roboter

main.cpp@1:6402638c6f6d, 2014-05-22 (annotated)

Committer:
satyson
Date:
Thu May 22 14:43:25 2014 +0000
Revision:
1:6402638c6f6d
Parent:
0:386c250325ce
Child:
2:cc7b01c5a9b0
WORK IN PROGRESS

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mmpeter 0:386c250325ce 1 #include "mbed.h"
mmpeter 0:386c250325ce 2 #include "m3pi_ng.h"
mmpeter 0:386c250325ce 3 #include "cmath"
mmpeter 0:386c250325ce 4 #include "iostream"
satyson 1:6402638c6f6d 5 #include "btbee.h"
mmpeter 0:386c250325ce 6
mmpeter 0:386c250325ce 7 using namespace std;
mmpeter 0:386c250325ce 8
mmpeter 0:386c250325ce 9 m3pi thinggy;
satyson 1:6402638c6f6d 10 btbee btbee;
satyson 1:6402638c6f6d 11
satyson 1:6402638c6f6d 12 DigitalOut mbed_led[] = {(LED1), (LED2),(LED3), (LED4)};
satyson 1:6402638c6f6d 13 DigitalOut m3pi_led[] = {(p13), (p14), (p15), (p16), (p17), (p18), (p19), (p20)};
satyson 1:6402638c6f6d 14 DigitalIn m3pi_pb(p21);
mmpeter 0:386c250325ce 15
mmpeter 0:386c250325ce 16 int main() {
mmpeter 0:386c250325ce 17
satyson 1:6402638c6f6d 18 //..................................................................................................\\
satyson 1:6402638c6f6d 19 //......BlueTooth Communication.....................................................................\\
satyson 1:6402638c6f6d 20 //..................................................................................................\\
satyson 1:6402638c6f6d 21
satyson 1:6402638c6f6d 22 // initialization stuff ////////////////////////////////////////////////////////////////////////////////////////////////////
satyson 1:6402638c6f6d 23 thinggy.locate(0,1);
satyson 1:6402638c6f6d 24 btbee.reset();
satyson 1:6402638c6f6d 25 for (int i = 0; i <4; i++) {
satyson 1:6402638c6f6d 26 mbed_led[i] = 0;
satyson 1:6402638c6f6d 27 }
satyson 1:6402638c6f6d 28 for (int i = 0; i <8; i++) {
satyson 1:6402638c6f6d 29 m3pi_led[i]=0;
satyson 1:6402638c6f6d 30 }
satyson 1:6402638c6f6d 31 m3pi_pb.mode(PullUp); // expected would be 1 when pb is pressed, 0 when not, opposite is the case
satyson 1:6402638c6f6d 32
satyson 1:6402638c6f6d 33 // end initialization stuff ////////////////////////////////////////////////////////////////////////////////////////////////
satyson 1:6402638c6f6d 34
satyson 1:6402638c6f6d 35 thinggy.locate(0,0);
satyson 1:6402638c6f6d 36 wait(0.3);
satyson 1:6402638c6f6d 37
satyson 1:6402638c6f6d 38 thinggy.locate(0,0);
satyson 1:6402638c6f6d 39 thinggy.printf("%s","btTest");
satyson 1:6402638c6f6d 40 thinggy.locate(0,1);
satyson 1:6402638c6f6d 41 thinggy.printf("%s","PBonLNK");
satyson 1:6402638c6f6d 42
satyson 1:6402638c6f6d 43 // wait for the user to push P21, should be pressed when the bt link is established (green led "link")
satyson 1:6402638c6f6d 44 while(m3pi_pb) {
satyson 1:6402638c6f6d 45 m3pi_led[0]=!m3pi_led[0];
satyson 1:6402638c6f6d 46 wait(0.1);
satyson 1:6402638c6f6d 47 }
satyson 1:6402638c6f6d 48
satyson 1:6402638c6f6d 49 int iline=1;
satyson 1:6402638c6f6d 50 char arr_read[30]; // this should be long enough to store any reply coming in over bt.
satyson 1:6402638c6f6d 51 int chars_read; // number of chars read in a bt reply
satyson 1:6402638c6f6d 52
satyson 1:6402638c6f6d 53 while (true) {
satyson 1:6402638c6f6d 54 // this writes "Line 001\n" to "Line 005\n" and then "end\n" to the btbee
satyson 1:6402638c6f6d 55 if ( btbee.writeable() ) {
satyson 1:6402638c6f6d 56 if (iline==6) {
satyson 1:6402638c6f6d 57 btbee.printf("Your Turn!\n");
satyson 1:6402638c6f6d 58 iline++;
satyson 1:6402638c6f6d 59 }//if
satyson 1:6402638c6f6d 60 else {
satyson 1:6402638c6f6d 61 if (iline <6){
satyson 1:6402638c6f6d 62 btbee.printf("Line %0.3d \n",iline);
satyson 1:6402638c6f6d 63 m3pi_led[0]=0;
satyson 1:6402638c6f6d 64 thinggy.locate(0,0);
satyson 1:6402638c6f6d 65 thinggy.printf("Sent %0.3d",iline);
satyson 1:6402638c6f6d 66 iline++;
satyson 1:6402638c6f6d 67 }
satyson 1:6402638c6f6d 68 }//else
satyson 1:6402638c6f6d 69 }//if_write
satyson 1:6402638c6f6d 70
satyson 1:6402638c6f6d 71 // check for answers after each write /not write
satyson 1:6402638c6f6d 72 while ( btbee.readable() ) {
satyson 1:6402638c6f6d 73 m3pi_led[7]=1;
satyson 1:6402638c6f6d 74 btbee.read_all(arr_read, 30, &chars_read );
satyson 1:6402638c6f6d 75 m3pi_led[6]=1;
satyson 1:6402638c6f6d 76 thinggy.locate(0,1);
satyson 1:6402638c6f6d 77 thinggy.print(arr_read,chars_read);
satyson 1:6402638c6f6d 78 m3pi_led[5]=1;
satyson 1:6402638c6f6d 79 }//while_readable
satyson 1:6402638c6f6d 80 wait(0.1);
satyson 1:6402638c6f6d 81 }//while_true
satyson 1:6402638c6f6d 82
satyson 1:6402638c6f6d 83 //..................................................................................................\\
satyson 1:6402638c6f6d 84 //......End BlueTooth Communication.................................................................\\
satyson 1:6402638c6f6d 85 //..................................................................................................\\
satyson 1:6402638c6f6d 86
satyson 1:6402638c6f6d 87 /*
satyson 1:6402638c6f6d 88
satyson 1:6402638c6f6d 89
satyson 1:6402638c6f6d 90
mmpeter 0:386c250325ce 91 float speed = 0.25;
mmpeter 0:386c250325ce 92 float correction;
mmpeter 0:386c250325ce 93 float k = -0.3;
mmpeter 0:386c250325ce 94 int sensor[5];
mmpeter 0:386c250325ce 95 int returned;
mmpeter 0:386c250325ce 96 thinggy.locate(0,1);
mmpeter 0:386c250325ce 97 thinggy.printf("Villan");
mmpeter 0:386c250325ce 98 thinggy.locate(0,0);
mmpeter 0:386c250325ce 99 thinggy.printf("Pimpin");
mmpeter 0:386c250325ce 100
satyson 1:6402638c6f6d 101
satyson 1:6402638c6f6d 102
mmpeter 0:386c250325ce 103 wait(1.0);
mmpeter 0:386c250325ce 104
mmpeter 0:386c250325ce 105 thinggy.sensor_auto_calibrate();
mmpeter 0:386c250325ce 106
mmpeter 0:386c250325ce 107 thinggy.calibrated_sensor(sensor);
mmpeter 0:386c250325ce 108 returned = (sensor[1] + sensor[2] + sensor[3])/3;
mmpeter 0:386c250325ce 109
mmpeter 0:386c250325ce 110
mmpeter 0:386c250325ce 111
mmpeter 0:386c250325ce 112 while(1) {
mmpeter 0:386c250325ce 113 // change "if" to while
mmpeter 0:386c250325ce 114 while(returned < 220){
mmpeter 0:386c250325ce 115 while(sensor[0] < sensor[4] && thinggy.line_position() != 0){
mmpeter 0:386c250325ce 116 thinggy.left_motor(.2);
mmpeter 0:386c250325ce 117 thinggy.right_motor(-.2);
mmpeter 0:386c250325ce 118 thinggy.calibrated_sensor(sensor);
mmpeter 0:386c250325ce 119 }
mmpeter 0:386c250325ce 120 while(sensor[4] > sensor[0] && thinggy.line_position() != 0){
mmpeter 0:386c250325ce 121 thinggy.left_motor(-.2);
mmpeter 0:386c250325ce 122 thinggy.right_motor(.2);
mmpeter 0:386c250325ce 123 thinggy.calibrated_sensor(sensor);
mmpeter 0:386c250325ce 124 }
mmpeter 0:386c250325ce 125 thinggy.calibrated_sensor(sensor);
mmpeter 0:386c250325ce 126 returned = (sensor[1] + sensor[2] + sensor[3])/3;
mmpeter 0:386c250325ce 127 }
mmpeter 0:386c250325ce 128
mmpeter 0:386c250325ce 129 // Curves and straightaways
mmpeter 0:386c250325ce 130 while(returned > 220){
mmpeter 0:386c250325ce 131
mmpeter 0:386c250325ce 132 float position = thinggy.line_position();
mmpeter 0:386c250325ce 133 correction = k*(position);
mmpeter 0:386c250325ce 134
mmpeter 0:386c250325ce 135 // -1.0 is far left, 1.0 is far right, 0.0 in the middle
mmpeter 0:386c250325ce 136
mmpeter 0:386c250325ce 137 //speed limiting for right motor
mmpeter 0:386c250325ce 138 if(speed + correction > 1){
mmpeter 0:386c250325ce 139 thinggy.right_motor(0.6);
mmpeter 0:386c250325ce 140 thinggy.left_motor(speed-correction);
mmpeter 0:386c250325ce 141 }
mmpeter 0:386c250325ce 142 else if(speed - correction > 1){
mmpeter 0:386c250325ce 143 thinggy.right_motor(speed+correction);
mmpeter 0:386c250325ce 144 thinggy.left_motor(0.6);
mmpeter 0:386c250325ce 145 }
mmpeter 0:386c250325ce 146 else{
mmpeter 0:386c250325ce 147 thinggy.left_motor(speed-correction);
mmpeter 0:386c250325ce 148 thinggy.right_motor(speed+correction);
mmpeter 0:386c250325ce 149 }
mmpeter 0:386c250325ce 150 thinggy.calibrated_sensor(sensor);
mmpeter 0:386c250325ce 151 returned = (sensor[1] + sensor[2] + sensor[3])/3;
mmpeter 0:386c250325ce 152 }
mmpeter 0:386c250325ce 153 thinggy.calibrated_sensor(sensor);
mmpeter 0:386c250325ce 154 returned = (sensor[1] + sensor[2] + sensor[3])/3;
mmpeter 0:386c250325ce 155 }
satyson 1:6402638c6f6d 156 */
mmpeter 0:386c250325ce 157 }
mmpeter 0:386c250325ce 158
mmpeter 0:386c250325ce 159
mmpeter 0:386c250325ce 160