File content as of revision 0:394261695392:

//ADJUST VARIABLES SECTION

const int preRunLength = 1600;
const int postRunLength = 0;
const int turnLength = 850;
const float leftAngleTurnThreshhold = -.045;
const float rightAngleTurnThreshhold = -.06;
const float baseSpeed = 0.25;

#include "PinDefinitions.h"

//PROTOTYPES
void motorCorrections();
float getLSreading();
float getLAreading();
float getRAreading();
float getRSreading();
float pidCorrection(int error);
float proportion(int error);
int derivative(int error);
float integral(int error);
void setRightPwm(float speed);
void setLeftPwm(float speed);
void postTurnRun();
void preTurnRun();

Serial pc(PA_11, PA_12); //SERIAL TX/RX
DigitalOut myled(LED1);

Ticker PIDcorrectionTicker;
Ticker gyroOffsetCalc;

float baselineRS;
float baselineLS;
float baselineLA;
float baselineRA;


bool safeToRunSystick;
int rightPulses;
int leftPulses;
float currentAngle = 0;
float refreshTimeSeconds = 0.05; //How fast the systicker is going
const int flashTime = 20; //The amount of time sensors are on before we take measurment (millisecond)

int prevError;
int integrator = 0;
int decayFactor = 1.1;
float Kp = 0.001;
//Kd =
float Ki = 0.002;

//FOR THE SENSORS RUN WHEN DARK TO FIND AMBIENT LIGHT
//HOW LONG DO I NEED TO TURN ON SENSORS TO GET A READING? 1MS? NO TIME AT ALL?



void motorCorrections()  //Three things, I should probably break into three functions!!
{
    if(safeToRunSystick) {
        int rightPulses = RENC.getPulses();
        int leftPulses = LENC.getPulses();
        //we want the difference which is the error
        int errorEncoder = rightPulses - leftPulses;
        float correction = pidCorrection(errorEncoder);
        if(correction > 0.5)
            correction = 0.5;
        else if(correction < -0.5)
            correction = -0.5;
        setRightPwm(baseSpeed-correction);
        setLeftPwm(baseSpeed+correction);
        //pc.printf("Error:%.6f  \r\i", correction);
        //pc.printf("Gyro Raw: %.6f  \r\n", currentAngle);
        RENC.reset();
        LENC.reset();
        myled = !myled;
    }
}


float getLSreading()
{
    int ambient = IRSensorLS.read(); //Ambient Reading
    IRledLS.write(1);
    wait_ms(flashTime); //Maybe we can decrease this amount
    float sensorReading = IRSensorLS.read(); //Will get the reading
    IRledLS.write(0);
    return sensorReading - ambient; //Ideally we will get pure IR reading

}

float getLAreading()
{
    int ambient = IRSensorLA.read(); //Ambient Reading
    IRledLA.write(1);
    wait_ms(flashTime); //Maybe we can decrease this amount
    float sensorReading = IRSensorLA.read(); //Will get the reading
    IRledLA.write(0);
    return sensorReading - ambient; //Ideally we will get pure IR reading
}

float getRAreading()
{
    int ambient = IRSensorRA.read(); //Ambient Reading
    IRledRA.write(1);
    wait_ms(flashTime); //Maybe we can decrease this amount
    float sensorReading = IRSensorRA.read(); //Will get the reading
    IRledRA.write(0);
    return sensorReading - ambient; //Ideally we will get pure IR reading
}

float getRSreading()
{
    int ambient = IRSensorRS.read(); //Ambient Reading
    IRledRS.write(1);
    wait_ms(flashTime); //Maybe we can decrease this amount
    float sensorReading = IRSensorRS.read(); //Will get the reading
    IRledRS.write(0);
    return sensorReading - ambient; //Ideally we will get pure IR reading
}
//The correction will be given in difference that power should be writen to the motors. We will chose baseline speed for both
float pidCorrection(int error)
{
    return proportion(error) + integral(error);
    // +derivative(error)
}

float proportion(int error)
{
    return Kp*error;
}
/*
int derivative(int error){
  int dError = error - prevError;
  int dt = timer.read_us();
  timer.reset();
  prevError = error;
  int correctionD = -(Kd*dError)/dt;
  return correctionD;
}
*/
float integral(int error)
{
    integrator +=error;
    integrator /= decayFactor;
    return Ki*integrator;
}


void setRightPwm(float speed)
{
    if (speed == 0) {
        RMOTORA = 1.0;
        RMOTORB = 1.0;
    }

    if (speed > 0) {
        RMOTORA = speed;
        RMOTORB = 0;
    } else {
        RMOTORA = 0;
        RMOTORB = -speed;
    }
}

