James Allen
working modified m3pimazesolver
Diff: main.cpp
- Revision:
- 0:05dee1d3d857
- Child:
- 1:f4db9b0bb5e2
diff -r 000000000000 -r 05dee1d3d857 main.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Thu Apr 04 03:44:59 2013 +0000
@@ -0,0 +1,362 @@
+#include "mbed.h"
+#include "m3pimaze.h"
+
+BusOut leds(LED1,LED2,LED3,LED4);
+m3pi m3pi(p23,p9,p10);
+char path[1000] = "";
+unsigned char path_length = 0; // the length of the path so far
+int sensors[5];
+
+// Minimum and maximum motor speeds
+#define MAX 0.25
+#define MIN 0
+
+// PID terms
+#define P_TERM 1
+#define I_TERM 0
+#define D_TERM 20
+
+/*=================================================================
+ FOLLOW LINE
+===================================================================*/
+void follow_line()
+ {
+
+ m3pi.locate(0,1);
+ m3pi.cls();
+ m3pi.printf("GO!!");
+
+ //wait(2.0);
+
+ //m3pi.sensor_auto_calibrate();//don't want this here.
+
+ float right;
+ float left;
+ float current_pos_of_line = 0.0;
+ float previous_pos_of_line = 0.0;
+ float derivative,proportional,integral = 0;
+ float power;
+ 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)
+ {
+
+ // Get the position of the line.
+ 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;
+
+ // 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
+ 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
+ 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))
+ {
+ // Found an intersection.
+ foundjunction=1;
+ return;
+ }
+ 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
+// three directions, applying the "left hand on the wall" strategy.
+
+char turn(unsigned char found_left, unsigned char found_forward, unsigned char found_right)
+{
+ // The order of the statements in this "if" is sufficient to implement a follow left-hand wall algorithm
+ if(found_left)
+ return 'L';
+ else if(found_forward)
+ return 'F';
+ else if(found_right)
+ return 'R';
+ 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;
+}
+/*=============================================================================================
+ SIMPLIFY
+===============================================================================================*/
+// change LBL to S (etc), to bypass dead ends
+void simplify()
+{
+ // only simplify the path if the second-to-last turn was a 'B'
+ if((path_length < 3) || (path[path_length-2] != 'B'))
+ return;
+
+
+ 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;
+ }
+ }
+
+ // Get the angle as a number between 0 and 360 degrees.
+ 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;
+ }
+
+ // The path is now two steps shorter.
+ path_length -= 2;
+}
+
+/*========================================================================================================
+ MAZESOLVE
+==========================================================================================================*/
+// This function is called once, from main.c.
+void mazesolve()
+{
+ // 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;
+ int sensors[5];
+ // Loop until we have solved the maze.
+ 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))
+ {
+ 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))
+ {
+ //move upon the home pad to show off
+ m3pi.forward(0.2);
+ wait(0.4);
+ 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);
+ 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
+ m3pi.cls();
+ if (found_left==1)
+ m3pi.printf("L");
+ if (found_right==1)
+ m3pi.printf("R");
+ if (found_forward==1)
+ m3pi.printf("F");
+ if (found_back==1)
+ m3pi.printf("B");
+ //wait (3);
+
+
+ unsigned char dir = turn(found_left, found_forward, found_right);
+
+ // Make the turn indicated by the path.
+ //doturn(dir);
+ doturn(dir);
+ // Store the intersection in the path variable.
+ path[path_length] = dir;
+ path_length ++;
+
+ // Need to insert check to make sure that the path_length does not
+ // exceed the bounds of the array.
+
+ // Simplify the learned path.
+ simplify();
+
+ }
+
+ // 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);
+ m3pi.printf("Finished");
+
+ // wait 15s to give time to turn off, or put the robot back to the start
+ wait(15);
+ // ideally we would use a button press here
+ // but I don't think it can easily be read
+
+ // 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);
+
+ // 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);
+
+
+ // Now we should be at the finish! Restart the loop.
+ }
+}
+
+/*========================================================================================================
+ MAIN
+=========================================================================================================*/
+int main() {
+ // int sensors[5];
+ m3pi.locate(0,1);
+ m3pi.sensor_auto_calibrate();
+ m3pi.printf("MazeSolve");
+
+ wait(2.0);
+
+ mazesolve();
+
+m3pi.forward(0.0);
+
+}