darron nielsen / Mbed 2 deprecated m3pi_LineFollower_dpn

Dependencies:   mbed

Files at this revision

API Documentation at this revision

Revision 0:301acbabd57a, committed 2010-11-11

Comitter:
microsat
Date:
Thu Nov 11 05:45:44 2010 +0000
Child:
1:e046cce6ceb8
Commit message:
need to work more on the terms. right now it more a PD controller than a PID

Changed in this revision

m3pi.lib Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
mbed.bld Show annotated file Show diff for this revision Revisions of this file 
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/m3pi.lib	Thu Nov 11 05:45:44 2010 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/chris/code/m3pi/#62ee1486ecb9

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Nov 11 05:45:44 2010 +0000
@@ -0,0 +1,62 @@
+#include "mbed.h"
+#include "m3pi.h"
+
+BusOut leds(LED1,LED2,LED3,LED4);
+m3pi m3pi(p23,p9,p10);
+
+#define MAX 1.0
+#define MIN 0
+#define P_TERM 1
+#define I_TERM 0
+#define D_TERM 20
+
+int main() {
+
+    m3pi.locate(0,1);
+    m3pi.printf("Line Flw");
+
+    wait(2.0);
+
+    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;
+    m3pi.sensor_auto_calibrate();
+    float speed = MAX;
+	
+    while (1) {
+
+        // 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;
+
+        power = (proportional * (P_TERM) ) + (integral*(I_TERM)) + (derivative*(D_TERM)) ;
+		// Remember the last position.
+        prev_pos_of_line = position_of_line;        
+        right = speed+power;
+        left  = speed-power;
+        
+        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);
+
+
+    }
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Thu Nov 11 05:45:44 2010 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/e2ac27c8e93e