Diff: main.cpp

Revision:

2:7da65637e512

Parent:

1:fb18b43590e6

Child:

3:4e34437daef4

--- a/main.cpp	Tue Nov 21 23:06:09 2017 +0000
+++ b/main.cpp	Wed Nov 22 01:40:27 2017 +0000
@@ -12,7 +12,44 @@
     }
 }
 
+
+            
+
 int main() {
+
+float leftValue;
+float rightValue;
+float leftFrontValue;
+float rightFrontValue;
+float leftError = 0;
+float rightError = 0;
+float leftFrontError = 0;
+float rightFrontError = 0;
+float adjust_l = 0;
+float adjust_r = 0;
+
+float leftIRBase;
+float rightIRBase;
+float leftFrontIRBase;
+float rightFrontIRBase;
+float p =0.45f;
+float i;
+float d; 
+    LeftIR = 1;
+            RightIR = 1;
+            leftIRBase = LeftReceiver;
+            rightIRBase = RightReceiver;
+            LeftIR = 0;
+            RightIR = 0;
+            wait_ms(10);
+            FrontLeftIR = 1;
+            leftFrontIRBase = FrontLeftReceiver;
+            FrontRightIR = 1;
+            rightFrontIRBase= FrontRightReceiver;
+            FrontLeftIR = 0;
+            FrontRightIR = 0;
+            wait_ms(10);
+            
     pid lman, rman;
     lman.kp = .009f;
     lman.ki = .0f;
@@ -42,22 +79,33 @@
        float dt = t_time.read();
        float lspeed = 0; float rspeed = 0;
             LeftIR = 1;
-            wait(.01f);
-            FrontLeftIR=1;
-            wait(.01f);
-            FrontRightIR=1;
-            wait(.01f);
-            RightIR=1;
-       ProcessIR(dt, ir_lman, lspeed, rspeed);
+            RightIR = 1;
+            wait(.01);
+            leftValue = LeftReceiver;
+            rightValue = RightReceiver;
+            LeftIR = 0;
+            RightIR = 0;
+            FrontLeftIR = 1;
+            leftFrontValue = FrontLeftReceiver;
+            FrontRightIR = 1;
+            rightFrontValue = FrontRightReceiver;
+            wait(.01);
+            FrontLeftIR = 0;
+            FrontRightIR = 0;
+            
+            
+       //ProcessIR(dt, ir_lman, lspeed, rspeed);
+       leftError = leftIRBase - leftValue;
+       rightError = rightIRBase - rightValue;
+       leftFrontError = leftFrontIRBase - leftFrontValue;
+       rightFrontError = rightFrontIRBase - rightFrontValue;
+       
+       adjust_l = p*leftError ;
+       adjust_r = p*rightError ;
+        lspeed = constrain(0.05f, 1.0f, lbase - adjust_l);
+    rspeed = constrain(0.05f, 1.0f, rbase-adjust_r);
        lpwmb = 0; rpwmb = 0;
        lpwmf = lspeed; rpwmf = rspeed;
-            LeftIR = 0;
-            wait(.01f);
-            FrontLeftIR=0;
-            wait(.01f);
-            FrontRightIR=0;
-            wait(.01f);
-            RightIR=0;
        t_time.reset();
     }
 }