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

Committer:
bayagich
Date:
Tue Jun 03 10:59:34 2014 +0000
Revision:
5:cee0a2fc8816
Parent:
4:057e904b1395
Child:
6:a6f752174eef
Not working at cross sections, only working at corners;

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