Amanda Bayagich / Mbed 2 deprecated Final_comments

AKUL check if this has the correct right and left turning, if not replace it Code with lots of comments

Dependencies:   m3pi_ng mbed

Fork of Working_on_Left_and_Right by der Roboter

main.cpp@9:accfae3aaf72, 2014-06-04 (annotated)

Committer:
akul
Date:
Wed Jun 04 11:38:06 2014 +0000
Revision:
9:accfae3aaf72
Parent:
8:d5a5d104996e
Child:
10:e2267771f7d4
mapping is pretty good (error checked all routes from start point 2) issue with negotiating curves in map

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();
akul 9:accfae3aaf72 49 int endpt = 1; //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
mmpeter 0:9ab1097149ca 57 while(sensor[0] < sensor[4] && thinggy.line_position() != 0){
bayagich 3:bac13ce5f5d0 58 cross = cross_detection(sensor, black_thresh, white_thresh);
bayagich 3:bac13ce5f5d0 59 if(cross){
bayagich 5:cee0a2fc8816 60 mapping(d, speed, turns);
bayagich 5:cee0a2fc8816 61 ++turns;
bayagich 5:cee0a2fc8816 62 cross = 0;
bayagich 3:bac13ce5f5d0 63 }
bayagich 8:d5a5d104996e 64 else {
bayagich 8:d5a5d104996e 65 thinggy.left_motor(turn_speed);
bayagich 8:d5a5d104996e 66 thinggy.right_motor(-turn_speed);
bayagich 8:d5a5d104996e 67 }
bayagich 4:057e904b1395 68
bayagich 4:057e904b1395 69 thinggy.calibrated_sensor(sensor);
maghayes 7:dd79b0942620 70
mmpeter 0:9ab1097149ca 71 }
mmpeter 0:9ab1097149ca 72 //turns left
mmpeter 0:9ab1097149ca 73 while(sensor[4] > sensor[0] && thinggy.line_position() != 0){
bayagich 3:bac13ce5f5d0 74 cross = cross_detection(sensor, black_thresh, white_thresh);
bayagich 3:bac13ce5f5d0 75 if(cross){
bayagich 5:cee0a2fc8816 76 mapping(d, speed, turns);
bayagich 4:057e904b1395 77 ++turns;
bayagich 5:cee0a2fc8816 78 cross = 0;
bayagich 3:bac13ce5f5d0 79 }
bayagich 8:d5a5d104996e 80 else{
bayagich 8:d5a5d104996e 81 thinggy.left_motor(-turn_speed);
bayagich 8:d5a5d104996e 82 thinggy.right_motor(turn_speed);
bayagich 8:d5a5d104996e 83 }
bayagich 4:057e904b1395 84
bayagich 4:057e904b1395 85 thinggy.calibrated_sensor(sensor);
mmpeter 0:9ab1097149ca 86 }
mmpeter 0:9ab1097149ca 87 thinggy.calibrated_sensor(sensor);
mmpeter 1:4f52a001926a 88 returned = (sensor[1] + sensor[2]*2 + sensor[3])/4;
bayagich 4:057e904b1395 89
mmpeter 0:9ab1097149ca 90 }//while returned <= 220
mmpeter 0:9ab1097149ca 91
mmpeter 0:9ab1097149ca 92 // Curves and straightaways
mmpeter 0:9ab1097149ca 93 while(returned > 240){
mmpeter 0:9ab1097149ca 94 float position = thinggy.line_position();
mmpeter 0:9ab1097149ca 95 correction = k*(position);
bayagich 4:057e904b1395 96 cross = cross_detection(sensor, black_thresh, white_thresh);
bayagich 4:057e904b1395 97 if(cross){
bayagich 5:cee0a2fc8816 98 mapping(d, speed, turns);
bayagich 5:cee0a2fc8816 99 ++turns;
bayagich 5:cee0a2fc8816 100 cross = 0;
bayagich 4:057e904b1395 101 }
mmpeter 0:9ab1097149ca 102 // -1.0 is far left, 1.0 is far right, 0.0 in the middle
mmpeter 0:9ab1097149ca 103
mmpeter 0:9ab1097149ca 104 //speed limiting for right motor
bayagich 5:cee0a2fc8816 105 if(speed + correction > 1){
mmpeter 0:9ab1097149ca 106 thinggy.right_motor(0.6);
mmpeter 0:9ab1097149ca 107 thinggy.left_motor(speed-correction);
mmpeter 0:9ab1097149ca 108 }
mmpeter 0:9ab1097149ca 109
mmpeter 0:9ab1097149ca 110 //speed limiting for left motor
bayagich 4:057e904b1395 111 else if(speed - correction > 1){
mmpeter 0:9ab1097149ca 112 thinggy.left_motor(0.6);
mmpeter 0:9ab1097149ca 113 thinggy.right_motor(speed+correction);
mmpeter 0:9ab1097149ca 114 }
mmpeter 0:9ab1097149ca 115 else{
mmpeter 0:9ab1097149ca 116 thinggy.left_motor(speed-correction);
mmpeter 0:9ab1097149ca 117 thinggy.right_motor(speed+correction);
mmpeter 0:9ab1097149ca 118
mmpeter 0:9ab1097149ca 119 //Infared: stop if obstructed
mmpeter 0:9ab1097149ca 120 m3pi_IN[0].mode(PullUp);
mmpeter 0:9ab1097149ca 121 while (m3pi_IN[0]==0){
mmpeter 0:9ab1097149ca 122 thinggy.stop();
bayagich 4:057e904b1395 123 }
mmpeter 0:9ab1097149ca 124 }
mmpeter 0:9ab1097149ca 125 thinggy.calibrated_sensor(sensor);
mmpeter 1:4f52a001926a 126 returned = (sensor[1] + sensor[2]*2 + sensor[3])/4;
mmpeter 0:9ab1097149ca 127 }//while returned > 220
mmpeter 0:9ab1097149ca 128
bayagich 2:b5031bb5303e 129 thinggy.calibrated_sensor(sensor);
bayagich 2:b5031bb5303e 130 returned = (sensor[1] + sensor[2]*2 + sensor[3])/4;
mmpeter 0:9ab1097149ca 131 }//while(1)
bayagich 2:b5031bb5303e 132
bayagich 2:b5031bb5303e 133
mmpeter 0:9ab1097149ca 134 }
bayagich 4:057e904b1395 135
bayagich 4:057e904b1395 136 //DONE
bayagich 2:b5031bb5303e 137 //REQUIRES: array of 5 ints
bayagich 2:b5031bb5303e 138 //EFFECTS: stops the robot if it comes to any interesection where a decision has to be made
bayagich 3:bac13ce5f5d0 139 // and returns true if there is a cross
bayagich 3:bac13ce5f5d0 140 bool cross_detection(int sensor[5], int black_thresh, int white_thresh){
bayagich 2:b5031bb5303e 141 //three directions to choose from NOT WORKING
bayagich 3:bac13ce5f5d0 142 if(sensor[0] > black_thresh and sensor[2] > black_thresh and sensor[4] > black_thresh){
bayagich 3:bac13ce5f5d0 143 return 1;
bayagich 2:b5031bb5303e 144 }
bayagich 2:b5031bb5303e 145 //left or forward
bayagich 3:bac13ce5f5d0 146 else if(sensor[0] > black_thresh and sensor[2] > black_thresh){
bayagich 3:bac13ce5f5d0 147 return 1;
bayagich 2:b5031bb5303e 148 }
bayagich 2:b5031bb5303e 149 //left or right WORKING
bayagich 2:b5031bb5303e 150 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 151 return 1;
bayagich 2:b5031bb5303e 152 }
bayagich 2:b5031bb5303e 153 //forward or right
bayagich 3:bac13ce5f5d0 154 else if (sensor[2] > black_thresh and sensor[4] > black_thresh){
bayagich 3:bac13ce5f5d0 155 return 1;
bayagich 2:b5031bb5303e 156 }
bayagich 3:bac13ce5f5d0 157
bayagich 3:bac13ce5f5d0 158 return 0;
bayagich 2:b5031bb5303e 159 }
bayagich 4:057e904b1395 160
bayagich 4:057e904b1395 161 //take in the starting point of the robot from bluetooth
bayagich 4:057e904b1395 162 int start(){
bayagich 4:057e904b1395 163 return 0;
bayagich 4:057e904b1395 164 }
bayagich 4:057e904b1395 165
bayagich 4:057e904b1395 166 //take in the ending point of the robot from bluetooth
bayagich 4:057e904b1395 167 int end(){
bayagich 4:057e904b1395 168 return 0;
bayagich 4:057e904b1395 169 }
bayagich 4:057e904b1395 170
bayagich 4:057e904b1395 171 //DONE
bayagich 4:057e904b1395 172 //gives the string to use for the map
bayagich 4:057e904b1395 173 string directions(int startpoint, int endpoint){
bayagich 4:057e904b1395 174 string charmap[6][6] = {
akul 9:accfae3aaf72 175 {"", "RLRLLR", "RLRLFF", "RLRRRLLF", "RLRRLFLRRRL", "RLRRLR"},
akul 9:accfae3aaf72 176 {"LRRLR", "", "LLF", "LLRLL", "LFRLRRL", "LRFLR"},
bayagich 4:057e904b1395 177 {"FFRL", "FRR", "", "LL", "FLRLRRL", "FFFLR"},
bayagich 4:057e904b1395 178 {"FRRLRL", "RLRR", "RR", "", "FFRL", "FRLRL"},
bayagich 4:057e904b1395 179 {"RLLLRFRLL", "RLLRLFR", "RLFLR", "RLFF", "", "RLLLRL"},
bayagich 4:057e904b1395 180 {"LRLL", "LFLR", "LRFFF", "RLRLF", "RLRRRL", ""}
bayagich 4:057e904b1395 181 };
bayagich 4:057e904b1395 182 return charmap[startpoint - 1][endpoint - 1];
bayagich 4:057e904b1395 183 }
bayagich 4:057e904b1395 184
bayagich 4:057e904b1395 185 //DONE
bayagich 4:057e904b1395 186 //takes in the string of directions and the number of turns
bayagich 4:057e904b1395 187 //completed and then tells it whether to do left, right, forward
bayagich 5:cee0a2fc8816 188 void mapping(string directions, float speed, int turns){
bayagich 4:057e904b1395 189
bayagich 4:057e904b1395 190 char x = directions[turns]; //something in the string
bayagich 4:057e904b1395 191
bayagich 4:057e904b1395 192 //tells it which direction to go
bayagich 5:cee0a2fc8816 193 if(x == 'L'){
maghayes 7:dd79b0942620 194 turn_left();
bayagich 5:cee0a2fc8816 195 return;
bayagich 4:057e904b1395 196 }
bayagich 4:057e904b1395 197 else if(x == 'R'){
maghayes 7:dd79b0942620 198 turn_right();
bayagich 5:cee0a2fc8816 199 return;
bayagich 4:057e904b1395 200 }
bayagich 4:057e904b1395 201 else if(x == 'F'){
bayagich 8:d5a5d104996e 202 thinggy.printf("F");
bayagich 8:d5a5d104996e 203 thinggy.forward(speed);
bayagich 8:d5a5d104996e 204 wait(.1);
bayagich 5:cee0a2fc8816 205 return;
bayagich 5:cee0a2fc8816 206 }
bayagich 5:cee0a2fc8816 207 else if (x == NULL){
bayagich 5:cee0a2fc8816 208 thinggy.stop();
bayagich 5:cee0a2fc8816 209 wait(300);
bayagich 5:cee0a2fc8816 210 return;
bayagich 4:057e904b1395 211 }
maghayes 7:dd79b0942620 212 }
maghayes 7:dd79b0942620 213
maghayes 7:dd79b0942620 214 void turn_left(){
maghayes 7:dd79b0942620 215 int n=0;
bayagich 8:d5a5d104996e 216 wait(.08);
akul 9:accfae3aaf72 217 while (n<700){
bayagich 8:d5a5d104996e 218 thinggy.left_motor(-0.2);
bayagich 8:d5a5d104996e 219 thinggy.right_motor(0.2);
maghayes 7:dd79b0942620 220 n++;
maghayes 7:dd79b0942620 221 }
maghayes 7:dd79b0942620 222 thinggy.forward(0.2);
maghayes 7:dd79b0942620 223 wait(0.15);
maghayes 7:dd79b0942620 224
maghayes 7:dd79b0942620 225 }
maghayes 7:dd79b0942620 226
maghayes 7:dd79b0942620 227 void turn_right(){
maghayes 7:dd79b0942620 228 int n=0;
bayagich 8:d5a5d104996e 229 wait(.08);
akul 9:accfae3aaf72 230 while (n<700){
bayagich 8:d5a5d104996e 231 thinggy.left_motor(0.2);
bayagich 8:d5a5d104996e 232 thinggy.right_motor(-0.2);
maghayes 7:dd79b0942620 233 n++;
maghayes 7:dd79b0942620 234 }
maghayes 7:dd79b0942620 235 thinggy.forward(0.2);
bayagich 8:d5a5d104996e 236 wait(0.15);
maghayes 7:dd79b0942620 237
bayagich 4:057e904b1395 238 }