der Roboter / Mbed 2 deprecated Working_on_Left_and_RightV21

s

Dependencies:   m3pi_ng mbed

Fork of Working_on_Left_and_Right by der Roboter

main.cpp@10:d1795905c412, 2014-06-04 (annotated)

Committer:
maghayes
Date:
Wed Jun 04 12:44:36 2014 +0000
Revision:
10:d1795905c412
Parent:
9:accfae3aaf72 
Work in Progress with Bluetooth Control;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mmpeter 0:9ab1097149ca 1 #include "mbed.h"
mmpeter 0:9ab1097149ca 2 #include "m3pi_ng.h"
mmpeter 0:9ab1097149ca 3 #include "cmath"
mmpeter 0:9ab1097149ca 4 #include "iostream"
bayagich 4:057e904b1395 5 #include <string>
maghayes 10:d1795905c412 6 #include "btbee.h"
mmpeter 0:9ab1097149ca 7
mmpeter 0:9ab1097149ca 8 //Access infared sensors
mmpeter 0:9ab1097149ca 9 DigitalIn m3pi_IN[] = {(p12)};
maghayes 10:d1795905c412 10 DigitalIn m3pi_pb = (p21);
maghayes 10:d1795905c412 11 DigitalOut led_1=(p13);
maghayes 10:d1795905c412 12 DigitalOut m3pi_led[] = {(p13), (p14), (p15), (p16), (p17), (p18), (p19), (p20)};
mmpeter 0:9ab1097149ca 13 using namespace std;
bayagich 2:b5031bb5303e 14
bayagich 3:bac13ce5f5d0 15 bool cross_detection(int sensor[5], int black_thresh, int white_thresh);
bayagich 5:cee0a2fc8816 16 void mapping(string directions, float speed, int turns);
bayagich 4:057e904b1395 17 string directions(int startpoint, int endpoint);
maghayes 7:dd79b0942620 18 void turn_right();
maghayes 7:dd79b0942620 19 void turn_left();
maghayes 7:dd79b0942620 20
bayagich 4:057e904b1395 21 int end();
bayagich 4:057e904b1395 22 int start();
bayagich 4:057e904b1395 23
mmpeter 0:9ab1097149ca 24
mmpeter 0:9ab1097149ca 25 m3pi thinggy;
maghayes 10:d1795905c412 26 btbee btbee;
bayagich 4:057e904b1395 27
bayagich 4:057e904b1395 28 int black_thresh = 300;
bayagich 4:057e904b1395 29 int white_thresh = 240;
mmpeter 0:9ab1097149ca 30 float speed = 0.25;
mmpeter 0:9ab1097149ca 31 float turn_speed = 0.2;
mmpeter 0:9ab1097149ca 32 float correction;
mmpeter 0:9ab1097149ca 33 float k = -0.3;
mmpeter 0:9ab1097149ca 34 int sensor[5];
bayagich 4:057e904b1395 35 int returned;
bayagich 4:057e904b1395 36
bayagich 4:057e904b1395 37
bayagich 5:cee0a2fc8816 38 int main() {
bayagich 3:bac13ce5f5d0 39 bool cross = 0;
mmpeter 0:9ab1097149ca 40
mmpeter 0:9ab1097149ca 41 wait(1.0);
mmpeter 0:9ab1097149ca 42
mmpeter 0:9ab1097149ca 43 thinggy.sensor_auto_calibrate();
mmpeter 0:9ab1097149ca 44
mmpeter 0:9ab1097149ca 45 thinggy.calibrated_sensor(sensor);
mmpeter 0:9ab1097149ca 46
mmpeter 0:9ab1097149ca 47 //find the average of the three sensors
mmpeter 0:9ab1097149ca 48 returned = (sensor[1] + sensor[2] + sensor[3])/3;
bayagich 4:057e904b1395 49
bayagich 4:057e904b1395 50 //finds the directions of the robot
maghayes 10:d1795905c412 51 char startpt; //start();
maghayes 10:d1795905c412 52 char endpt; //end();
maghayes 10:d1795905c412 53 char arr_read[2] = {'0','0'};
maghayes 10:d1795905c412 54 int chars_read;
maghayes 10:d1795905c412 55 btbee.reset();
maghayes 10:d1795905c412 56
maghayes 10:d1795905c412 57 while(m3pi_pb) {
maghayes 10:d1795905c412 58 m3pi_led[0]=!m3pi_led[0];
maghayes 10:d1795905c412 59 wait(0.1);
maghayes 10:d1795905c412 60 btbee.printf("Go!\n");
maghayes 10:d1795905c412 61 }
maghayes 10:d1795905c412 62 // check for answers after each write /not write
maghayes 10:d1795905c412 63
maghayes 10:d1795905c412 64 btbee.read_all(arr_read, 2, &chars_read );
maghayes 10:d1795905c412 65 thinggy.locate(0,0);
maghayes 10:d1795905c412 66 thinggy.cls();
maghayes 10:d1795905c412 67
maghayes 10:d1795905c412 68 startpt = arr_read[0];
maghayes 10:d1795905c412 69 endpt = arr_read[1];
maghayes 10:d1795905c412 70
maghayes 10:d1795905c412 71 wait(0.1);
maghayes 10:d1795905c412 72 thinggy.printf("Going");
maghayes 10:d1795905c412 73 thinggy.locate(0,1);
maghayes 10:d1795905c412 74 thinggy.printf("%c", startpt);
maghayes 10:d1795905c412 75 thinggy.printf(" ");
maghayes 10:d1795905c412 76 thinggy.printf("%c", endpt);
maghayes 10:d1795905c412 77
maghayes 10:d1795905c412 78 //turn char endpoints into int endpoints
maghayes 10:d1795905c412 79 int startpt_int = startpt - '0';
maghayes 10:d1795905c412 80 int endpt_int = endpt - '0';
maghayes 10:d1795905c412 81
bayagich 4:057e904b1395 82 string d = directions(startpt, endpt);
bayagich 4:057e904b1395 83 int turns = 0;
mmpeter 0:9ab1097149ca 84
maghayes 10:d1795905c412 85
mmpeter 0:9ab1097149ca 86 while(1) {
bayagich 4:057e904b1395 87 //check if it needs to turn
mmpeter 0:9ab1097149ca 88 while(returned <= 240){
mmpeter 0:9ab1097149ca 89 //turns right
mmpeter 0:9ab1097149ca 90 while(sensor[0] < sensor[4] && thinggy.line_position() != 0){
bayagich 3:bac13ce5f5d0 91 cross = cross_detection(sensor, black_thresh, white_thresh);
bayagich 3:bac13ce5f5d0 92 if(cross){
bayagich 5:cee0a2fc8816 93 mapping(d, speed, turns);
bayagich 5:cee0a2fc8816 94 ++turns;
bayagich 5:cee0a2fc8816 95 cross = 0;
bayagich 3:bac13ce5f5d0 96 }
bayagich 8:d5a5d104996e 97 else {
bayagich 8:d5a5d104996e 98 thinggy.left_motor(turn_speed);
bayagich 8:d5a5d104996e 99 thinggy.right_motor(-turn_speed);
bayagich 8:d5a5d104996e 100 }
bayagich 4:057e904b1395 101
bayagich 4:057e904b1395 102 thinggy.calibrated_sensor(sensor);
maghayes 7:dd79b0942620 103
mmpeter 0:9ab1097149ca 104 }
mmpeter 0:9ab1097149ca 105 //turns left
mmpeter 0:9ab1097149ca 106 while(sensor[4] > sensor[0] && thinggy.line_position() != 0){
bayagich 3:bac13ce5f5d0 107 cross = cross_detection(sensor, black_thresh, white_thresh);
bayagich 3:bac13ce5f5d0 108 if(cross){
bayagich 5:cee0a2fc8816 109 mapping(d, speed, turns);
bayagich 4:057e904b1395 110 ++turns;
bayagich 5:cee0a2fc8816 111 cross = 0;
bayagich 3:bac13ce5f5d0 112 }
bayagich 8:d5a5d104996e 113 else{
bayagich 8:d5a5d104996e 114 thinggy.left_motor(-turn_speed);
bayagich 8:d5a5d104996e 115 thinggy.right_motor(turn_speed);
bayagich 8:d5a5d104996e 116 }
bayagich 4:057e904b1395 117
bayagich 4:057e904b1395 118 thinggy.calibrated_sensor(sensor);
mmpeter 0:9ab1097149ca 119 }
mmpeter 0:9ab1097149ca 120 thinggy.calibrated_sensor(sensor);
mmpeter 1:4f52a001926a 121 returned = (sensor[1] + sensor[2]*2 + sensor[3])/4;
bayagich 4:057e904b1395 122
mmpeter 0:9ab1097149ca 123 }//while returned <= 220
mmpeter 0:9ab1097149ca 124
mmpeter 0:9ab1097149ca 125 // Curves and straightaways
mmpeter 0:9ab1097149ca 126 while(returned > 240){
mmpeter 0:9ab1097149ca 127 float position = thinggy.line_position();
mmpeter 0:9ab1097149ca 128 correction = k*(position);
bayagich 4:057e904b1395 129 cross = cross_detection(sensor, black_thresh, white_thresh);
bayagich 4:057e904b1395 130 if(cross){
bayagich 5:cee0a2fc8816 131 mapping(d, speed, turns);
bayagich 5:cee0a2fc8816 132 ++turns;
bayagich 5:cee0a2fc8816 133 cross = 0;
bayagich 4:057e904b1395 134 }
mmpeter 0:9ab1097149ca 135 // -1.0 is far left, 1.0 is far right, 0.0 in the middle
mmpeter 0:9ab1097149ca 136
mmpeter 0:9ab1097149ca 137 //speed limiting for right motor
bayagich 5:cee0a2fc8816 138 if(speed + correction > 1){
mmpeter 0:9ab1097149ca 139 thinggy.right_motor(0.6);
mmpeter 0:9ab1097149ca 140 thinggy.left_motor(speed-correction);
mmpeter 0:9ab1097149ca 141 }
mmpeter 0:9ab1097149ca 142
mmpeter 0:9ab1097149ca 143 //speed limiting for left motor
bayagich 4:057e904b1395 144 else if(speed - correction > 1){
mmpeter 0:9ab1097149ca 145 thinggy.left_motor(0.6);
mmpeter 0:9ab1097149ca 146 thinggy.right_motor(speed+correction);
mmpeter 0:9ab1097149ca 147 }
mmpeter 0:9ab1097149ca 148 else{
mmpeter 0:9ab1097149ca 149 thinggy.left_motor(speed-correction);
mmpeter 0:9ab1097149ca 150 thinggy.right_motor(speed+correction);
mmpeter 0:9ab1097149ca 151
mmpeter 0:9ab1097149ca 152 //Infared: stop if obstructed
mmpeter 0:9ab1097149ca 153 m3pi_IN[0].mode(PullUp);
mmpeter 0:9ab1097149ca 154 while (m3pi_IN[0]==0){
mmpeter 0:9ab1097149ca 155 thinggy.stop();
bayagich 4:057e904b1395 156 }
mmpeter 0:9ab1097149ca 157 }
mmpeter 0:9ab1097149ca 158 thinggy.calibrated_sensor(sensor);
mmpeter 1:4f52a001926a 159 returned = (sensor[1] + sensor[2]*2 + sensor[3])/4;
mmpeter 0:9ab1097149ca 160 }//while returned > 220
mmpeter 0:9ab1097149ca 161
bayagich 2:b5031bb5303e 162 thinggy.calibrated_sensor(sensor);
bayagich 2:b5031bb5303e 163 returned = (sensor[1] + sensor[2]*2 + sensor[3])/4;
mmpeter 0:9ab1097149ca 164 }//while(1)
bayagich 2:b5031bb5303e 165
bayagich 2:b5031bb5303e 166
mmpeter 0:9ab1097149ca 167 }
bayagich 4:057e904b1395 168
bayagich 4:057e904b1395 169 //DONE
bayagich 2:b5031bb5303e 170 //REQUIRES: array of 5 ints
bayagich 2:b5031bb5303e 171 //EFFECTS: stops the robot if it comes to any interesection where a decision has to be made
bayagich 3:bac13ce5f5d0 172 // and returns true if there is a cross
bayagich 3:bac13ce5f5d0 173 bool cross_detection(int sensor[5], int black_thresh, int white_thresh){
bayagich 2:b5031bb5303e 174 //three directions to choose from NOT WORKING
bayagich 3:bac13ce5f5d0 175 if(sensor[0] > black_thresh and sensor[2] > black_thresh and sensor[4] > black_thresh){
bayagich 3:bac13ce5f5d0 176 return 1;
bayagich 2:b5031bb5303e 177 }
bayagich 2:b5031bb5303e 178 //left or forward
bayagich 3:bac13ce5f5d0 179 else if(sensor[0] > black_thresh and sensor[2] > black_thresh){
bayagich 3:bac13ce5f5d0 180 return 1;
bayagich 2:b5031bb5303e 181 }
bayagich 2:b5031bb5303e 182 //left or right WORKING
bayagich 2:b5031bb5303e 183 else if (sensor[0] > black_thresh and sensor[1] < white_thresh and sensor[2] < white_thresh and sensor[3] < white_thresh and sensor[4] > black_thresh ){
bayagich 3:bac13ce5f5d0 184 return 1;
bayagich 2:b5031bb5303e 185 }
bayagich 2:b5031bb5303e 186 //forward or right
bayagich 3:bac13ce5f5d0 187 else if (sensor[2] > black_thresh and sensor[4] > black_thresh){
bayagich 3:bac13ce5f5d0 188 return 1;
bayagich 2:b5031bb5303e 189 }
bayagich 3:bac13ce5f5d0 190
bayagich 3:bac13ce5f5d0 191 return 0;
bayagich 2:b5031bb5303e 192 }
bayagich 4:057e904b1395 193
bayagich 4:057e904b1395 194 //take in the starting point of the robot from bluetooth
bayagich 4:057e904b1395 195 int start(){
bayagich 4:057e904b1395 196 return 0;
bayagich 4:057e904b1395 197 }
bayagich 4:057e904b1395 198
bayagich 4:057e904b1395 199 //take in the ending point of the robot from bluetooth
bayagich 4:057e904b1395 200 int end(){
bayagich 4:057e904b1395 201 return 0;
bayagich 4:057e904b1395 202 }
bayagich 4:057e904b1395 203
bayagich 4:057e904b1395 204 //DONE
bayagich 4:057e904b1395 205 //gives the string to use for the map
bayagich 4:057e904b1395 206 string directions(int startpoint, int endpoint){
bayagich 4:057e904b1395 207 string charmap[6][6] = {
akul 9:accfae3aaf72 208 {"", "RLRLLR", "RLRLFF", "RLRRRLLF", "RLRRLFLRRRL", "RLRRLR"},
akul 9:accfae3aaf72 209 {"LRRLR", "", "LLF", "LLRLL", "LFRLRRL", "LRFLR"},
bayagich 4:057e904b1395 210 {"FFRL", "FRR", "", "LL", "FLRLRRL", "FFFLR"},
bayagich 4:057e904b1395 211 {"FRRLRL", "RLRR", "RR", "", "FFRL", "FRLRL"},
bayagich 4:057e904b1395 212 {"RLLLRFRLL", "RLLRLFR", "RLFLR", "RLFF", "", "RLLLRL"},
bayagich 4:057e904b1395 213 {"LRLL", "LFLR", "LRFFF", "RLRLF", "RLRRRL", ""}
bayagich 4:057e904b1395 214 };
bayagich 4:057e904b1395 215 return charmap[startpoint - 1][endpoint - 1];
bayagich 4:057e904b1395 216 }
bayagich 4:057e904b1395 217
bayagich 4:057e904b1395 218 //DONE
bayagich 4:057e904b1395 219 //takes in the string of directions and the number of turns
bayagich 4:057e904b1395 220 //completed and then tells it whether to do left, right, forward
bayagich 5:cee0a2fc8816 221 void mapping(string directions, float speed, int turns){
bayagich 4:057e904b1395 222
bayagich 4:057e904b1395 223 char x = directions[turns]; //something in the string
bayagich 4:057e904b1395 224
bayagich 4:057e904b1395 225 //tells it which direction to go
bayagich 5:cee0a2fc8816 226 if(x == 'L'){
maghayes 7:dd79b0942620 227 turn_left();
bayagich 5:cee0a2fc8816 228 return;
bayagich 4:057e904b1395 229 }
bayagich 4:057e904b1395 230 else if(x == 'R'){
maghayes 7:dd79b0942620 231 turn_right();
bayagich 5:cee0a2fc8816 232 return;
bayagich 4:057e904b1395 233 }
bayagich 4:057e904b1395 234 else if(x == 'F'){
bayagich 8:d5a5d104996e 235 thinggy.printf("F");
bayagich 8:d5a5d104996e 236 thinggy.forward(speed);
bayagich 8:d5a5d104996e 237 wait(.1);
bayagich 5:cee0a2fc8816 238 return;
bayagich 5:cee0a2fc8816 239 }
bayagich 5:cee0a2fc8816 240 else if (x == NULL){
bayagich 5:cee0a2fc8816 241 thinggy.stop();
bayagich 5:cee0a2fc8816 242 wait(300);
bayagich 5:cee0a2fc8816 243 return;
bayagich 4:057e904b1395 244 }
maghayes 7:dd79b0942620 245 }
maghayes 7:dd79b0942620 246
maghayes 7:dd79b0942620 247 void turn_left(){
maghayes 7:dd79b0942620 248 int n=0;
bayagich 8:d5a5d104996e 249 wait(.08);
akul 9:accfae3aaf72 250 while (n<700){
bayagich 8:d5a5d104996e 251 thinggy.left_motor(-0.2);
bayagich 8:d5a5d104996e 252 thinggy.right_motor(0.2);
maghayes 7:dd79b0942620 253 n++;
maghayes 7:dd79b0942620 254 }
maghayes 7:dd79b0942620 255 thinggy.forward(0.2);
maghayes 7:dd79b0942620 256 wait(0.15);
maghayes 7:dd79b0942620 257
maghayes 7:dd79b0942620 258 }
maghayes 7:dd79b0942620 259
maghayes 7:dd79b0942620 260 void turn_right(){
maghayes 7:dd79b0942620 261 int n=0;
bayagich 8:d5a5d104996e 262 wait(.08);
akul 9:accfae3aaf72 263 while (n<700){
bayagich 8:d5a5d104996e 264 thinggy.left_motor(0.2);
bayagich 8:d5a5d104996e 265 thinggy.right_motor(-0.2);
maghayes 7:dd79b0942620 266 n++;
maghayes 7:dd79b0942620 267 }
maghayes 7:dd79b0942620 268 thinggy.forward(0.2);
bayagich 8:d5a5d104996e 269 wait(0.15);
maghayes 7:dd79b0942620 270
bayagich 4:057e904b1395 271 }