James Allen
working modified m3pimazesolver
Diff: main.cpp
- Revision:
- 2:73f7cc4c30c7
- Parent:
- 1:f4db9b0bb5e2
diff -r f4db9b0bb5e2 -r 73f7cc4c30c7 main.cpp
--- a/main.cpp Fri Apr 05 18:46:50 2013 +0000
+++ b/main.cpp Sat Apr 06 15:22:01 2013 +0000
@@ -17,11 +17,11 @@
#define I_TERM 0
#define D_TERM 20
-/*=================================================================
+/*==========================================================================================================================
FOLLOW LINE
-===================================================================*/
+============================================================================================================================*/
void follow_line()
- {
+{
m3pi.locate(0,1);
m3pi.cls();
@@ -40,68 +40,66 @@
float speed = MAX;
int foundjunction=0;
int countdown=100; //make sure we don't stop for a junction too soon after starting
- while (foundjunction==0)
- {
+ while (foundjunction==0) {
// Get the position of the line.
- current_pos_of_line = m3pi.line_position();
+ current_pos_of_line = m3pi.line_position();
proportional = current_pos_of_line;
-
+
// Compute the derivative
derivative = current_pos_of_line - previous_pos_of_line;
-
+
// Compute the integral
- // integral += proportional;
-
+ // integral += proportional;
+
// Remember the last position.
previous_pos_of_line = current_pos_of_line;
-
+
// Compute the power
power = (proportional * (P_TERM) ) + (integral*(I_TERM)) + (derivative*(D_TERM)) ;
-
- // Compute new speeds
+
+ // Compute new speeds
right = speed+power;
left = speed-power;
-
+
// limit checks
if (right < MIN)
right = MIN;
else if (right > MAX)
right = MAX;
-
+
if (left < MIN)
left = MIN;
else if (left > MAX)
left = MAX;
-
- // set speed
+
+ // set speed
m3pi.left_motor(left);
m3pi.right_motor(right);
-
- if (countdown>0) countdown--; else {
- // Next, we are going to use the sensors to look for whether there is still a line ahead
- // and try to detect dead ends and possible left or right turns.
- m3pi.readsensor(sensors);
-
- if((sensors[1] < 400) && (sensors[2] < 400) && (sensors[3] < 400))
- {
- // There is no line visible ahead, and we didn't see any
- // intersection. Must be a dead end.
- foundjunction=1;
- return;
- }
- else if((sensors[0] > 700) || (sensors[4] > 700))
- {
+
+ if (countdown>0) countdown--;
+ else {
+ // Next, we are going to use the sensors to look for whether there is still a line ahead
+ // and try to detect dead ends and possible left or right turns.
+ m3pi.readsensor(sensors);
+
+ if((sensors[1] < 400) && (sensors[2] < 400) && (sensors[3] < 400)) {
+ // There is no line visible ahead, and we didn't see any
+ // intersection. Must be a dead end.
+ foundjunction=1;
+ return;
+ } else if((sensors[0] > 700) || (sensors[4] > 700)) {
// Found an intersection.
foundjunction=1;
return;
- }
- else foundjunction=0;
-
-}}}
-/*===========================================================================================================
+ } else foundjunction=0;
+
+ }
+ }
+}
+/*=========================================================================================================================
TURN
-=============================================================================================================*/
+===========================================================================================================================*/
// This function decides which way to turn during the learning phase of
// maze solving. It uses the variables found_left, found_straight, and
// found_right, which indicate whether there is an exit in each of the
@@ -119,27 +117,34 @@
else
return 'B';
}
-/*============================================================================================================
+/*==========================================================================================================================
DO TURN
-==============================================================================================================*/
+============================================================================================================================*/
void doturn(unsigned char dir)
{
- if (dir=='L')
- {m3pi.left(0.25);wait(0.28);}
- else if(dir=='R')
- {m3pi.right(0.25);wait(0.28);}
- //else if(dir=='F')
- //{m3pi.forward(0.3);wait(0.15);}
- else if(dir=='B')
- {m3pi.right(0.25);wait(0.6);}
-
- m3pi.forward(0.1);wait(0.1);m3pi.forward(0);
- return;
+ if (dir=='L') {
+ m3pi.left(0.25);
+ wait(0.28);
+ } else if(dir=='R') {
+ m3pi.right(0.25);
+ wait(0.28);
+ }
+ //else if(dir=='F')
+ //{m3pi.forward(0.3);wait(0.15);}
+ else if(dir=='B') {
+ m3pi.right(0.25);
+ wait(0.6);
+ }
+
+ m3pi.forward(0.1);
+ wait(0.1);
+ m3pi.forward(0);
+ return;
}
-/*=============================================================================================
+/*==========================================================================================================================
SIMPLIFY
-===============================================================================================*/
+============================================================================================================================*/
// change LBL to S (etc), to bypass dead ends
void simplify()
{
@@ -150,19 +155,17 @@
int total_angle = 0;
int i;
- for(i=1;i<=3;i++)
- {
- switch(path[path_length-i])
- {
- case 'R':
- total_angle += 90;
- break;
- case 'L':
- total_angle += 270;
- break;
- case 'B':
- total_angle += 180;
- break;
+ for(i=1; i<=3; i++) {
+ switch(path[path_length-i]) {
+ case 'R':
+ total_angle += 90;
+ break;
+ case 'L':
+ total_angle += 270;
+ break;
+ case 'B':
+ total_angle += 180;
+ break;
}
}
@@ -170,161 +173,158 @@
total_angle = total_angle % 360;
// Replace all of those turns with a single one.
- switch(total_angle)
- {
- case 0:
- path[path_length - 3] = 'F';
- break;
- case 90:
- path[path_length - 3] = 'R';
- break;
- case 180:
- path[path_length - 3] = 'B';
- break;
- case 270:
- path[path_length - 3] = 'L';
- break;
+ switch(total_angle) {
+ case 0:
+ path[path_length - 3] = 'F';
+ break;
+ case 90:
+ path[path_length - 3] = 'R';
+ break;
+ case 180:
+ path[path_length - 3] = 'B';
+ break;
+ case 270:
+ path[path_length - 3] = 'L';
+ break;
}
// The path is now two steps shorter.
path_length -= 2;
}
-/*==============================================================================================================
+/*==========================================================================================================================
PRESSED
-================================================================================================================*/
+============================================================================================================================*/
void pressed()
{
-
- // wait 15s to give time to turn off, or put the robot back to the start
- wait(2);
- // ideally we would use a button press here
- // but I don't think it can easily be read
+
+ // wait 1s to give time to GET OUT OF WAY
+ wait(1);
+ //
- // Re-run the maze. It's not necessary to identify the
- // intersections, so this loop is really simple.
- int i;
- for(i=0;i<path_length;i++)
- {
- follow_line();
+ // Re-run the maze. It's not necessary to identify the
+ // intersections, so this loop is really simple.
+ int i;
+ for(i=0; i<path_length; i++) {
+ follow_line();
+
+ // Drive straight while slowing down
+ //m3pi.forward(0.5);
+ //wait(0.05);
+ m3pi.forward(0.2);
+ wait(0.2);
- // Drive straight while slowing down
- //m3pi.forward(0.5);
- //wait(0.05);
- m3pi.forward(0.2);
- wait(0.2);
+ // Make a turn according to the instruction stored in
+ // path[i].
+ doturn(path[i]);
+ }
- // Make a turn according to the instruction stored in
- // path[i].
- doturn(path[i]);
- }
-
- // Follow the last segment up to the finish.
- follow_line();
- m3pi.forward(0.2);
- wait(0.6);
- return;
+ // Follow the last segment up to the finish.
+ follow_line();
+ m3pi.forward(0.2);
+ wait(0.6);
- // Now we should be at the finish! Restart the loop.
-
+ //celebrate with music
+ char dixie[]={'V','1','5','O','5','G','1','6','E','1','6','C','8','R','2','4','C','8','R','2','4','C','1','6','D','1','6','E','1','6','F','1','6','G','8','R','2','4','G','8','R','2','4','G','8','E','1','6'};
+ int numb=49;
+ m3pi.playtune(dixie,numb);
+ return;
+
+ // Now we should be at the finish! Restart the loop.
+
}
-/*========================================================================================================
+/*=========================================================================================================================
MAZESOLVE
-==========================================================================================================*/
+===========================================================================================================================*/
// This function is called once, from main.c.
void mazesolve()
{
- // These variables record whether the robot has seen a line to the
+ // These variables record whether the robot has seen a line to the
// left, straight ahead, and right, while examining the current
// intersection.
- unsigned char found_left=0;
- unsigned char found_forward=0;
- unsigned char found_right=0;
- unsigned char found_back=0;
+ unsigned char found_left=0;
+ unsigned char found_forward=0;
+ unsigned char found_right=0;
+ unsigned char found_back=0;
int sensors[5];
// Loop until we have solved the maze.
- while(1)
- {
-
+ while(1) {
+
// Follow the line until an intersection is detected
follow_line();
// Bump forward a bit in case sensor was triggered at an angle
m3pi.forward(0.1);
- wait(0.1);
-
- found_left=0;found_forward=0;found_right=0;
-
- m3pi.readsensor(sensors);//sensors are turned off outside the follow line function so we must call sensors eachtime they are needed
-
- // Check for left and right exits.
- if((sensors[1]<700) && (sensors [2]<700) && (sensors[3]<700))
- {
+ wait(0.1);
+
+ found_left=0;
+ found_forward=0;
+ found_right=0;
+
+ m3pi.readsensor(sensors);/*sensors are turned off outside the follow line
+ function so we must call sensors eachtime they are needed*/
+
+ // Check for left and right exits.
+ if((sensors[1]<700) && (sensors [2]<700) && (sensors[3]<700)) {
found_back=1;
}
-
- if(sensors[0] > 700)
- {
- // Drive straight a bit more - this is enough to line up our
- // wheels with the intersection.
- m3pi.forward(0.2);
- wait(0.2);
- m3pi.readsensor(sensors);//what's beyond the intersection
- // Check for the ending spot.
- // If all five sensors are on dark black, we have
- // solved the maze.
- if((sensors[0]>900) && (sensors[1] > 900) && (sensors[2] > 900) && (sensors[3] > 900) && (sensors[4]>900))
- {
+
+ if(sensors[0] > 700) {
+ // Drive straight a bit more - this is enough to line up our
+ // wheels with the intersection.
+ m3pi.forward(0.2);
+ wait(0.2);
+ m3pi.readsensor(sensors);//what's beyond the intersection
+ // Check for the ending spot.
+ // If all five sensors are on dark black, we have
+ // solved the maze.
+ if((sensors[0]>900) && (sensors[1] > 900) && (sensors[2] > 900) && (sensors[3] > 900) && (sensors[4]>900)) {
//move upon the home pad to show off
- m3pi.forward(0.2);
- wait(0.4);
- m3pi.printf("Finished");
- break;
- }
- else found_left = 1;
-
- }
-
- else if(sensors[4] > 700 )
- {
+ m3pi.forward(0.2);
+ wait(0.4);
+ m3pi.printf("Press P21 Restart");
+ break;
+ } else found_left = 1;
+
+ }
+
+ else if(sensors[4] > 700 ) {
//move wheels to intersection
- m3pi.forward(0.2);
- wait(0.2);
- //what is past the intersection
- m3pi.readsensor(sensors);
- // Check for the ending spot.
- // If all five sensors are on dark black, we have
- // solved the maze.
- if((sensors[0]>900) && (sensors[1] > 900) && (sensors[2] > 900) && (sensors[3] > 900) && (sensors[4]>900))
- {
- m3pi.forward(0.2);
- wait(0.4);
- m3pi.printf("Finished");
- break;
- }
- //can we go forward
- else if((sensors[1] > 700 )|| (sensors[2] > 700) || (sensors[3] > 700))
- {
+ m3pi.forward(0.2);
+ wait(0.2);
+ //what is past the intersection
+ m3pi.readsensor(sensors);
+ // Check for the ending spot.
+ // If all five sensors are on dark black, we have
+ // solved the maze.
+ if((sensors[0]>900) && (sensors[1] > 900) && (sensors[2] > 900) && (sensors[3] > 900) && (sensors[4]>900)) {
+ m3pi.forward(0.2);
+ wait(0.4);
+ m3pi.printf("Press P21 Restart");
+ break;
+ }
+ //can we go forward
+ else if((sensors[1] > 700 )|| (sensors[2] > 700) || (sensors[3] > 700)) {
found_forward = 1;
-
- }
- //then go right
- else found_right=1;
- }
-
- //debug code
+
+ }
+ //then go right
+ else found_right=1;
+ }
+
+ //debug code
m3pi.cls();
if (found_left==1)
- m3pi.printf("L");
+ m3pi.printf("L");
if (found_right==1)
- m3pi.printf("R");
+ m3pi.printf("R");
if (found_forward==1)
- m3pi.printf("F");
+ m3pi.printf("F");
if (found_back==1)
- m3pi.printf("B");
+ m3pi.printf("B");
//wait (3);
-
+
unsigned char dir = turn(found_left, found_forward, found_right);
@@ -336,49 +336,47 @@
path_length ++;
// Need to insert check to make sure that the path_length does not
- // exceed the bounds of the array.
+ // exceed the bounds of the array.
// Simplify the learned path.
simplify();
-
+
}
-
- // Solved the maze!
+
+ // Solved the maze!
// Now enter an infinite loop - we can re-run the maze as many
// times as we want to.
- while(1)
- {
- m3pi.forward(0.0);
- pb.mode(PullUp);
-
- if(pb)
- {
- do
- {
-
- }
- while(pb);
+ while(1) {
+ m3pi.forward(0.0);
+ pb.mode(PullUp);
+ wait(1);
+
+ if(pb) {
+ do {
+
+ } while(pb);
pressed();
- }
-
+ }
+
}
-
+
}
-/*========================================================================================================
+/*=========================================================================================================================
MAIN
-=========================================================================================================*/
-int main() {
- // int sensors[5];
+===========================================================================================================================*/
+int main()
+{
+// int sensors[5];
m3pi.locate(0,1);
m3pi.sensor_auto_calibrate();
m3pi.printf("MazeSolve");
wait(2.0);
- mazesolve();
+ mazesolve();
-m3pi.forward(0.0);
+ m3pi.forward(0.0);
}