File content as of revision 9:97941581fe81:

#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
DigitalIn PushButton(PC_13);
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.146f; //.09
const float lbase = 0.17f;  //.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;
  float 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 = .0021f;
    lman.ki = .0000f;
    lman.kd = -.0005f;
    rman.kp = .0019f;
    rman.ki = .0000f;
    rman.kd = -.0005;
}
void resetpid(struct pid *e) {
  e->integral = 0.0f;
  e->prev = 0.0f;
}

inline void brake(){
    lpwmf = 1.0f;
    lpwmb = 1.0f;
    rpwmf = 1.0f;
    rpwmb = 1.0f;
}

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

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) < 214*num_times){
        lpwmb = 0.0f;
        rpwmf = 0.0f;
        lpwmf = lbase;
        rpwmb = rbase;
        //printEncoders();   
    }
    lpwmf = 0.0f;
    rpwmb = 0.0f;
    wait(1.0);
    return;
}

void turn_left(int num_times){
    int r_init = RightEncoder.getPulses();
    //pc.printf("r_init %d \n", r_init);
    while((RightEncoder.getPulses() - r_init) < 142*num_times){
        lpwmf = 0.0f;
        rpwmb = 0.0f;
        lpwmb = lbase;
        rpwmf = rbase;
        //printEncoders();
    }
    lpwmb = 0.0f;
    rpwmf = 0.0f;
    wait(1.0);
}

void FlashFrontIRs(float& flIR, float& frIR){
    using namespace constants;
    FrontLeftIR = 1;
    FrontRightIR = 1;
    wait_ms(10);
    flIR = FrontLeftReceiver;
    frIR = FrontRightReceiver;
    return;
}

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 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.03f, 1.0f, lbase - adjust_l - frontP*totalFrontChange);
            rspeed = constrain(0.03f, 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(){ //We only use this when there is nothing to the sides of the walls
    using namespace constants;
        //brake();
        //wait(0.1);
        while(LeftEncoder.getPulses() < 435 && RightEncoder.getPulses() < 435){ //Only traverse one cell at a time.
            float dt = 1.0f;
            float FLIR, FRIR;
            FlashFrontIRs(FLIR, FRIR);
            /*if(FLIR > 0.15f || FRIR > 0.15f){
                brake();
                return;
            }*/
            float left_encoder = (float)LeftEncoder.getPulses();
            float right_encoder = (float)RightEncoder.getPulses();
            float error = (float)left_encoder - (float)right_encoder; //Can be pos or neg
            if(error == 0.0f){
                resetpid(&lman);
                resetpid(&rman);
            }
            float adjust_r = getFix(&rman, error, dt);
            float adjust_l = getFix(&lman, error, dt);
            lpwmf = constrain(0.06f, 0.45f, lbase-adjust_l);
            lpwmb = 0.0f;
            rpwmf = constrain(0.06f, 0.45f, rbase+adjust_r);
            rpwmb = 0.0f;
            printf("Left Encoder: %.2f Right Encoder: %.2f DT: %.2f Error: %.2f adjust_l: %.2f adjust_r: %.2f \n", left_encoder, right_encoder, dt, error, adjust_l, adjust_r);
            t_time.reset();
    }
    RightEncoder.reset();
    LeftEncoder.reset();
    return;
}
    
#endif