File content as of revision 18:14bf07a27998:

 //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"

DigitalOut led1(LED1);  //for debugging, on-board led. turn on by
                        //led1 = true;
QEI encoder_Right(PB_3, PA_15, NC, 360, QEI::X4_ENCODING);
QEI encoder_Left(PA_1, PC_4, NC, 360, QEI::X4_ENCODING);
PwmOut m_Right_F(PB_10);
PwmOut m_Right_B(PC_7);
PwmOut m_Left_F(PA_7);
PwmOut m_Left_B(PB_6);
//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
 
Serial pc (PA_2, PA_3); //serial comm enabled on pins pa_2 and pa_3
Timer timer;
 
double Kp = 0.27;//.005;
double Ki = 0.001;//0.0000001;
double Kd = 0.001;
double i_speed = 0.15;
double C_speed = 0;
int integrator = 0;
int decayFactor = 1;
double Error = 0;
float IRError = 0; //added
double prevError = 0;
int counter = 0;
float threshold = 50;
float turnThreshold = 25;
float LS_reading = 0;
float RS_reading = 0;
float LF_reading = 0;
float RF_reading = 0;
int LEDSelector = 0;    
float sum = 0;
float size = 0;
double IR_Kp = 0.001;
double IR_correction = 0;

void IRP_controller() {   //+ means need to turn left, - means turn right
    IRError = RS_reading - LS_reading;
    IR_correction = (IR_Kp*IRError);
    }

void resetEncoders(){
    encoder_Right.reset();
    encoder_Left.reset();
    
    }


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()
{
    IRP_controller();
    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;
        
        
    switch(LEDSelector) {   //multiplexes IRLEDS
        case 0:
            LS_IRE.write(1);
            if (LS_IRR.read() == 0) {
                LS_IRE.write(0);
                break;
                }
            LS_reading = LS_IRR.read() * 33000;
            LS_IRE.write(0);
            LEDSelector++;
            break;
        case 1:
            RS_IRE.write(1);
             if (RS_IRR.read() == 0) {
                RS_IRE.write(0);
                break;
                }
            RS_reading = RS_IRR.read() * 36332.5;//scale from 49450
            RS_IRE.write(0);
            LEDSelector++;
            break;
        case 2:
            LF_IRE.write(1);
             if (LF_IRR.read() == 0) {
                LF_IRE.write(0);
                break;
                }
            LF_reading = LF_IRR.read() * 50000;//scale from 33000
            LF_IRE.write(0);
            LEDSelector++;
            break;
        case 3:
            RF_IRE.write(1);
             if (RF_IRR.read() == 0) {
                RF_IRE.write(0);
                break;
                }
            RF_reading = RF_IRR.read() * 33000;
            RF_IRE.write(0);
            LEDSelector = 0;
            break;
        }
}

bool corner() {
    if (RS_reading > 1078000000 && LS_reading > 1078000000 && RF_reading > 1080000000 && LF_reading > 1080000000)
        return true;
    else return false;
    }

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 IRForward() {  
    printf(" IRError: %f\n " , IRError);
    sum += IRError;
    size++;
    float avg = sum/size;
    //printf(" avg : %f\n " ,avg);  
    double high_threshold = 0.25;
    double low_threshold = 0.05;
    
    if (IRError > 30) {
        double high_speed = i_speed + IR_correction;
        double low_speed = i_speed - IR_correction;
        if (high_speed > high_threshold)   //upper bound
          high_speed = high_threshold;
        if (high_speed < low_threshold)
           high_speed = low_threshold;
        if (low_speed < low_threshold)
            low_speed = low_threshold;
         if (low_speed > high_threshold)
             low_speed = high_threshold;
        
        m_Left_F.write(low_speed);
        m_Right_F.write(high_speed);
        printf(" + left: %f\n " , low_speed);
        printf(" + right: %f\n " , high_speed);
        printf("\n");
        //wait(2);
        resetEncoders();
        }
    if (IRError < -30) {
        double high_speed = i_speed - IR_correction;
        double low_speed = i_speed + IR_correction;
   if (high_speed > high_threshold)   //upper bound
          high_speed = high_threshold;
        if (high_speed < low_threshold)
           high_speed = low_threshold;
        if (low_speed < low_threshold)
            low_speed = low_threshold;
         if (low_speed > high_threshold)
             low_speed = high_threshold;
        m_Left_F.write(high_speed);
        m_Right_F.write(low_speed);
        printf(" - left: %f\n " , high_speed);
        printf(" - right: %f\n " , low_speed);
        printf("\n");
        resetEncoders();
        //wait(2);
    }
    if (IRError <= 30 && IRError >= -30) {
       forward();
       printf("forward\n");
       printf("\n");
    }
}

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.2) {   //upper bound, can not go faster
        f1_speed = 0.2;
    }
    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 stop()
{
    m_Right_F.write(0);
    m_Right_B.write(0);
    m_Left_F.write(0);
    m_Left_B.write(0);    
}


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 backUpForTurn()
{
    stop();
    while (RF_reading > 50 && LF_reading > 50) {
        m_Left_B.write(i_speed);
        m_Right_B.write(i_speed);
    }
    stop();
}

