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@10:e2267771f7d4, 2014-06-05 (annotated)

Committer:
bayagich
Date:
Thu Jun 05 08:49:03 2014 +0000
Revision:
10:e2267771f7d4
Parent:
9:accfae3aaf72 
AKUL check if this has the correct turn left and turn right, if not replace it. ; ; Code with comments, may not have the correct turn left, turn right;

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 10:e2267771f7d4 13 //EFFECTS: looks at a number of sensors to determine if the robot has reached a cross.
bayagich 10:e2267771f7d4 14 // If the robot has reached a cross, it returns TRUE. If not, it returns FALSE
bayagich 3:bac13ce5f5d0 15 bool cross_detection(int sensor[5], int black_thresh, int white_thresh);
bayagich 10:e2267771f7d4 16
bayagich 10:e2267771f7d4 17 // REQUIRES: Startpoint and endpoint must be between 1 and 6, inclusive
bayagich 10:e2267771f7d4 18 // EFFECTS: returns a string of directions fo either L (left), R (right), or
bayagich 10:e2267771f7d4 19 // F (forward) to tell the robot how to get from Startpoint to endpoints
bayagich 10:e2267771f7d4 20 string directions(int startpoint, int endpoint);
bayagich 10:e2267771f7d4 21
bayagich 10:e2267771f7d4 22
bayagich 5:cee0a2fc8816 23 void mapping(string directions, float speed, int turns);
bayagich 10:e2267771f7d4 24
maghayes 7:dd79b0942620 25 void turn_right();
maghayes 7:dd79b0942620 26 void turn_left();
mmpeter 0:9ab1097149ca 27
mmpeter 0:9ab1097149ca 28 m3pi thinggy;
bayagich 4:057e904b1395 29
bayagich 10:e2267771f7d4 30 //black and white thresholds chose after testing
bayagich 10:e2267771f7d4 31 int black_thresh = 300;
bayagich 10:e2267771f7d4 32 int white_thresh = 240;
bayagich 10:e2267771f7d4 33
bayagich 10:e2267771f7d4 34 //normal speed and turn speed to slow
bayagich 10:e2267771f7d4 35 float speed = 0.25;
bayagich 10:e2267771f7d4 36 float turn_speed = 0.2;
bayagich 10:e2267771f7d4 37
bayagich 10:e2267771f7d4 38 //used to change the direction of the car
bayagich 10:e2267771f7d4 39 float correction;
bayagich 10:e2267771f7d4 40
bayagich 10:e2267771f7d4 41 //k was chosen after testing
bayagich 10:e2267771f7d4 42 float k = -0.3;
bayagich 10:e2267771f7d4 43
bayagich 10:e2267771f7d4 44 int sensor[5];
bayagich 10:e2267771f7d4 45 int returned;
bayagich 4:057e904b1395 46
bayagich 4:057e904b1395 47
bayagich 5:cee0a2fc8816 48 int main() {
bayagich 3:bac13ce5f5d0 49 bool cross = 0;
mmpeter 0:9ab1097149ca 50
mmpeter 0:9ab1097149ca 51 wait(1.0);
bayagich 10:e2267771f7d4 52
bayagich 10:e2267771f7d4 53 //calibrate the sensors
mmpeter 0:9ab1097149ca 54 thinggy.sensor_auto_calibrate();
mmpeter 0:9ab1097149ca 55 thinggy.calibrated_sensor(sensor);
mmpeter 0:9ab1097149ca 56
mmpeter 0:9ab1097149ca 57 //find the average of the three sensors
mmpeter 0:9ab1097149ca 58 returned = (sensor[1] + sensor[2] + sensor[3])/3;
bayagich 4:057e904b1395 59
bayagich 10:e2267771f7d4 60 //enter in the endpoint and startpoint here
bayagich 10:e2267771f7d4 61 int startpt = 2;
bayagich 10:e2267771f7d4 62 int endpt = 1;
bayagich 4:057e904b1395 63 string d = directions(startpt, endpt);
bayagich 10:e2267771f7d4 64
bayagich 10:e2267771f7d4 65 //declares the number of turns so that it can be incremented after each turn
bayagich 4:057e904b1395 66 int turns = 0;
mmpeter 0:9ab1097149ca 67
bayagich 10:e2267771f7d4 68 //performs movement
mmpeter 0:9ab1097149ca 69 while(1) {
bayagich 10:e2267771f7d4 70
bayagich 10:e2267771f7d4 71 //checks if it needs to turn
mmpeter 0:9ab1097149ca 72 while(returned <= 240){
bayagich 10:e2267771f7d4 73
mmpeter 0:9ab1097149ca 74 //turns right
mmpeter 0:9ab1097149ca 75 while(sensor[0] < sensor[4] && thinggy.line_position() != 0){
bayagich 3:bac13ce5f5d0 76 cross = cross_detection(sensor, black_thresh, white_thresh);
bayagich 10:e2267771f7d4 77
bayagich 10:e2267771f7d4 78 //checks if there is a cross
bayagich 3:bac13ce5f5d0 79 if(cross){
bayagich 5:cee0a2fc8816 80 mapping(d, speed, turns);
bayagich 5:cee0a2fc8816 81 ++turns;
bayagich 5:cee0a2fc8816 82 cross = 0;
bayagich 3:bac13ce5f5d0 83 }
bayagich 10:e2267771f7d4 84 //corrects turn
bayagich 8:d5a5d104996e 85 else {
bayagich 8:d5a5d104996e 86 thinggy.left_motor(turn_speed);
bayagich 8:d5a5d104996e 87 thinggy.right_motor(-turn_speed);
bayagich 8:d5a5d104996e 88 }
bayagich 4:057e904b1395 89
bayagich 4:057e904b1395 90 thinggy.calibrated_sensor(sensor);
maghayes 7:dd79b0942620 91
mmpeter 0:9ab1097149ca 92 }
mmpeter 0:9ab1097149ca 93 //turns left
mmpeter 0:9ab1097149ca 94 while(sensor[4] > sensor[0] && thinggy.line_position() != 0){
bayagich 3:bac13ce5f5d0 95 cross = cross_detection(sensor, black_thresh, white_thresh);
bayagich 10:e2267771f7d4 96 //checks for cross
bayagich 3:bac13ce5f5d0 97 if(cross){
bayagich 5:cee0a2fc8816 98 mapping(d, speed, turns);
bayagich 4:057e904b1395 99 ++turns;
bayagich 5:cee0a2fc8816 100 cross = 0;
bayagich 3:bac13ce5f5d0 101 }
bayagich 10:e2267771f7d4 102 //corrects pathway
bayagich 8:d5a5d104996e 103 else{
bayagich 8:d5a5d104996e 104 thinggy.left_motor(-turn_speed);
bayagich 8:d5a5d104996e 105 thinggy.right_motor(turn_speed);
bayagich 8:d5a5d104996e 106 }
bayagich 4:057e904b1395 107
bayagich 4:057e904b1395 108 thinggy.calibrated_sensor(sensor);
mmpeter 0:9ab1097149ca 109 }
mmpeter 0:9ab1097149ca 110 thinggy.calibrated_sensor(sensor);
mmpeter 1:4f52a001926a 111 returned = (sensor[1] + sensor[2]*2 + sensor[3])/4;
bayagich 4:057e904b1395 112
mmpeter 0:9ab1097149ca 113 }//while returned <= 220
mmpeter 0:9ab1097149ca 114
mmpeter 0:9ab1097149ca 115 // Curves and straightaways
mmpeter 0:9ab1097149ca 116 while(returned > 240){
mmpeter 0:9ab1097149ca 117 float position = thinggy.line_position();
mmpeter 0:9ab1097149ca 118 correction = k*(position);
bayagich 4:057e904b1395 119 cross = cross_detection(sensor, black_thresh, white_thresh);
bayagich 10:e2267771f7d4 120 //checks for cross
bayagich 4:057e904b1395 121 if(cross){
bayagich 5:cee0a2fc8816 122 mapping(d, speed, turns);
bayagich 5:cee0a2fc8816 123 ++turns;
bayagich 5:cee0a2fc8816 124 cross = 0;
bayagich 4:057e904b1395 125 }
mmpeter 0:9ab1097149ca 126 // -1.0 is far left, 1.0 is far right, 0.0 in the middle
mmpeter 0:9ab1097149ca 127
mmpeter 0:9ab1097149ca 128 //speed limiting for right motor
bayagich 5:cee0a2fc8816 129 if(speed + correction > 1){
mmpeter 0:9ab1097149ca 130 thinggy.right_motor(0.6);
mmpeter 0:9ab1097149ca 131 thinggy.left_motor(speed-correction);
mmpeter 0:9ab1097149ca 132 }
mmpeter 0:9ab1097149ca 133
mmpeter 0:9ab1097149ca 134 //speed limiting for left motor
bayagich 4:057e904b1395 135 else if(speed - correction > 1){
mmpeter 0:9ab1097149ca 136 thinggy.left_motor(0.6);
mmpeter 0:9ab1097149ca 137 thinggy.right_motor(speed+correction);
mmpeter 0:9ab1097149ca 138 }
mmpeter 0:9ab1097149ca 139 else{
mmpeter 0:9ab1097149ca 140 thinggy.left_motor(speed-correction);
mmpeter 0:9ab1097149ca 141 thinggy.right_motor(speed+correction);
mmpeter 0:9ab1097149ca 142
bayagich 10:e2267771f7d4 143 //Infared: will stop if obstructed
mmpeter 0:9ab1097149ca 144 m3pi_IN[0].mode(PullUp);
mmpeter 0:9ab1097149ca 145 while (m3pi_IN[0]==0){
mmpeter 0:9ab1097149ca 146 thinggy.stop();
bayagich 4:057e904b1395 147 }
mmpeter 0:9ab1097149ca 148 }
bayagich 10:e2267771f7d4 149
bayagich 10:e2267771f7d4 150 //recalibrate
mmpeter 0:9ab1097149ca 151 thinggy.calibrated_sensor(sensor);
mmpeter 1:4f52a001926a 152 returned = (sensor[1] + sensor[2]*2 + sensor[3])/4;
mmpeter 0:9ab1097149ca 153 }//while returned > 220
mmpeter 0:9ab1097149ca 154
bayagich 2:b5031bb5303e 155 thinggy.calibrated_sensor(sensor);
bayagich 2:b5031bb5303e 156 returned = (sensor[1] + sensor[2]*2 + sensor[3])/4;
mmpeter 0:9ab1097149ca 157 }//while(1)
bayagich 2:b5031bb5303e 158
bayagich 2:b5031bb5303e 159
mmpeter 0:9ab1097149ca 160 }
bayagich 4:057e904b1395 161
bayagich 10:e2267771f7d4 162 //EFFECTS: looks at a number of sensors to determine if the robot has reached a cross.
bayagich 10:e2267771f7d4 163 // If the robot has reached a cross, it returns TRUE. If not, it returns FALSE
bayagich 3:bac13ce5f5d0 164 bool cross_detection(int sensor[5], int black_thresh, int white_thresh){
bayagich 2:b5031bb5303e 165 //three directions to choose from NOT WORKING
bayagich 3:bac13ce5f5d0 166 if(sensor[0] > black_thresh and sensor[2] > black_thresh and sensor[4] > black_thresh){
bayagich 3:bac13ce5f5d0 167 return 1;
bayagich 2:b5031bb5303e 168 }
bayagich 2:b5031bb5303e 169 //left or forward
bayagich 3:bac13ce5f5d0 170 else if(sensor[0] > black_thresh and sensor[2] > black_thresh){
bayagich 3:bac13ce5f5d0 171 return 1;
bayagich 2:b5031bb5303e 172 }
bayagich 2:b5031bb5303e 173 //left or right WORKING
bayagich 2:b5031bb5303e 174 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 175 return 1;
bayagich 2:b5031bb5303e 176 }
bayagich 2:b5031bb5303e 177 //forward or right
bayagich 3:bac13ce5f5d0 178 else if (sensor[2] > black_thresh and sensor[4] > black_thresh){
bayagich 3:bac13ce5f5d0 179 return 1;
bayagich 2:b5031bb5303e 180 }
bayagich 3:bac13ce5f5d0 181
bayagich 3:bac13ce5f5d0 182 return 0;
bayagich 2:b5031bb5303e 183 }
bayagich 4:057e904b1395 184
bayagich 4:057e904b1395 185
bayagich 10:e2267771f7d4 186 // REQUIRES: Startpoint and endpoint must be between 1 and 6, inclusive
bayagich 10:e2267771f7d4 187 // EFFECTS: returns a string of directions fo either L (left), R (right), or
bayagich 10:e2267771f7d4 188 // F (forward) to tell the robot how to get from Startpoint to endpoints
bayagich 4:057e904b1395 189 string directions(int startpoint, int endpoint){
bayagich 4:057e904b1395 190 string charmap[6][6] = {
akul 9:accfae3aaf72 191 {"", "RLRLLR", "RLRLFF", "RLRRRLLF", "RLRRLFLRRRL", "RLRRLR"},
akul 9:accfae3aaf72 192 {"LRRLR", "", "LLF", "LLRLL", "LFRLRRL", "LRFLR"},
bayagich 4:057e904b1395 193 {"FFRL", "FRR", "", "LL", "FLRLRRL", "FFFLR"},
bayagich 4:057e904b1395 194 {"FRRLRL", "RLRR", "RR", "", "FFRL", "FRLRL"},
bayagich 4:057e904b1395 195 {"RLLLRFRLL", "RLLRLFR", "RLFLR", "RLFF", "", "RLLLRL"},
bayagich 4:057e904b1395 196 {"LRLL", "LFLR", "LRFFF", "RLRLF", "RLRRRL", ""}
bayagich 4:057e904b1395 197 };
bayagich 10:e2267771f7d4 198
bayagich 4:057e904b1395 199 return charmap[startpoint - 1][endpoint - 1];
bayagich 4:057e904b1395 200 }
bayagich 4:057e904b1395 201
bayagich 10:e2267771f7d4 202 //MODIFIES: Direction and speed
bayagich 10:e2267771f7d4 203 //EFFECTS: Looks at the turn in the string of directions and tells it whether
bayagich 10:e2267771f7d4 204 // whether to go left, right, forward
bayagich 5:cee0a2fc8816 205 void mapping(string directions, float speed, int turns){
bayagich 4:057e904b1395 206
bayagich 10:e2267771f7d4 207 //char x is L, R, F, or NULL to perform at the specified turn
bayagich 10:e2267771f7d4 208 char x = directions[turns];
bayagich 4:057e904b1395 209
bayagich 10:e2267771f7d4 210 //left turn
bayagich 5:cee0a2fc8816 211 if(x == 'L'){
maghayes 7:dd79b0942620 212 turn_left();
bayagich 5:cee0a2fc8816 213 return;
bayagich 4:057e904b1395 214 }
bayagich 10:e2267771f7d4 215 //right turn
bayagich 4:057e904b1395 216 else if(x == 'R'){
maghayes 7:dd79b0942620 217 turn_right();
bayagich 5:cee0a2fc8816 218 return;
bayagich 4:057e904b1395 219 }
bayagich 10:e2267771f7d4 220 //move forward
bayagich 4:057e904b1395 221 else if(x == 'F'){
bayagich 8:d5a5d104996e 222 thinggy.forward(speed);
bayagich 8:d5a5d104996e 223 wait(.1);
bayagich 5:cee0a2fc8816 224 return;
bayagich 5:cee0a2fc8816 225 }
bayagich 10:e2267771f7d4 226 //if it reaches the last element in the string
bayagich 5:cee0a2fc8816 227 else if (x == NULL){
bayagich 5:cee0a2fc8816 228 thinggy.stop();
bayagich 5:cee0a2fc8816 229 wait(300);
bayagich 5:cee0a2fc8816 230 return;
bayagich 4:057e904b1395 231 }
maghayes 7:dd79b0942620 232 }
maghayes 7:dd79b0942620 233
bayagich 10:e2267771f7d4 234 //EFFECTS: turns the robot left
maghayes 7:dd79b0942620 235 void turn_left(){
maghayes 7:dd79b0942620 236 int n=0;
bayagich 8:d5a5d104996e 237 wait(.08);
akul 9:accfae3aaf72 238 while (n<700){
bayagich 8:d5a5d104996e 239 thinggy.left_motor(-0.2);
bayagich 8:d5a5d104996e 240 thinggy.right_motor(0.2);
maghayes 7:dd79b0942620 241 n++;
maghayes 7:dd79b0942620 242 }
maghayes 7:dd79b0942620 243 thinggy.forward(0.2);
maghayes 7:dd79b0942620 244 wait(0.15);
maghayes 7:dd79b0942620 245 }
maghayes 7:dd79b0942620 246
bayagich 10:e2267771f7d4 247 //EFFECTS: turns the robot right
maghayes 7:dd79b0942620 248 void turn_right(){
maghayes 7:dd79b0942620 249 int n=0;
bayagich 8:d5a5d104996e 250 wait(.08);
akul 9:accfae3aaf72 251 while (n<700){
bayagich 8:d5a5d104996e 252 thinggy.left_motor(0.2);
bayagich 8:d5a5d104996e 253 thinggy.right_motor(-0.2);
maghayes 7:dd79b0942620 254 n++;
maghayes 7:dd79b0942620 255 }
maghayes 7:dd79b0942620 256 thinggy.forward(0.2);
bayagich 8:d5a5d104996e 257 wait(0.15);
bayagich 4:057e904b1395 258 }