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

Committer:
bayagich
Date:
Tue Jun 03 12:48:53 2014 +0000
Revision:
8:d5a5d104996e
Parent:
7:dd79b0942620
Child:
9:accfae3aaf72
Still need to work on left and 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 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 8:d5a5d104996e 49 int endpt = 4; //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] = {
bayagich 4:057e904b1395 175 {"", "RLLR", "RLFF", "RRRLLF", "RRLFLRRRL", "RRLR"},
bayagich 8:d5a5d104996e 176 {"LRRL", "", "LLF", "LLRL", "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);
bayagich 8:d5a5d104996e 217 while (n<850){
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);
bayagich 8:d5a5d104996e 230 while (n<850){
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 }