Diff: main_old.cpp

Revision:

1:18f0113ac5a2

diff -r bbbd4077bcc4 -r 18f0113ac5a2 main_old.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main_old.cpp	Mon Nov 20 01:16:14 2017 +0000
@@ -0,0 +1,52 @@
+/*
+#include "mbed.h"
+#include "QEI.h"
+#include "header.h"
+int main_old() {
+    pid lman, rman;
+    lman.kp = .004f;
+    lman.ki = .0f;
+    lman.kd = .0f;
+    rman.kp = .5f;
+    rman.ki = .1f;
+    rman.kd = .4f;
+    
+    lpwmf.period(0.01f);
+    lpwmf = 0; //Previously started on, replace with lpwmf = lbase to make it start on (not a good idea)
+    rpwmf.period(0.01f);
+    rpwmf = 0;
+    
+    
+    t_time.start();
+        
+    pid ir_lman, ir_rman;
+    ir_lman.kp = .004f;
+    ir_lman.ki = .0f;
+    ir_lman.kd = .0f;
+    ir_rman.kp = .5f;
+    ir_rman.ki = .1f;
+    ir_rman.kd = .4f;    
+    while(1){       
+       float dt = t_time.read();
+       t_time.reset();
+       float error = (LeftEncoder.getPulses() - RightEncoder.getPulses())/16.0f; //Can be pos or neg, Range from 0 - 255 for one revolution
+         if(error == 0){
+            resetpid(&lman);
+            resetpid(&rman);
+          }
+        float adjust_l = getFix(&lman, error, dt); 
+        LeftIR = 1;
+        RightIR = 1;
+        float leftread = LeftReceiver.read();
+        float rightread = RightReceiver.read();
+        float error_IR = (rightread - leftread);
+        if(error_IR < 0.02f){
+            resetpid(&ir_lman);
+            resetpid(&ir_rman);    
+        }
+        //float adjust_l = getFix(&ir_lman, error, dt);
+        lpwmf = constrain(0.0f, 0.3f, lbase + adjust_l);
+        rpwmf = rbase+.2f;
+    }
+}
+*/
\ No newline at end of file