Diff: main.cpp

Revision:

0:f562e4f9c29f

Child:

1:5431d59ee324

Child:

3:88a6763a7e44

diff -r 000000000000 -r f562e4f9c29f main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Oct 04 11:43:07 2022 +0000
@@ -0,0 +1,76 @@
+#include "mbed.h"
+#include "m3pi.h"
+
+m3pi m3pi;
+
+// Minimum and maximum motor speeds
+#define MAX 1.0
+#define MIN 0
+
+// PID terms
+#define P_TERM 1
+#define I_TERM 0
+#define D_TERM 20
+
+int main() {
+    float a=m3pi.battery();
+    float b=m3pi.pot_voltage();
+    m3pi.locate(0,0);
+    
+       
+    m3pi.cls();
+    m3pi.printf("%f.3 ",a);
+    m3pi.locate(0,1);
+    m3pi.printf("%f.3 ",b);
+
+    wait(2.0);
+
+    m3pi.sensor_auto_calibrate();
+
+    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;
+    
+    while (1) {
+
+        // 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);
+
+    }
+}
\ No newline at end of file