Diff: main.cpp

Revision:

0:8adde5eaea5a

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Nov 08 20:30:42 2017 +0000
@@ -0,0 +1,30 @@
+#include "mbed.h"
+#include "actions.h"
+        
+int main() {
+    pid lman, rman;
+    lman.kp = .5;
+    lman.ki = .1;
+    lman.kd = .4;
+    rman.kp = .5;
+    rman.ki = .1;
+    rman.kd = .4;
+    //INITIALIZATION
+    while(1) {
+       float left_encoder = lfront.read();
+       float right_encoder = rfront.read();
+       float bl = lback.read();
+       float br = rback.read();
+        printf("Left Encoder: %.2f %%\n", lfront.read());
+        printf("Right Encoder: %.2f %%\n", rfront.read());
+        printf("BL Encoder: %.2f %%\n", lback.read());
+        printf("BR Encoder: %.2f %%\n", rback.read());
+        
+        float error = left_encoder - right_encoder; //Can be pos or neg
+        if(error == 0){
+            resetpid(lman);
+            resetpid(rman);
+        }
+        float adjust_r = getFix(rman, error); //Only right wheel adjusted for this example, we can change this later / make it more complicated
+    }
+}