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

Committer:
bayagich
Date:
Tue Jun 03 08:03:58 2014 +0000
Revision:
4:057e904b1395
Parent:
3:bac13ce5f5d0
Child:
5:cee0a2fc8816
Spinning at turns ;

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