void setLeftPwm(float speed)
{
    if (speed == 0) {
        LMOTORA = 1.0;
        LMOTORB = 1.0;
    }

    if (speed > 0) {
        LMOTORA = speed;
        LMOTORB = 0;
    } else {
        LMOTORA = 0;
        LMOTORB = -speed;
    }
}

void setBaselines()
{
    baselineLS = getLSreading();
    baselineRS = getRSreading();
    baselineLA = getLAreading();
    baselineRA = getRAreading();
}


void turnAround()
{
    safeToRunSystick = false;   
    RENC.reset();
    LENC.reset();
    setRightPwm(-.1);
    setLeftPwm(.1);
    myled = !myled;
    while(RENC.getPulses() < 1700 && LENC.getPulses() < 1700) {
    }
    pc.printf("WE MADE IT OUT  \r\n");
    myled = 0;
    setRightPwm(0);
    setLeftPwm(0);
    wait(0.2);
    RENC.reset();
    LENC.reset();
    safeToRunSystick = true;

}

void turnLeft()
{
    safeToRunSystick = false;
    preTurnRun();
    //Now should be in position to turn
    setRightPwm(.1);
    setLeftPwm(-.1);
    RENC.reset();
    LENC.reset();
    myled = !myled;
    while(RENC.getPulses() < turnLength && LENC.getPulses() < turnLength) {}
    setRightPwm(0);
    setLeftPwm(0);
    wait(0.2);
    safeToRunSystick = true;
    //At this point the car should be pivoted
    postTurnRun();
}

void turnRight()
{
    safeToRunSystick = false;
    preTurnRun();
    //Now should be in position to turn
    setRightPwm(-.1);
    setLeftPwm(+.1);
    RENC.reset();
    LENC.reset();
    myled = !myled;
    while(RENC.getPulses() < turnLength && LENC.getPulses() < turnLength) {}
    setRightPwm(0);
    setLeftPwm(0);
    wait(0.2);
    postTurnRun();
    safeToRunSystick = true;
    //At this point the car should be pivoted
}



void preTurnRun(){  //this code runs before and after every turn to ensure that the car is positioned at the right spot when it turns
        RENC.reset();
        LENC.reset();
       int rightPulses = 0;
       int leftPulses = 0;
        //we want the difference which is the error
        while(RENC.getPulses() < preRunLength && LENC.getPulses() < preRunLength) {
        rightPulses = RENC.getPulses() - rightPulses;
        leftPulses = LENC.getPulses() - leftPulses;
        int errorEncoder = rightPulses - leftPulses;
        float correction = proportion(errorEncoder);
        if(correction > 0.5)
            correction = 0.5;
        else if(correction < -0.5)
            correction = -0.5;
        setRightPwm(baseSpeed-correction);
        setLeftPwm(baseSpeed+correction);
        }
}

void postTurnRun(){  //this code runs before and after every turn to ensure that the car is positioned at the right spot when it turns
        RENC.reset();
        LENC.reset();
       int rightPulses = 0;
       int leftPulses = 0;
        //we want the difference which is the error
        while(RENC.getPulses() < postRunLength && LENC.getPulses() < postRunLength) {
        rightPulses = RENC.getPulses() - rightPulses;
        leftPulses = LENC.getPulses() - leftPulses;
        int errorEncoder = rightPulses - leftPulses;
        float correction = proportion(errorEncoder);
        if(correction > 0.5)
            correction = 0.5;
        else if(correction < -0.5)
            correction = -0.5;
        setRightPwm(baseSpeed-correction);
        setLeftPwm(baseSpeed+correction);
        }
}

int main()
{
    wait(0.2);
    myled =1;
    wait(0.5);
    myled =0;
    wait(0.2);
    setBaselines();
    safeToRunSystick = true;
    PIDcorrectionTicker.attach(&motorCorrections, refreshTimeSeconds);
    while(1) {
        //Turns if theres a wall infront of it
        myled = !myled;
        float leftSSensor = getLSreading();
        float rightSSensor = getRSreading();
        if((leftSSensor > 0.1 + baselineLS) || (rightSSensor > 0.1 + baselineRS)) {
       //     turnAround();
        }
        float leftASensor = getLAreading();
        float rightASensor = getRAreading();
        
        float leftAStrig = leftASensor- baselineLA;
        float rightAStrig = rightASensor-baselineRA;
       
        if(rightAStrig < rightAngleTurnThreshhold)
            turnRight();
        else if(leftAStrig < leftAngleTurnThreshhold)
            turnLeft();
        
      
        //pc.printf("Left angle: %.6f  \r\n", leftASensor);
       // pc.printf("Right angle: %.6f  Left angle: %.6f \r\n", rightAStrig, leftAStrig);
       // pc.printf("Gyro accum: %.6f  \r\n", accumGyro);
      
        
    }
}