Amanda Bayagich
/
CrossandMapping
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Fork of
WorkingwithIR
by 
der Roboter
Revision 4:057e904b1395, committed 2014-06-03
- Comitter:
- bayagich
- Date:
- Tue Jun 03 08:03:58 2014 +0000
- Parent:
- 3:bac13ce5f5d0
- Child:
- 5:cee0a2fc8816
- Commit message:
- Spinning at turns ;
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Mon May 26 09:34:16 2014 +0000
+++ b/main.cpp Tue Jun 03 08:03:58 2014 +0000
@@ -2,6 +2,7 @@
#include "m3pi_ng.h"
#include "cmath"
#include "iostream"
+#include <string>
//Access infared sensors
DigitalIn m3pi_IN[] = {(p12)};
@@ -10,19 +11,34 @@
using namespace std;
bool cross_detection(int sensor[5], int black_thresh, int white_thresh);
+void mapping(m3pi thinggy, string directions, float speed, int turns);
+string directions(int startpoint, int endpoint);
+int end();
+int start();
+
m3pi thinggy;
-
-int main() {
-
+
+ int black_thresh = 300;
+ int white_thresh = 240;
float speed = 0.25;
float turn_speed = 0.2;
float correction;
float k = -0.3;
int sensor[5];
- int returned;
- int black_thresh = 300;
- int white_thresh = 240;
+ int returned;
+
+
+int main() {
+
+ //float speed = 0.25;
+ //float turn_speed = 0.2;
+ //float correction;
+ //float k = -0.3;
+ //int sensor[5];
+ //int returned;
+ //int black_thresh = 300;
+ //int white_thresh = 240;
bool cross = 0;
thinggy.locate(0,1);
thinggy.printf("Villan");
@@ -37,52 +53,69 @@
//find the average of the three sensors
returned = (sensor[1] + sensor[2] + sensor[3])/3;
+
+ //finds the directions of the robot
+ int startpt = 2; //start();
+ int endpt = 3; //end();
+ string d = directions(startpt, endpt);
+ int turns = 0;
while(1) {
- //check if it needs to turn
+ //check if it needs to turn
while(returned <= 240){
//turns right
while(sensor[0] < sensor[4] && thinggy.line_position() != 0){
- thinggy.left_motor(turn_speed);
- thinggy.right_motor(-turn_speed);
- thinggy.calibrated_sensor(sensor);
cross = cross_detection(sensor, black_thresh, white_thresh);
if(cross){
- //ask for command (left, right or forward)
+ mapping(thinggy, d, speed, turns);
+ ++turns;
}
+ else {
+ thinggy.left_motor(turn_speed);
+ thinggy.right_motor(-turn_speed);
+ }
+
+ thinggy.calibrated_sensor(sensor);
}
//turns left
while(sensor[4] > sensor[0] && thinggy.line_position() != 0){
- thinggy.left_motor(-turn_speed);
- thinggy.right_motor(turn_speed);
- thinggy.calibrated_sensor(sensor);
cross = cross_detection(sensor, black_thresh, white_thresh);
if(cross){
- //ask for command (left, right or forward)
+ mapping(thinggy, d, speed, turns);
+ ++turns;
}
+ else {
+ thinggy.left_motor(-turn_speed);
+ thinggy.right_motor(turn_speed);
+ }
+
+ thinggy.calibrated_sensor(sensor);
+
}
thinggy.calibrated_sensor(sensor);
returned = (sensor[1] + sensor[2]*2 + sensor[3])/4;
+
}//while returned <= 220
// Curves and straightaways
while(returned > 240){
-
- cross_detection(sensor, black_thresh, white_thresh);
-
float position = thinggy.line_position();
correction = k*(position);
-
+ cross = cross_detection(sensor, black_thresh, white_thresh);
+ if(cross){
+ mapping(thinggy, d, speed, turns);
+ ++turns;
+ }
// -1.0 is far left, 1.0 is far right, 0.0 in the middle
//speed limiting for right motor
- if(speed + correction > 1){
+ else if(speed + correction > 1){
thinggy.right_motor(0.6);
thinggy.left_motor(speed-correction);
}
//speed limiting for left motor
- if(speed - correction > 1){
+ else if(speed - correction > 1){
thinggy.left_motor(0.6);
thinggy.right_motor(speed+correction);
}
@@ -94,7 +127,7 @@
m3pi_IN[0].mode(PullUp);
while (m3pi_IN[0]==0){
thinggy.stop();
- }
+ }
}
thinggy.calibrated_sensor(sensor);
returned = (sensor[1] + sensor[2]*2 + sensor[3])/4;
@@ -106,36 +139,115 @@
}
-
+
+//DONE
//REQUIRES: array of 5 ints
//EFFECTS: stops the robot if it comes to any interesection where a decision has to be made
// and returns true if there is a cross
bool cross_detection(int sensor[5], int black_thresh, int white_thresh){
//three directions to choose from NOT WORKING
if(sensor[0] > black_thresh and sensor[2] > black_thresh and sensor[4] > black_thresh){
- thinggy.stop();
- wait(300);
return 1;
}
//left or forward
else if(sensor[0] > black_thresh and sensor[2] > black_thresh){
- thinggy.stop();
- wait(300);
return 1;
}
//left or right WORKING
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 ){
- thinggy.stop();
- wait(300);
return 1;
}
//forward or right
else if (sensor[2] > black_thresh and sensor[4] > black_thresh){
- thinggy.stop();
- wait(300);
return 1;
}
return 0;
}
-
\ No newline at end of file
+
+//take in the starting point of the robot from bluetooth
+int start(){
+ return 0;
+}
+
+//take in the ending point of the robot from bluetooth
+int end(){
+ return 0;
+}
+
+//DONE
+//gives the string to use for the map
+string directions(int startpoint, int endpoint){
+ string charmap[6][6] = {
+ {"", "RLLR", "RLFF", "RRRLLF", "RRLFLRRRL", "RRLR"},
+ {"LRRL", "", "LLF", "LFRLLF", "LFRLRRL", "LRFLR"},
+ {"FFRL", "FRR", "", "LL", "FLRLRRL", "FFFLR"},
+ {"FRRLRL", "RLRR", "RR", "", "FFRL", "FRLRL"},
+ {"RLLLRFRLL", "RLLRLFR", "RLFLR", "RLFF", "", "RLLLRL"},
+ {"LRLL", "LFLR", "LRFFF", "RLRLF", "RLRRRL", ""}
+ };
+ return charmap[startpoint - 1][endpoint - 1];
+}
+
+//DONE
+//takes in the string of directions and the number of turns
+//completed and then tells it whether to do left, right, forward
+void mapping(m3pi thinggy, string directions, float speed, int turns){
+
+ char x = directions[turns]; //something in the string
+
+ //tells it which direction to go
+ if(x == NULL){
+ thinggy.stop();
+ wait(300);
+ }
+ else if(x == 'L'){
+ thinggy.left(speed);
+ thinggy.calibrated_sensor(sensor);
+ returned = (sensor[1] + sensor[2]*2 + sensor[3])/4;
+ while(returned <= 240){
+ //turns right
+ while(sensor[0] < sensor[4] && thinggy.line_position() != 0){
+ thinggy.left_motor(turn_speed);
+ thinggy.right_motor(-turn_speed);
+ thinggy.calibrated_sensor(sensor);
+ }
+ //turns left
+ while(sensor[4] > sensor[0] && thinggy.line_position() != 0){
+ thinggy.left_motor(-turn_speed);
+ thinggy.right_motor(turn_speed);
+ thinggy.calibrated_sensor(sensor);
+
+ }
+ thinggy.calibrated_sensor(sensor);
+ returned = (sensor[1] + sensor[2]*2 + sensor[3])/4;
+
+ }//while returned <= 220
+ }
+ else if(x == 'R'){
+ thinggy.calibrated_sensor(sensor);
+ returned = (sensor[1] + sensor[2]*2 + sensor[3])/4;
+ while(returned <= 240){
+ //turns right
+ while(sensor[0] < sensor[4] && thinggy.line_position() != 0){
+ thinggy.left_motor(turn_speed);
+ thinggy.right_motor(-turn_speed);
+ thinggy.calibrated_sensor(sensor);
+ }
+ //turns left
+ while(sensor[4] > sensor[0] && thinggy.line_position() != 0){
+ thinggy.left_motor(-turn_speed);
+ thinggy.right_motor(turn_speed);
+ thinggy.calibrated_sensor(sensor);
+
+ }
+ thinggy.calibrated_sensor(sensor);
+ returned = (sensor[1] + sensor[2]*2 + sensor[3])/4;
+
+ }//while returned <= 220
+ thinggy.right(speed);
+ }
+ else if(x == 'F'){
+ //do nothing
+ }
+}
\ No newline at end of file