File content as of revision 11:e647b14ff4ef:

 //added routines to main for easier testing of movement functions
//wrote testStop();
//Kevin Lee Nov 30, 2017
//edited testStop();
//started a testTurns();
//testTurns not working as thought
//Myles Byrne Nov 30, 2017

//TODO: test quarterTurnLeft(); , quarterTurnRight(); for delay constants 
//TODO: finetune PID , Multiplexing for IR LEDS , 
//SUGGESTIONS: implement turn functions using encoders. i.e. each pulse represents 1 degree, need difference of 90 pulses for right turn


#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 IRError = 0; //added
double prevError = 0;
Serial pc (PA_2, PA_3); //serial comm enabled on pins pa_2 and pa_3
Timer timer;
int counter = 0;
float threshold = 0.00085;
float turnThreshold = 0.0004;
    


//IRR = IR Reciver
AnalogIn RS_IRR(PA_0); //Right Side 
AnalogIn RF_IRR(PA_4); //Right Front
AnalogIn LF_IRR(PC_1); //Left Front
AnalogIn LS_IRR(PC_0); //Left Side

//IRE = IR Emitter
DigitalOut RS_IRE(PC_10); //Right Side
DigitalOut RF_IRE(PC_11); //Right Front
DigitalOut LF_IRE(PB_0); //Left Front
DigitalOut LS_IRE(PB_7); //Left Side 

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);
    C_speed = P_controller(Error) + I_controller(Error) + ex;
    if (C_speed < 0)
        C_speed = C_speed*-1;
    float Front_IRError = RF_IRR.read() + LF_IRR.read();  //rev1
    float Side_IRError = RS_IRR.read() - LS_IRR.read();   //rev1,define side_error as positive if closer to right
}


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 forwardSlow()
{
    float iS_speed = 0.05;
    double f1_speed = iS_speed + C_speed;
    double f2_speed = iS_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.15) {   //upper bound, can not go faster
        f1_speed = 0.15;
    }
    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(iS_speed);
        m_Left_F.write(iS_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 quarterTurnRight() //rev1, for ladders
{
    m_Left_B.write(0);
    m_Right_F.write(0);
    m_Left_F.write(i_speed);
    m_Right_B.write(i_speed);
    wait(.1); //delay needs testing
}

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); //this is dependent on i_speed, can we write a function that varies with i_speed?
}

void quarterTurnLeft() { //rev1, for ladders
    m_Left_F.write(0);
    m_Right_B.write(0);
    m_Right_F.write(i_speed);
    m_Left_B.write(i_speed);
    wait(.1); //time needs testing
}

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


/*void main1() {
    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();
    LS_IRE.write(1);
    RS_IRE.write(1);  
    
    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();
        
    LS_IRE.write(0);
    RS_IRE.write(0); 
    }
    stop();
} 
*/


void testStop() //rev1
{
    //printf("\nAnalogIn example\n");
    LF_IRE.write(1);
    RF_IRE.write(1);
    forward();  
    while(1) {
        if (RF_IRR.read() > turnThreshold && LF_IRR.read() > turnThreshold)
        {
            stop();
            forwardSlow();
            if (RF_IRR.read() > threshold && LF_IRR.read() > threshold)
            {
                stop();
                break;
            }
        }
        float value1 = LF_IRR.read();
        float value2 = RF_IRR.read(); 
        printf("LF Led: %f\n", value1);
        wait(0.25);
        printf("RF Led: %f\n\r", value2);
    }
            
}


void testTurnAround() //rev1
{
    
}

void testTurns()
{
    LF_IRE.write(1);
    RF_IRE.write(1);
    forward();  
    while(1)
    {
        if (RF_IRR.read() > turnThreshold && LF_IRR.read() > turnThreshold)
        {
            stop();
            forwardSlow();
            if (RF_IRR.read() > threshold && LF_IRR.read() > threshold)
            {
                stop();
                if (LS_IRR.read() > turnThreshold) {
                    if (RS_IRR.read() < turnThreshold)
                        turnRight();
                    else
                        turnAround(); 
                }
        
                else if (RS_IRR.read() > turnThreshold) {
                    if (LS_IRR.read() < turnThreshold)
                        turnLeft();
                    else
                        turnAround();
                }
                else
                    turnAround();
        
                LS_IRE.write(0);
                RS_IRE.write(0); 
            }
            //stop();
            break;
        }
    }
        float value1 = LF_IRR.read();
        float value2 = RF_IRR.read(); 
        printf("LF Led: %f\n", value1);
        wait(0.25);
        printf("RF Led: %f\n\r", value2);
}



int main()
{
    systicker.attach_us(&systick, 1000);    //rev1
    
    testStop();
}




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