void turnRight() {
    //double scale = RF_reading / LF_reading;
    m_Left_F.write(0.5);
    m_Right_B.write(0.5);
    resetEncoders();
    wait(0.1); //*scale);
    
    }

/*void turnRight() {
    double turnSpeed = 0.1;
    printf("Turning Error: %f\n " , IRError ); 
    m_Left_F.write(turnSpeed+0.1);
    m_Right_B.write(turnSpeed/2); 
    //while (IRError > 0.05 || IRError < -0.05) 
    while((LF_reading - RF_reading) > -24)
    {printf("LF-RF: %f\n" , LF_reading-RF_reading); }
    printf("Final Error: %f\n " , IRError);
    stop();
    }*/
    
void turnLeft()
{
    m_Right_F.write(0.5);
    m_Left_B.write(0.5);
    resetEncoders(); 
    wait(0.1); //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);
    resetEncoders();
    wait(.6);
}

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


bool testStop() //rev1
{
    //bool stopOut=false;
    //while(!stopOut) { //loop
        //forward();
        
        if (RF_reading > turnThreshold  && LF_reading > turnThreshold && RS_reading > turnThreshold  && LS_reading > turnThreshold ) //IR Receivers will read higher analog values than Threshold if wall is near
        {
            printf(" 1 stop\n");
            stop(); //stop the rat
            //wait(1);    //delay optional
            while(1){
                //forwardSlow();
                if (RF_reading > threshold && LF_reading > threshold)
                {
                    printf(" 2 stop\n");
                    stop();
                    return true;
                    break;
                }
            }
            
        }
        return false;
    }
            //printf(" end");
//}


void testTurnAround() //rev1
{
    
}

void testRightTurn() {
    forward();
    while(1) {
        if (LF_reading > threshold && RF_reading > threshold)
            break;
        }
    float leftCount = encoder_Left.getPulses();
    float rightCount = encoder_Right.getPulses();
    while (encoder_Left.getPulses() < (leftCount + 180) && encoder_Right.getPulses() > rightCount + 180) {
        m_Left_F.write(0.1);
        m_Right_B.write(0.1);
        }
        stop();
    }

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

void debugIR() {
    while(1) {
        printf("LS %f\n ", LS_reading);
        wait(0.1);
        printf("LF %f\n ", LF_reading);
        wait(0.1);
        printf("RS %f\n ", RS_reading);
        wait(0.1);
        printf("RF %f\n ", RF_reading);
        wait(0.1);
        printf("\n");
        }
    }
    
void afterTurnForward() {
    resetEncoders();
    Timer turn;
    turn.start();
    int current = turn.read_ms();
    while ((turn.read_ms() - current) < 500)
            { forward();
            printf("%f\n", turn.read_ms());
             }
    printf("out of after\n");
    stop();
    //resetEncoders();
    wait(1);
    }
    
void LeftOrRight() {
    while(1){
    if(IRError > 30){
        turnLeft();
        stop();
        break;
        }
    else if(IRError < -30){
        turnRight();
        stop(); 
        break;
        }
    else{
        stop();
        //break;
        }
    }
}

int main()
{
    systicker.attach_us(&systick, 1000);    //rev1
    wait(1);    //DONT DELETE INITIAL WAIT
    //testStop();
    //turnAround(); 
    
    while (1) {
         
        IRForward(); 
        if (testStop()) {
            backUpForTurn();
            LeftOrRight();
            stop();
            wait(0.05); 
            afterTurnForward();
            //wait(0.5);
            stop();
            }
            
        /*if (corner) {     //TEST CORNER() test
            turnAround();
            stop();
            break;
            }
        */       
           //printf("%d\n", RF_reading);      
        }
    
}