Jon Marsh
/
m3pi_MazeSolverLVC
Code to be modified by LVC students
main.cpp
- Committer:
- jonmarsh
- Date:
- 2011-03-10
- Revision:
- 0:64bbd41c50cb
- Child:
- 1:092dad68a533
File content as of revision 0:64bbd41c50cb:
#include "mbed.h" #include "m3pimaze.h" BusOut leds(LED1,LED2,LED3,LED4); m3pi m3pi(p23,p9,p10); #define MAX 0.5 #define MIN 0 #define P_TERM 1 #define I_TERM 0 #define D_TERM 20 // Global variables // The path array stores the route info. Each element shows what we did at an intersection // 'L' for left // 'R' for right // 'F' for forward // 'B' for back // char path[1000] = ""; unsigned char path_length = 0; // the length of the path so far void follow_line() { float right; float left; float position_of_line = 0.0; float prev_pos_of_line = 0.0; float derivative,proportional; float integral = 0; float power; float speed = MAX; int foundjunction=0; int sensors[5]; while (foundjunction==0) { // Get the position of the line. position_of_line = m3pi.line_position(); proportional = position_of_line; // Compute the derivative derivative = position_of_line - prev_pos_of_line; // Compute the integral integral += proportional; // Remember the last position. prev_pos_of_line = position_of_line; // Compute 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); m3pi.readsensor(sensors); if(sensors[1] < 100 && sensors[2] < 100 && sensors[3] < 100) { // There is no line visible ahead, and we didn't see any // intersection. Must be a dead end. foundjunction=1; } // you need to put some code here to look for a left or right junction } } // 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; int sensors[5]; // Follow the line until an intersection is detected follow_line(); m3pi.forward(0.0); // Check for a forward exit. m3pi.readsensor(sensors); if(sensors[1] > 200 || sensors[2] > 200 || sensors[3] > 200) found_forward = 1; // Check for left and right exits. //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"); wait (3); // Check for the ending spot. // If all five sensors are on dark black, we have // solved the maze. if(sensors[0]>600 && sensors[1] > 600 && sensors[2] > 600 && sensors[3] > 600 && sensors[4]>600) m3pi.printf("Done"); } void checksensors() { int sensors[5]; while (1) { m3pi.readsensor(sensors); m3pi.cls(); if (sensors[0]>200) m3pi.printf("D"); else m3pi.printf("L"); if (sensors[1]>200) m3pi.printf("D"); else m3pi.printf("L"); if (sensors[2]>200) m3pi.printf("D"); else m3pi.printf("L"); if (sensors[3]>200) m3pi.printf("D"); else m3pi.printf("L"); if (sensors[4]>200) m3pi.printf("D"); else m3pi.printf("L"); } } int main() { // int sensors[5]; m3pi.locate(0,1); m3pi.sensor_auto_calibrate(); m3pi.printf("MazeSolve"); wait(2.0); mazesolve(); m3pi.forward(0.0); }