File content as of revision 3:81d97506bd46:

#ifndef Header_H
#define Header_H
//IR:
//PB 7 Left Side
//PB 0 Front Left
//PC 11 Front Right 
//PC 10 Side Right
//Receiver:
// Left Side PC_0
// Front Left PC_1
// Front Right PA_4
// Side Left PA_0
DigitalOut LeftIR(PB_7);
DigitalOut FrontLeftIR(PB_0);
DigitalOut FrontRightIR(PC_11);
DigitalOut RightIR(PC_10);
AnalogIn LeftReceiver(PC_0);
AnalogIn FrontLeftReceiver(PC_1);
AnalogIn FrontRightReceiver(PA_4);
AnalogIn RightReceiver(PA_0);
PwmOut lpwmf(PA_7);
PwmOut lpwmb(PB_6);
PwmOut rpwmf(PB_10);
PwmOut rpwmb(PC_7);
PinName rfront(PB_3);
PinName rback(PA_15);
PinName lfront(PA_1);
PinName lback(PC_4);
Serial pc(SERIAL_TX, SERIAL_RX); 
QEI LeftEncoder(lfront, lback, NC, 4096, QEI::X4_ENCODING);
QEI RightEncoder(rfront, rback, NC, 4096, QEI::X4_ENCODING);       
const float rbase = 0.16f +.1f;
const float lbase = 0.09f + .1f;
const float WALL_IR_L = 0.9f;
const float WALL_IR_R = 0.9f;
const float WALL_IR_FL = 0.92f;
const float WALL_IR_FR = 0.92f;    
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)/(float)time;
  e->integral += error * time;
  e->prev = error;
  return (float)(e->kp * error + e->ki * e->integral + e->kd * d);
}

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

struct IRstruct{
    IRstruct(){
        statuscode = 0;
    }
    float leftIR, leftfrontIR, rightfrontIR, rightIR;
    unsigned int statuscode;
};

unsigned int readIR(IRstruct& in_ir){
    in_ir.leftIR = LeftReceiver.read();
    in_ir.rightIR = RightReceiver.read();
    in_ir.leftfrontIR = FrontLeftReceiver.read();
    in_ir.rightfrontIR = FrontRightReceiver.read();
    pc.printf("LeftIR: %f RightIR: %f leftfronIR: %f rightfrontIR: %f \n", in_ir.leftIR, in_ir.rightIR, in_ir.leftfrontIR, in_ir.rightfrontIR); 
    
    //We then set the status codes to be analyzed later for what we should do
    in_ir.statuscode = (in_ir.statuscode|0x0);
    
    //0000...FL,FR,L,R
    //Bottom is more of an abstraction, we probably don't even need it
    if(in_ir.leftIR > WALL_IR_L)
        in_ir.statuscode = (in_ir.statuscode|2); // 0...0 | 1 -> 0...1
    if(in_ir.rightIR > WALL_IR_R)
        in_ir.statuscode = in_ir.statuscode|1;
    if(in_ir.leftfrontIR > WALL_IR_FL)
        in_ir.statuscode = in_ir.statuscode|8;
    if(in_ir.rightfrontIR > WALL_IR_FR)
        in_ir.statuscode = in_ir.statuscode|4;
    return in_ir.statuscode;
}

void turn_right(){
    float l_init = LeftEncoder.getPulses();
    while((LeftEncoder.getPulses() - l_init) < 10.0f){
        lpwmb = 0.0f;
        rpwmf = 0.0f;
        lpwmf = lbase;
        rpwmb = rbase;
    }
}

void turn_left(){
    float r_init = RightEncoder.getPulses();
    while((RightEncoder.getPulses() - r_init) < 1000.0f){
        lpwmf = 0.0f;
        rpwmb = 0.0f;
        lpwmb = lbase;
        rpwmf = rbase;
    }
}

void ProcessIR(float dt, pid& IR_lman, float& lspeed, float& rspeed){
    IRstruct ir_read;
    int status = readIR(ir_read); 
    if(ir_read.leftIR > WALL_IR_L && ir_read.leftfrontIR > WALL_IR_FL && ir_read.rightfrontIR > WALL_IR_FR){ //High in front and left -> Turn right
        turn_right();
    }
    else if(ir_read.rightIR > WALL_IR_R && ir_read.leftfrontIR > WALL_IR_FL && ir_read.rightfrontIR > WALL_IR_FR){ //High in front and right -> Turn left
        turn_left();
    }
    float error_ir = (ir_read.rightIR - ir_read.leftIR)/16.0f;
    if(error_ir < 0.0001f){    
        resetpid(&IR_lman);
    }
    float adjust_l = getFix(&IR_lman, error_ir, dt);
    lspeed = constrain(0.0f, 0.4f, lbase - adjust_l);
    rspeed = constrain(0.0f, 0.4f, lbase);
}
    
#endif