Diff: main.cpp

Revision:

0:64bbd41c50cb

Child:

1:092dad68a533

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Mar 10 12:17:35 2011 +0000
@@ -0,0 +1,167 @@
+#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);
+
+}