File content as of revision 8:22e399fe87a4:

#ifndef Header_H
#define Header_H
#include "constants.h"
#define NORTH 1
#define WEST 3
#define SOUTH 5
#define EAST 7
//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(PB_6);
PwmOut lpwmb(PA_7);
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.15f; //.09
const float lbase = 0.163f;  //.1

static int global_orientation = 0;
struct pid;

Timer t_time;
//Initializes different PIDs for use
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?)
  public:
  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;
};

pid lman, rman;

void initmotors_and_pid(){
    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;
    lman.kp = .009f;
    lman.ki = .0f;
    lman.kd = .0f;
    rman.kp = .5f;
    rman.ki = .1f;
    rman.kd = .4f;
}
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;    
}

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

void turn_right(int num_times){ //Turns clockwise
    int l_init = LeftEncoder.getPulses();
    pc.printf("l_init %d \n", l_init);
    for(int i = 0; i < num_times; i++){
        if(global_orientation == 1){
            global_orientation = 7; //It's now east
        }    
        else
            global_orientation-=2;
    }
    while((LeftEncoder.getPulses() - l_init) < 166*num_times){
        lpwmb = 0.0f;
        rpwmf = 0.0f;
        lpwmf = lbase;
        rpwmb = rbase;
    }
    lpwmf = 0.0f;
    rpwmb = 0.0f;
    wait(1.0);
}

void turn_left(int num_times){
    int r_init = RightEncoder.getPulses();
    pc.printf("r_init %d \n", r_init);
    while((RightEncoder.getPulses() - r_init) < 150*num_times){
        lpwmf = 0.0f;
        rpwmb = 0.0f;
        lpwmb = lbase;
        rpwmf = rbase;
    }
    lpwmb = 0.0f;
    rpwmf = 0.0f;
    wait(1.0);
}
void FlashIRs(float& leftIRBase, float& rightIRBase, float& leftFrontIRBase, float& rightFrontIRBase){
    using namespace constants;
    LeftIR = 1;
    RightIR = 1;
    wait_ms(10);
    leftIRBase = LeftReceiver;
    rightIRBase = RightReceiver;
    LeftIR = 0;
    RightIR = 0;
    FrontLeftIR = 1;
    leftFrontIRBase = FrontLeftReceiver;
    FrontRightIR = 1;
    rightFrontIRBase= FrontRightReceiver;
    wait_ms(10);
    FrontLeftIR = 0;
    FrontRightIR = 0;
}

void printIRSensors(float leftValue, float rightValue, float leftFrontValue, float rightFrontValue){
    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");
}

void printEncoders(){
    pc.printf("LeftEncoder: %d \n", LeftEncoder.getPulses());
    pc.printf("lEncoderDifference %f \n", LeftEncoder.getPulses());
}

void usePIDBoth(){
     using namespace constants;
        while(LeftEncoder.getPulses() < 130 && RightEncoder.getPulses() < 130){       
           float dt = t_time.read();
           float lspeed = 0; float rspeed = 0;
           prevRightFrontValue = rightFrontValue;
           prevLeftFrontValue = leftFrontValue;
           FlashIRs(leftValue, rightValue, leftFrontValue, rightFrontValue);
           //If we detect a wall, turn right or turn left.
            if(leftValue> 0.34f && leftFrontValue > 0.15f)
            {
                return;
                //turn_right(1);
            }
            else if(rightFrontValue > 0.15f && rightValue > 0.35f)
            {
                return;
                //turn_left(1);
            }
            //Calculates PID Adjustment from basic Algebra
           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 ; //Sets the final adjustment values we expect for the motor from Sensor readings
           
           prevleftError = leftError;
           prevrightError = rightError;
            lspeed = constrain(0.05f, 1.0f, lbase - adjust_l - frontP*totalFrontChange);
            rspeed = constrain(0.05f, 1.0f, rbase-adjust_r - frontP*totalFrontChange);
           //Set the motors to the desired speed. 0 Values indicate to the H-Bridge to go forward
           lpwmb = 0; rpwmb = 0;
           lpwmf = lspeed; rpwmf = rspeed;
           t_time.reset();
        }
}

void usePIDEncoder(){
    using namespace constants;
        while(LeftEncoder.getPulses() < 130 && RightEncoder.getPulses() < 130){ //Only traverse one cell at a time.
            float dt = t_time.read();
            float left_encoder = LeftEncoder.getPulses();
            float right_encoder = RightEncoder.getPulses();
            printf("Left Encoder: %.2f %%\n", left_encoder);
            printf("Right Encoder: %.2f %%\n", right_encoder);
            float error = left_encoder - right_encoder; //Can be pos or neg
            if(error == 0){
                resetpid(&lman);
                resetpid(&rman);
            }
            float adjust_r = getFix(&rman, error, dt);
            lpwmf = lbase;
            lpwmb = 0.0f;
            rpwmf = constrain(0.04f, 0.5f, rbase-adjust_r);
            rpwmb = 0.0f;
            if(RightEncoder.getPulses() > cell_length && LeftEncoder.getPulses() > cell_length){ //We've traversed a cell, update cell count;
                RightEncoder.reset(); 
                LeftEncoder.reset();   
            }
        if(leftValue> 0.6f && leftFrontValue > 0.6f)//Maybe we should brake in here?
        {
            return;
        }
        else if(rightFrontValue > 0.6f && rightValue > 0.6f)
        {
            return;
        }   
       t_time.reset();
    }
  }
    
#endif