Diff: main.cpp

Revision:

9:97941581fe81

Parent:

8:22e399fe87a4

--- a/main.cpp	Tue Nov 28 21:45:07 2017 +0000
+++ b/main.cpp	Fri Dec 08 05:14:27 2017 +0000
@@ -2,10 +2,150 @@
 #include "QEI.h"
 #include "header.h"
 #include "algorithm.h"
+/*
 int main() {
+    using namespace std;
+    bool pushed = true;
+    PushButton.mode(PullDown);
+    t_time.start();
     initmotors_and_pid(); //Sets the PWM frequency
-    algorithm();
+    while(pushed){
+        if(PushButton.read() == 0){ //If PushButton is pressed down
+            pushed = false;
+        }
+        wait(0.1);
+    }
+    while(!pushed){
+        RightEncoder.reset(); 
+        LeftEncoder.reset(); 
+        //usePIDEncoder();
+        //algorithm();
+        RightEncoder.reset(); 
+        LeftEncoder.reset(); 
+    }
 }
-
+*/
+int main(){
+    using namespace std;
+    bool pushed = true;
+    PushButton.mode(PullDown);
+    t_time.start();
+    initmotors_and_pid(); //Sets the PWM frequency
+    while(pushed){
+        if(PushButton.read() == 0){ //If PushButton is pressed down
+            pushed = false;
+        }
+        wait(0.1);
+    }
+    while(!pushed){   
+           using namespace constants;
+           float dt = t_time.read();
+           float lspeed = 0; float rspeed = 0;
+           prevRightFrontValue = rightFrontValue; //update previous values for change in IRfront
+           prevLeftFrontValue = leftFrontValue;
+                //IR SAMPLING
+                LeftIR = 1;
+                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;
+               //pc.printf( "%f \n" , leftValue);
+               //pc.printf("%f \n", rightValue);
+          //TURNING CONDITIONS
+            if(leftValue> 0.34f && leftFrontValue > 0.17f)    //.34L .15LF   Wall on left and in front
+            {
+                brake();
+                wait(0.2);
+                turn_right(1);
+               // pc.printf("I turned right\n");
+            }
+            else if(rightValue> 0.55f && rightFrontValue > 0.14f)  //.35R .15RF Wall on right and in front
+            {
+                brake();
+                wait(0.2);
+                turn_left(1);
+                //pc.printf("I turned left\n");
+            }
+            else if((leftValue < 0.34 && leftFrontValue < 0.0195f))    // No wall on left and in front
+            {   
+                LeftEncoder.reset();
+                RightEncoder.reset();
+                //brake();
+                //wait(0.4);
+                usePIDEncoder();
+                //pc.printf("Use encoder PID for left side\n");
+            }
+            else if((rightValue < 0.34f && rightFrontValue < 0.0205f))  // No wall on right and in front
+            {    
+                LeftEncoder.reset();
+                RightEncoder.reset();
+                //brake();
+                //wait(0.4);
+                usePIDEncoder();
+                //pc.printf("Use encoder PID for right side\n");
+            }  
+            else if(leftValue < 0.35f && leftFrontValue < 0.167f && rightValue < 0.35f && rightFrontValue < 0.167f)
+            {
+                LeftEncoder.reset();
+                RightEncoder.reset();
+                //brake();
+                //wait(0.4);
+                usePIDEncoder();
+                //pc.printf("Use encoder PID for left and right sides");
+            }
+           //ProcessIR(dt, ir_lman, lspeed, rspeed);
+           leftError = leftIRBase - leftValue;
+           rightError = rightIRBase - rightValue;
+           totalRightError += rightError;
+           totalLeftError += leftError;
+           leftFrontError = leftFrontIRBase - leftFrontValue;
+           rightFrontError = rightFrontIRBase - rightFrontValue;
+           changeLeftFrontValue = leftFrontValue-prevLeftFrontValue;
+           changeRightFrontValue = rightFrontValue - prevRightFrontValue;
+           
+           if(changeLeftFrontValue <0){
+               changeLeftFrontValue = 0;
+               }
+               
+            if(changeRightFrontValue <0){
+               changeRightFrontValue = 0;
+               }
+               totalFrontChange = changeRightFrontValue + changeLeftFrontValue;
+               
+           
+           adjust_l = p*leftError-d*prevleftError-i*totalLeftError ;
+           adjust_r = p*rightError-d*prevrightError-i*totalRightError ;
+           
+           prevleftError = leftError;
+           prevrightError = rightError;
+          // if(adjust_l >0)
+           {
+            //   adjust_l=0;
+           }
+          // if(adjust_r >0)
+           {
+            //   adjust_r=0;
+           }
+        lspeed = constrain(0.05f, 1.0f, lbase-adjust_l - frontP*totalFrontChange);
+        rspeed = constrain(0.05f, 1.0f, rbase-adjust_r - frontP*totalFrontChange);
+            lpwmb = 0; rpwmb = 0;
+          lpwmf = lspeed; rpwmf = rspeed;
+           //pc.printf("left %f \n", leftValue);
+           //pc.printf("right %f \n\n", rightValue);
+           //pc.printf("front left%f \n", leftFrontValue);
+           //pc.printf("front right%f \n\n", rightFrontValue);
+           //pc.printf(" \n");
+           t_time.reset();
+        }
+    }
+    
 
-