MEGACARAZ
/
Turning
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.
Dependencies: btbee m3pi_ng mbed
Diff: main.cpp
- Revision:
- 5:0c2da538ada2
- Parent:
- 4:a0baf6752548
- Child:
- 6:8bd154642fb4
--- a/main.cpp Wed Jun 03 13:37:01 2015 +0000
+++ b/main.cpp Thu Jun 04 07:59:52 2015 +0000
@@ -2,6 +2,8 @@
#include "m3pi_ng.h"
#include "time.h"
#include "btbee.h"
+#include <vector>
+#include <string>
m3pi m3pi;
btbee btbee;
@@ -32,6 +34,9 @@
Timer TurnTime;
Timer LapTime;
int counter = 0;
+ string path;
+ char dir;
+
void RightTurn() {
@@ -46,6 +51,9 @@
if( TurnTime.read() > 0.2375) {
+ LapTime.reset();
+ LapTime.start();
+
return;
}
@@ -68,6 +76,9 @@
if( TurnTime.read() > 0.2375) {
+ LapTime.reset();
+ LapTime.start();
+
return;
}
@@ -75,10 +86,127 @@
}
}
+
+void TurnAround() {
+
+ m3pi.stop();
+ TurnTime.reset();
+ TurnTime.start();
+
+ while (1) {
+
+ m3pi.left_motor(0.5);
+ m3pi.right_motor(-0.5);
+
+ if( TurnTime.read() > 0.225) {
+
+ m3pi.stop();
+
+ return;
+
+ }
+ }
+ }
+
+void GoStraight() {
+
+ TurnTime.reset();
+ TurnTime.start();
+
+ while(1) {
+
+ m3pi.right_motor(0.5);
+ m3pi.left_motor(0.5);
+
+ if(TurnTime.read() > 0.02) {
+
+ LapTime.reset();
+ LapTime.start();
+
+ return;
+
+ }
+ }
+ }
+
+
+bool CrossDetect() {
+
+ if(s1 > 500 and s2 > 500 and s3 > 500 and s4 > 500 and s5 > 500) {
+
+ return(1);
+
+ }
+
+ else if(s1 > 700 and s2 > 700) {
+
+ return(1);
+
+ }
+
+ else if(s5 > 700 and s4 > 700) {
+
+ return(1);
+
+ }
+
+ else {
+
+ return(0);
+
+ }
+
+ }
+
int main() {
+
+vector < string > place(6, "");
+vector < vector < string > > rga(6,place);
-btbee.reset();
+ rga.at(0).at(0) = "X";
+ rga.at(0).at(1) = "RLLRX";
+ rga.at(0).at(2) = "RLSSX";
+ rga.at(0).at(3) = "RLSRLX";
+ rga.at(0).at(4) = "RRLSLRRRLX";
+ rga.at(0).at(5) = "RRLRX";
+
+ rga.at(1).at(0) = "LRRLX";
+ rga.at(1).at(1) = "X";
+ rga.at(1).at(2) = "LLSX";
+ rga.at(1).at(3) = "LLRLX";
+ rga.at(1).at(4) = "LSRLRRLX";
+ rga.at(1).at(5) = "LRSLRX";
+
+ rga.at(2).at(0) = "SSRLX";
+ rga.at(2).at(1) = "SRRX";
+ rga.at(2).at(2) = "X";
+ rga.at(2).at(3) = "LLX";
+ rga.at(2).at(4) = "LRSRLX";
+ rga.at(2).at(5) = "SSSLRX";
+
+ rga.at(3).at(0) = "SRRLLRLX";
+ rga.at(3).at(1) = "RLRRX";
+ rga.at(3).at(2) = "RRX";
+ rga.at(3).at(3) = "X";
+ rga.at(3).at(4) = "SSRLX";
+ rga.at(3).at(5) = "SRLRLX";
+
+ rga.at(4).at(0) = "RLLLRSRLLX";
+ rga.at(4).at(1) = "RLLRLSRX";
+ rga.at(4).at(2) = "RLLRLRSX";
+ rga.at(4).at(3) = "RLSSX";
+ rga.at(4).at(4) = "X";
+ rga.at(4).at(5) = "RLLLRLX";
+
+ rga.at(5).at(0) = "LRLLX";
+ rga.at(5).at(1) = "LSLX";
+ rga.at(5).at(2) = "LRSSSX";
+ rga.at(5).at(3) = "RLRLSX";
+ rga.at(5).at(4) = "RLRRRLX";
+ rga.at(5).at(5) = "X";
+
+ btbee.reset();
m3pi_pb.mode(PullUp);
m3pi.printf("Wait 4");
@@ -129,9 +257,12 @@
LapTime.start();
+ path = rga.at(3).at(0);
+
while (1) {
+ LapTime.start();
// Get the position of the line.
current_pos_of_line = m3pi.line_position();
@@ -190,51 +321,55 @@
//
// }
-
-
- if(s1 > 500 and s2 > 500 and s3 > 500 and s4 > 500 and s5 > 500 and LapTime.read() > 0.1) {
-
- if( counter == 2) {
+
+ if(CrossDetect() and LapTime > 0.1) {
+
+ btbee.printf("\n");
+ dir = path.at(counter);
+ btbee.printf("%c\n", dir);
+
+ switch(dir) {
- m3pi.stop();
- exit(1);
+ case 'R': {
+
+ RightTurn();
+ btbee.printf("Turning right\n");
+ break;
+
+ }
+
+ case 'L': {
+
+ LeftTurn();
+ btbee.printf("Turning left\n");
+ break;
+
+ }
+
+ case 'S': {
+
+ GoStraight();
+ btbee.printf("Going straight\n");
+ break;
+
+ }
+
+ case 'X': {
+
+ btbee.printf("Reached goal. Turning around\n");
+ TurnAround();
+ exit(1);
+
+ }
+
+ }
+
+ counter++;
}
-
- m3pi.stop();
- btbee.printf("Detected full cross");
- LeftTurn();
- btbee.printf("\n");
- LapTime.reset();
- LapTime.start();
- counter++;
-
- }
-
- else if( s5 > 700 and s4 > 700 and LapTime.read() > 0.1) {
-
- m3pi.stop();
- btbee.printf("Detected right side half cross\n");
- RightTurn();
- btbee.printf("\n");
- LapTime.reset();
- LapTime.start();
-
- }
-
- else if( s1 > 700 and s2 > 700 and LapTime.read() > 0.1) {
-
- m3pi.stop();
- btbee.printf("Detected left side half cross\n");
- LeftTurn();
- btbee.printf("\n");
- LapTime.reset();
- LapTime.start();
-
- }
+
}
-}
-
\ No newline at end of file
+}
\ No newline at end of file