Diff: main.cpp

Revision:

0:e60be1842101

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Nov 30 15:17:45 2010 +0000
@@ -0,0 +1,91 @@
+#include "mbed.h"
+#include "m3pi.h"
+
+BusOut leds(LED1,LED2,LED3,LED4);
+m3pi m3pi(p23,p9,p10);
+
+#define MAX 1.0
+#define MIN 0.2
+
+
+#define P_TERM 0.8
+#define I_TERM 0
+#define D_TERM 20
+
+int main() {
+
+    m3pi.locate(0,1);
+    for (int countdown=3; countdown >=1; countdown--) {
+        m3pi.printf("%d", countdown);
+        wait(1);
+        m3pi.cls();
+
+    }
+    m3pi.cls();
+    wait(0.001);
+    m3pi.printf("GO!");
+    wait(1);
+    m3pi.cls();
+    m3pi.printf("VB: %f", m3pi.battery());
+
+
+    float right;
+    float left;
+    float position_of_line = 0.0;
+    float act_position_of_line = 0.0;
+    float prev_pos_of_line = 0.0;
+    float derivative,proportional;
+    float integral = 0;
+    float power;
+    m3pi.sensor_auto_calibrate();
+    float speed = MAX;
+
+    while (1) {
+        for (int i=0; i<1000; i++) {
+
+            // Get the position of the line.
+            act_position_of_line = m3pi.line_position();
+            position_of_line = act_position_of_line - 0.40;
+
+            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;
+
+            if ((position_of_line < 0.20) && (position_of_line > -0.20)) {
+                m3pi.left_motor(MAX);
+                m3pi.right_motor(MAX);
+
+            }
+
+            // set speed
+            m3pi.left_motor(left);
+            m3pi.right_motor(right);
+
+            //m3pi.cls();
+            //m3pi.printf("VB: %f", m3pi.battery());
+        }
+
+
+    }
+}
\ No newline at end of file