File content as of revision 7:e10dc3cb9212:

#include "mbed.h"
#include "QEI.h"

QEI encoder_Right(PB_3, PA_15, NC, 360, QEI::X4_ENCODING);
QEI encoder_Left(PA_1, PC_4, NC, 360, QEI::X4_ENCODING);
double Kp = 0.27;//.005;
double Ki = 0.001;//0.0000001;
double Kd = 0.001;
PwmOut m_Right_F(PB_10);
PwmOut m_Right_B(PC_7);
PwmOut m_Left_F(PA_7);
PwmOut m_Left_B(PB_6);
double i_speed = 0.15;
double C_speed(0);
int integrator = 0;
int decayFactor = 1;
double Error = 0;
double prevError = 0;
Serial pc (PA_2, PA_3); //serial comm enabled on pins pa_2 and pa_3
Timer timer;
int counter = 0;

double P_controller(int error)
{
    double correction = (Kp*error);
    return correction;
}

double I_controller(int error)
{
    integrator += error;
    double correction = Ki*integrator;
    integrator /= decayFactor;
    return correction;
}

double D_controller(int error)
{
    int dError = error - prevError;
    int dt = timer.read_us();
    timer.reset();
    prevError = error;
    double correction;
    if (dt==0)
        correction=0;
    else
        correction = Kd*dError/dt;
    return correction;
}

Ticker systicker;
//speed = speed + P_Controller(error) + I_Controller(error) + D_Controller(error);
void systick()
{
    double R_en_count = encoder_Right.getPulses()/100;
    double L_en_count = encoder_Left.getPulses()/100;
    Error = R_en_count - L_en_count;
    double ex = D_controller(Error);
    // if (ex < 0)
    //    ex = -ex;
    C_speed = P_controller(Error) + I_controller(Error) + ex;
    if (C_speed < 0)
        C_speed = C_speed*-1;
}


void forward()
{
    double f1_speed = i_speed + C_speed;
    double f2_speed = i_speed - C_speed;

    /*pc.printf("C: %f", C_speed);
    if (C_speed < 0)
        pc.printf("-");
    if (C_speed > 0)
        pc.printf("+");
    */


    if(f1_speed >= 0.7) {   //upper bound, can not go faster
        f1_speed = 0.7;
    }
    if (f1_speed <= 0.05)
        f1_speed = 0.05;

    if(f2_speed <= 0.05) {  //lower bound, should not be slower than this
        f2_speed = 0.05;
    }

    pc.printf(" f1: %f", f1_speed);
    pc.printf(" f2: %f", f2_speed);

    //problems when left wheel is held for the + case
    if (Error > 0) { //right wheel is turning more
        m_Left_F.write(f1_speed);
        m_Right_F.write(f2_speed); //f2_speed
    }
    if (Error < 0) { //left wheel is turning more
        m_Right_F.write(f1_speed);
        m_Left_F.write(f2_speed);  //f2_speed
    }
    if (Error == 0) {
        m_Right_F.write(i_speed);
        m_Left_F.write(i_speed);
    }
}

void backUp()
{
    m_Left_F.write(0);
    m_Right_F.write(0);
    m_Left_B.write(i_speed);
    m_Right_B.write(i_speed);
    wait(.15);
}

void turnRight()
{
    m_Left_B.write(0);
    m_Right_F.write(0);
    m_Left_F.write(i_speed);
    m_Right_B.write(i_speed);
    wait(.2);
}

void turnLeft()
{
    m_Left_F.write(0);
    m_Right_B.write(0);
    m_Right_F.write(i_speed);
    m_Left_B.write(i_speed);
    wait(.2);
}

void turnAround()
{
    m_Left_B.write(0);
    m_Right_F.write(0);
    m_Left_F.write(i_speed);
    m_Right_B.write(i_speed);
    wait(.4);
}

void stop()
{
    m_Right_F.write(0);
    m_Right_B.write(0);
    m_Left_F.write(0);
    m_Left_B.write(0);    
}

void debugEncoder()
{
    while(1) {
        wait(1);
        pc.printf("Right: %i", encoder_Right.getPulses());
        pc.printf("  Left: %i", encoder_Left.getPulses(), "\n");
        pc.printf("\n");
    }
}

void debugError()
{
    while(1) {
        pc.printf("Error: %i\n", Error);
    }
}

AnalogIn RS_IRR(PA_0);
AnalogIn RF_IRR(PA_4); //Right Front
AnalogIn LF_IRR(PC_1); //Left Front
AnalogIn LS_IRR(PC_0); //Left Side
 
DigitalOut RS_IRE(PC_10);
DigitalOut RF_IRE(PC_11); //Right Front
DigitalOut LF_IRE(PB_0); //Left Front
DigitalOut LS_IRE(PB_7); //Left Side 

 

int main()
{
    float threshold = 0.001;
    float turnThreshold = 0.001;
    printf("\nAnalogIn example\n");
    LF_IRE.write(1);
    RF_IRE.write(1);  
    while (1){ 
    while (LF_IRR.read() < threshold && RF_IRR.read() < threshold){
        forward();
        float value1 = LF_IRR.read();
        float value2 = RF_IRR.read(); 
        printf("LF Led: %f\n", value1);
        wait(0.5);
        printf("RF Led: %f\n", value2);
    }
    
    backUp();
    
    if (LS_IRR.read() > turnThreshold)
        if (RS_IRR.read() < turnThreshold)
            turnRight();
        else
            turnAround();
    else if (RS_IRR.read() > turnThreshold)
        if (LS_IRR.read() < turnThreshold)
            turnRight();
        else
            turnAround();
    else
        turnAround();
}
    stop();
}
    /*while(RF_IRR.read() * 100000 < 175 && LF_IRR.read() * 100000 < 175) {
        
        /*meas = LS_IRR.read(); // Converts and read the analog input value (value from 0.0 to 1.0)
        meas = meas * 100000; // Change the value to be in the 0 to 3300 range
        printf("measure = %.0f mV\n", meas);
        */
        /*if (meas > 2000) { // If the value is greater than 2V then switch the LED on
          LS_IRE = 1;
        }
        else {
          LS_IRE = 0;
        }
        */
        /*
        forward();
        //wait(0.2); // 200 ms
    }
    stop();
    */
    



/*int main()      //only runs once
{
    systicker.attach_us(&systick, 1000);    //enable interrupt
    while (1) {
        forward();
    }
    //
    //debugEncoder();
}
*/