File content as of revision 0:e997997dca8e:

#include "mbed.h"
#include "QEI.h"
 
Serial pc(SERIAL_TX, SERIAL_RX); 
 
PwmOut lpwmf(PA_7);
PwmOut lpwmb(PB_6);
 
PwmOut rpwmf(PB_10);
PwmOut rpwmb(PC_7);
 
 
PinName rfront(PB_3);
PinName rback(PA_15);
//DigitalIn lfront(PB_3);
//DigitalIn lback(PA_15);
//Right Encoder
PinName lfront(PA_1);
PinName lback(PC_4);
 
//QEI::Encoding encoding = QEI::Encoding[1];
 
QEI LeftEncoder(lfront, lback, NC, 4096, QEI::X4_ENCODING);
QEI RightEncoder(rfront, rback, NC, 4096, QEI::X4_ENCODING);       
 
const float rbase = 0.135f;
const float lbase = 0.12f;
 
Timer t_time;
 
struct pid { //Note that because we have two different types of distance sensors (Andrew's works a little differently than Jeffrey's we should have two different errors. To stay straight though we can just use one side right?)
  pid(){
        integral = 0.0f;
        prev = 0.0f;
        kp = .01f; //the ks should be negative to counteract error
        ki = 0.0f;
        kd = 0.8f;
      }
  float integral;
  int prev;
  float kp; //the ks should be negative to counteract error
  float ki;
  float kd;
};
 
void resetpid(struct pid *e) {
  e->integral = 0.0f;
  e->prev = 0.0f;
}
 
float getFix(struct pid *e, float error, float time) {
  float d = (error - e->prev)/time;
  e->integral += error * time;
  e->prev = error;
  float temp = (e->kp * error + e->ki * e->integral + e->kd * d);
  return temp;

}
 
float constrain(float l_limit, float u_limit, float val){
    if (val < l_limit){
        return l_limit;
        }
    else if(val > u_limit){
        return u_limit;
        }    
    return val;    
    }
 
int main() {
    pid lman, rman;
    lman.kp = .0040f;
    lman.ki = .0002f;
    lman.kd = .0015f;
    rman.kp = .5f;
    rman.ki = .1f;
    rman.kd = .4f;
    
    
    lpwmf.period(0.01f);
    lpwmf = lbase;
    rpwmf.period(0.01f);
    rpwmf = rbase;
    t_time.start();
    
    while(1){
        float dt = t_time.read();
        t_time.reset();
        float error = (LeftEncoder.getPulses() - RightEncoder.getPulses())/16.0f; //Can be pos or neg, Range from 0 - 255 for one revolution

//        pc.printf("no %d \n", RightEncoder.getPulses());
//        pc.printf("yas %d \n", LeftEncoder.getPulses());


        if(error == 0.0f){
            resetpid(&lman);
            resetpid(&rman);
        }
        float adjust_l = getFix(&lman, error, dt); 
        pc.printf("%f \n", adjust_l);
        
        float lspeed = constrain(0.0f, 0.4f, lbase - adjust_l);
        float rspeed = rbase;
        
        pc.printf("no %f \n", rspeed);
        pc.printf("yas %f \n", lspeed);
        
        lpwmf = lspeed;
        rpwmf = rspeed;
        
        wait(0.2f);
        
        }
        t_time.stop();
    }