Diff: main.cpp

Revision:

8:22e399fe87a4

Parent:

7:7cdb0381e1b8

Child:

9:97941581fe81

--- a/main.cpp	Tue Nov 28 19:58:02 2017 +0000
+++ b/main.cpp	Tue Nov 28 21:45:07 2017 +0000
@@ -1,166 +1,10 @@
 #include "mbed.h"
 #include "QEI.h"
 #include "header.h"
-
-inline void pulse_ir(int in){
-    for(int i = 0; i < 4; i++){
-        LeftIR = in;
-        
-        FrontLeftIR = in;
-        FrontRightIR = in;
-        RightIR = in;
-    }
-}
-
-
-            
-
+#include "algorithm.h"
 int main() {
-
-float leftValue;
-float rightValue;
-float leftFrontValue;
-float rightFrontValue;
-float leftError = 0;
-float rightError = 0;
-float prevrightError = 0;
-float prevleftError = 0;
-float prevLeftFrontValue= 0;
-float prevRightFrontValue = 0;
-float changeLeftFrontValue = 0;
-float changeRightFrontValue = 0;
-float totalFrontChange = 0;
-float leftFrontError = 0;
-float rightFrontError = 0;
-float totalLeftError = 0;
-float totalRightError =0;
-float adjust_l = 0;
-float adjust_r = 0;
-
-float leftIRBase;
-float rightIRBase;
-float leftFrontIRBase;
-float rightFrontIRBase;
-float p =0.2f;    //.32
-float i =0.0001f;
-float d=0.15f; 
-float frontP = 1.0;
-    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;
-    lman.kd = .0f;
-    rman.kp = .5f;
-    rman.ki = .1f;
-    rman.kd = .4f;
-    
-    lpwmf.period(0.01f);
-    lpwmb.period(0.01f);
-    lpwmf = 0; //Previously started on, replace with lpwmf = lbase to make it start on (not a good idea)
-    rpwmb=0;
-    rpwmf.period(0.01f);
-    rpwmb.period(0.01f);
-    rpwmb=0;
-    rpwmf = 0;
-    
-    pid ir_lman, ir_rman;
-    ir_lman.kp = .0001f;
-    ir_lman.ki = .0f;
-    ir_lman.kd = .0f;
-    ir_rman.kp = .5f;
-    ir_rman.ki = .1f;
-    ir_rman.kd = .4f;    
-    t_time.start();
-    while(1){       
-       float dt = t_time.read();
-       float lspeed = 0; float rspeed = 0;
-       prevRightFrontValue = rightFrontValue;
-       prevLeftFrontValue = leftFrontValue;
-            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);
-        if(leftValue> 0.34f && leftFrontValue > 0.15f)
-        {
-            turn_right();
-        }
-        else if(rightFrontValue > 0.15f && rightValue > 0.35f)
-        {
-            turn_left();
-        }
-            
-            
-            
-       //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", rightFrontValue);
-       //pc.printf(" \n");
-       t_time.reset();
-    }
+    initmotors_and_pid(); //Sets the PWM frequency
+    algorithm();
 }