File content as of revision 1:fb18b43590e6:

#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() {
    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;
            wait(.01f);
            FrontLeftIR=1;
            wait(.01f);
            FrontRightIR=1;
            wait(.01f);
            RightIR=1;
       ProcessIR(dt, ir_lman, lspeed, rspeed);
       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();
    }
}