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@7:dd79b0942620, 2014-06-03 (annotated)

Committer:
maghayes
Date:
Tue Jun 03 12:10:29 2014 +0000
Revision:
7:dd79b0942620
Parent:
6:a6f752174eef
Child:
8:d5a5d104996e
working on left and right turning;

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