File content as of revision 6:ce498700a28c:

#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;
    }
}


            

int main() {

float leftValue;
float rightValue;
float leftFrontValue;
float rightFrontValue;
float leftError = 0;
float rightError = 0;
float prevrightError = 0;
float prevleftError = 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.42f;
float i =0.0f;
float d=0.1f; 
    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;
            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.35f && leftFrontValue > 0.16f)
        {
            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;
       
       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);
    rspeed = constrain(0.05f, 1.0f, rbase-adjust_r);
       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();
    }
}