File content as of revision 10:c57f6a5042d7:

#include "rtos.h"
#include "PID.h"
#include "PololuQik2.h"
#include "QEI.h"
#include "mbed.h"
#include "HCSR04.h"
#include "stdio.h"
#include "LPC17xx.h"
#include "Sharp.h"
 
#define PIN_TRIGGERL    (p12)
#define PIN_ECHOL       (p11)
#define PIN_TRIGGERR    (p29)
#define PIN_ECHOR       (p30)
#define PULSE_PER_REV  (1192)
#define WHEEL_CIRCUM   (12.56637)
#define DIST_PER_PULSE (0.01054225722682)
#define MTRS_TO_INCH   (39.3701)
#define MAX_SPEED      (0.3*127)
#define PPR            (4331/4)
#define LEFT           (1)
#define RIGHT          (0)
#define STRAIGHT       (2)
#define FORWARD        (1)
#define BACKWARD       (0)
#define TOOLS          (0)
#define MID            (1)
#define RIGS           (2)
#define FIRST_WAVE     (0)
#define FAR            (1)
 
 
float range, range2, pid_return;
void errFunction(void);
bool cRc;
 
//Hardware Initialization
Serial bt(p13,p14);
Serial pc(USBTX,USBRX);
HCSR04 rangeFinderLeft( PIN_TRIGGERL, PIN_ECHOL );
HCSR04 rangeFinderRight( PIN_TRIGGERR, PIN_ECHOR );
PID pid1(15.0,0.0,4.0,0.02);
PololuQik2 motors(p9, p10, p8, p15, errFunction, cRc);
QEI rightEncoder(p17,p18,NC,PPR,QEI::X4_ENCODING);
QEI leftEncoder(p16,p15,NC,PPR,QEI::X4_ENCODING);
Sharp IR(p20);
//InterruptIn encoder(p29);
 
 
//Functions
 
float wall_follow(int side, int direction, int section);
void wall_follow2(int side, int direction, int section, float location);
void wall_follow3(int &currentLocation, int &WaveOpening);
void leftTurn(void);
void slightleft(void);
void rightTurn(void);
void us_distance(void);
void tools_section(float* location, float &current);
void mid_section(float* location, float &current, int* direction);
void mid_section2(float* location, float &current, int* direction);
void rig_section(float* location, float &current, int* direction, int rig);
void overBump(int section);
void alignWithWall(int section);
void UntilWall(int dir);

 
//Variables
 
int main(void)
{
    float location[3], current=0;
    int direction[3];
    double distance;
 
    pc.baud(115200);
    bt.baud(115200);
    motors.begin();
 
    bt.printf("START\r\n");
    //Go to tools
    tools_section(location, current);
    mid_section(location, current, direction);
    //mid_section2(location, current, direction);
   /* while(1) {
        //bt.printf("IR %f\r\n", US.getDistance());
        rangeFinderLeft.startMeas();
        wait_ms(20);
        rangeFinderLeft.getMeas(range);
        bt.printf("Range = %f\n\r", range);
 
        wait_ms(200);
    }*/
 
 
    //leftTurn();
    //rightTurn();
 
 
}
 
void errFunction(void)
{
    //Nothing
}
 
void us_distance(void)
{
    pc.printf("Ultra Sonic\n\r");
    rangeFinderLeft.startMeas();
    wait_us(20);
    if ( (rangeFinderLeft.getMeas(range) == RANGE_MEAS_VALID)) {
        pc.printf("Range = %f\n\r", range);
    }
}
 
float wall_follow(int side, int direction, int section)
{
    float location, wavegap=0, set=5;
    int dir=1;
 
    pid1.reset();
 
    if(direction == BACKWARD) dir=-1;
    if(section == TOOLS)set= 10;
 
    leftEncoder.reset();
    rightEncoder.reset();
 
    location=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
 
    while(location< 73) {
        location=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
 
        pid1.setInputLimits(0, set);
        pid1.setOutputLimits( -MAX_SPEED, MAX_SPEED);
        pid1.setSetPoint(set);
        if(side) {
            rangeFinderLeft.startMeas();
            wait_ms(38);
            rangeFinderLeft.getMeas(range);
        } else {
            rangeFinderRight.startMeas();
            wait_ms(38);
            rangeFinderRight.getMeas(range);
            pc.printf("%d\r\n",range);
        }
 
        if(range > 20) {
            wavegap=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
            //bt.printf("wavegap %f\r\n",wavegap);
            // AT WAVE OPENING!!!!
            motors.setMotor1Speed(dir*0.4*127);//left
            motors.setMotor0Speed(dir*0.4*127);//right
        } else {
 
            pid1.setProcessValue(range);
            pid_return = pid1.compute();
 
            if(pid_return > 0) {
                if(side) {
                    motors.setMotor0Speed(dir*MAX_SPEED - dir*pid_return);//right
                    motors.setMotor1Speed(dir*MAX_SPEED);//left
                } else {
                    motors.setMotor1Speed(dir*MAX_SPEED - dir*pid_return);//left
                    motors.setMotor0Speed(dir*MAX_SPEED);//right
                }
            } else if(pid_return < 0) {
                if(side) {
                    motors.setMotor0Speed(dir*MAX_SPEED);//right
                    motors.setMotor1Speed(dir*MAX_SPEED + dir*pid_return);//left
                } else {
                    motors.setMotor1Speed(dir*MAX_SPEED);//left
                    motors.setMotor0Speed(dir*MAX_SPEED + dir*pid_return);//right
                }
            } else {
                motors.setMotor0Speed(dir*MAX_SPEED);//right
                motors.setMotor1Speed(dir*MAX_SPEED);//left
            }
        }
    }
    return wavegap;
}
 
/* MODIFIED WALL_FOLLOW FOR NAVIGATION */
 
void wall_follow2(int side, int direction, int section, float location)
{
    bool SeeWaveGap = false, dir=1, limit=83;
    float set=5, loc=0;
 
    pid1.reset();
 
    if(direction == BACKWARD) dir=-1;
    if(section == TOOLS){
        set= 5;
        limit = 78;
    }

    
    leftEncoder.reset();
    rightEncoder.reset();
 
    while(dir*loc + location <= limit) {
        loc=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
 
        pid1.setInputLimits(0.0, set);
        pid1.setOutputLimits( -MAX_SPEED, MAX_SPEED);
        pid1.setSetPoint(set);
 
        if(side) {
            rangeFinderLeft.startMeas();
            wait_ms(38);
            rangeFinderLeft.getMeas(range);
        } else {
            rangeFinderRight.startMeas();
            wait_ms(38);
            rangeFinderRight.getMeas(range);
        }
        
        if(section == RIGS){
            rangeFinderRight.startMeas();
            wait_ms(38);
            rangeFinderRight.getMeas(range);
            if(range< 20){
                motors.stopBothMotors(); 
                break;
            }
        }
 
 
        //bt.printf("wall follow 2 range %f\r\n",range);
        //bt.printf("loc+location = %f\r\n", loc+location);
        if(range > 20 && !SeeWaveGap) {
                motors.setMotor0Speed(dir*MAX_SPEED - dir*pid_return);//right
                motors.setMotor1Speed(dir*MAX_SPEED);//left
                wait_ms(30);
                SeeWaveGap = true;
         } else if(range > 20 && SeeWaveGap){
            motors.stopBothMotors(); 
            bt.printf("wavegap\r\n");
            // AT WAVE OPENING!!!!
            break;
        } else {
 
        pid1.setProcessValue(range);
        pid_return = pid1.compute();
        //bt.printf("Range: %f\n      PID:   %f\r\n", range, pid_return);
 
        if(pid_return > 0) {
            if(side) {
                motors.setMotor0Speed(dir*MAX_SPEED - dir*pid_return);//right
                motors.setMotor1Speed(dir*MAX_SPEED);//left
            } else {
                motors.setMotor1Speed(dir*MAX_SPEED - dir*pid_return);//left
                motors.setMotor0Speed(dir*MAX_SPEED);//right
            }
        } else if(pid_return < 0) {
            if(side) {
                motors.setMotor0Speed(dir*MAX_SPEED);//right
                motors.setMotor1Speed(dir*MAX_SPEED + dir*pid_return);//left
            } else {
                motors.setMotor1Speed(dir*MAX_SPEED);//left
                motors.setMotor0Speed(dir*MAX_SPEED + dir*pid_return);//right
            }
        } else {
            motors.setMotor0Speed(dir*MAX_SPEED);
            motors.setMotor1Speed(dir*MAX_SPEED);
        }
    }}
    motors.stopBothMotors();
}
 
 
void alignWithWall(int section)
{
    float usValue = 0;
 
    if(section == TOOLS) {
        // turn at an angle
        leftEncoder.reset();
        rightEncoder.reset();
        motors.setMotor0Speed(-1.2*MAX_SPEED); //right
        motors.setMotor1Speed(0.4*MAX_SPEED); //left
        while(rightEncoder.getPulses()>-1000);
        motors.stopBothMotors();
 
        //go backwards toward wall
        leftEncoder.reset();
        rightEncoder.reset();
        motors.setMotor0Speed(-MAX_SPEED); //right
        motors.setMotor1Speed(-MAX_SPEED); //left
        while(abs(leftEncoder.getPulses()) < 300 || abs(rightEncoder.getPulses()) < 300);
 
        motors.stopBothMotors();
 
        // turn left towards wall
        leftEncoder.reset();
        rightEncoder.reset();
        motors.setMotor0Speed(MAX_SPEED); //right
        motors.setMotor1Speed(-MAX_SPEED); //left
        while(rightEncoder.getPulses() < 10 || abs(leftEncoder.getPulses()) < 10);
 
        motors.stopBothMotors();
 
        motors.setMotor0Speed(0.7*MAX_SPEED); //right
        motors.setMotor1Speed(-0.7*MAX_SPEED); //left
    } else {
        rightTurn();
        motors.setMotor0Speed(-0.7*MAX_SPEED); //right
        motors.setMotor1Speed(0.7*MAX_SPEED); //left
    }
 
    usValue = 0;
    while(1) {
        rangeFinderLeft.startMeas();
        wait_ms(20);
        rangeFinderLeft.getMeas(range);
        //bt.printf("Range %f \t OldValue %f\n\r",range, usValue);
        if(range > usValue && usValue != 0 && range < 25) {
            break;
        } else {
            usValue = range;
        }
    }
    motors.stopBothMotors();
}
 
void rightTurn(void)
{
    motors.begin();
    leftEncoder.reset();
    rightEncoder.reset();
    motors.setMotor0Speed(-0.5*127);//right
    motors.setMotor1Speed(0.5*127);//left
    while(leftEncoder.getPulses()<900 || rightEncoder.getPulses()>-900);
    motors.stopBothMotors();
}
 
void leftTurn(void)
{
    /*
    leftEncoder.reset();
    rightEncoder.reset();
    motors.setMotor0Speed(0.4*MAX_SPEED); //right
    motors.setMotor1Speed(-MAX_SPEED); //left
    while(abs(leftEncoder.getPulses())<2500);
    motors.stopBothMotors();
    */
    motors.begin();
    leftEncoder.reset();
    rightEncoder.reset();
    motors.setMotor0Speed(0.5*127);// right
    motors.setMotor1Speed(-0.5*127);// left
    while(abs(leftEncoder.getPulses())<1100 || rightEncoder.getPulses()<1100);
    motors.stopBothMotors();
}
 
void slightleft(void){
    
    leftEncoder.reset();
    rightEncoder.reset();
    motors.setMotor0Speed(0.5*127);// right
    motors.setMotor1Speed(-0.5*127);// left
    while(abs(leftEncoder.getPulses())<100 || rightEncoder.getPulses()<100);
    motors.stopBothMotors();
}

void UntilWall(int dir){
    
    if(dir == BACKWARD) dir=-1; 
    
    leftEncoder.reset();
    rightEncoder.reset();
    motors.setMotor0Speed(dir*0.2*127); //right
    motors.setMotor1Speed(dir*0.2*127); //left
    
    range = 30;
    
    while(range > 20){
        rangeFinderRight.startMeas();
        wait_ms(20);
        rangeFinderRight.getMeas(range);
    }
    
    motors.setMotor0Speed(dir*-0.2*127); //right
    motors.setMotor1Speed(dir*-0.2*127); //left
    wait_ms(5);
    motors.stopBothMotors();
} 
 
void overBump(int section)
{
    int preLeft=5000, preRight=5000, out=0;
    
    motors.begin();
    
    leftEncoder.reset();
    rightEncoder.reset();
    motors.setMotor0Speed(-0.2*127); //right
    motors.setMotor1Speed(-0.2*127); //left
    while(abs(leftEncoder.getPulses()) < 50 || abs(rightEncoder.getPulses())< 50);
    motors.stopBothMotors();
    
    leftEncoder.reset();
    rightEncoder.reset();
    motors.setMotor0Speed(0.2*127); //right
    motors.setMotor1Speed(0.2*127); //left
    while(/*(abs(leftEncoder.getPulses()) < 1000 || abs(rightEncoder.getPulses())< 1000)*/ IR.getDistance() >20 && preLeft!=0){
       preLeft=leftEncoder.getPulses();
       preRight=rightEncoder.getPulses();
       wait_ms(100);
       //bt.printf(" first while left %d right %d \r\n", preLeft, preRight);
       if(leftEncoder.getPulses() == preLeft || rightEncoder.getPulses()== preRight) preLeft=preRight=0;
    }
    
    motors.stopBothMotors();
    motors.begin();
    wait(2);
    /*
        motors.stopBothMotors();
        motors.setMotor0Speed(0.15*127); //right
        motors.setMotor1Speed(0.15*127); //left
        preLeft=preRight=5000 ;
        leftEncoder.reset();
        rightEncoder.reset();
     */
//   while(/*(abs(leftEncoder.getPulses()) < 1000 || abs(rightEncoder.getPulses())< 1000)*/ IR.getDistance() >20 && preLeft!=0){
    /*        preLeft=leftEncoder.getPulses();
            preRight=rightEncoder.getPulses();
            bt.printf("second while left %d right %d \r\n", preLeft, preRight);
            wait_ms(200);
            if(leftEncoder.getPulses() == preLeft || rightEncoder.getPulses()== preRight) preLeft=preRight=0;
        }*/
        
    leftEncoder.reset();
    rightEncoder.reset();
    motors.setMotor0Speed(0.3*127); //right
    motors.setMotor1Speed(0.3*127); //left
 
    while(!out) {
        preLeft=leftEncoder.getPulses();
        preRight=rightEncoder.getPulses();
 
        rangeFinderLeft.startMeas();
        rangeFinderRight.startMeas();
        wait_ms(20);
        rangeFinderLeft.getMeas(range);
        rangeFinderRight.getMeas(range2);
        if(range < 10 || range2 < 10) out=1;
 
        if(leftEncoder.getPulses() == preLeft || rightEncoder.getPulses()== preRight) {
            motors.setMotor0Speed(0.4*127); //right
            motors.setMotor1Speed(0.4*127); //left
        }
        if(abs(leftEncoder.getPulses()) >1000 || abs(leftEncoder.getPulses())>1000) out=1;
    }
 
    motors.stopBothMotors();
    wait(2);
    motors.begin();
    
    preLeft=preRight=5000 ;
    leftEncoder.reset();
    rightEncoder.reset();
    motors.setMotor0Speed(.25*127); //right
    motors.setMotor1Speed(.25*127); //left
 
    if(section == TOOLS || section == MID) {
        while(IR.getDistance() > 20 ) {
            //bt.printf("IR %f\r\n", IR.getDistance());
            //bt.printf("third while left %d right %d \r\n", preLeft, preRight);
        }
    } else while((abs(leftEncoder.getPulses()) < 200 || abs(rightEncoder.getPulses())< 200));
 
    motors.setMotor0Speed(-.25*127); //right
    motors.setMotor1Speed(-.25*127); //left
    wait_ms(10);
    motors.stopBothMotors();
    wait(2);
    motors.begin();
 
}
 
void tools_section(float* location, float &current)
{
    wall_follow(LEFT,FORWARD, TOOLS);
    // current position in reference to the starting position
    current=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
    bt.printf("current %f \r\n",current);
 
    
    motors.setMotor0Speed(-.2*127); //right
    motors.setMotor1Speed(-.2*127); //left
    wait_ms(5);
    motors.stopBothMotors();
    
    motors.setMotor0Speed(.2*127); //right
    motors.setMotor1Speed(.2*127); //left
    while(IR.getDistance()>6);
    
    motors.setMotor0Speed(-.2*127); //right
    motors.setMotor1Speed(-.2*127); //left
    wait_ms(5);
    motors.stopBothMotors();
    
 
    //Tool aquiring
    wait(2);
    // After tool is aquired
 
    alignWithWall(TOOLS);
    
    wait_ms(100);
    
    wall_follow2(LEFT,FORWARD,MID, current);
    current= 78;
    
    rangeFinderLeft.startMeas();
    wait_ms(20);
    rangeFinderLeft.getMeas(range);
 
    if(range < 20) {
        wall_follow2(LEFT,BACKWARD,TOOLS, current);
        
        location[0]= current - ((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
        current= location[0];
        
        leftEncoder.reset();
        rightEncoder.reset();
        motors.setMotor0Speed(-MAX_SPEED); //right
        motors.setMotor1Speed(-MAX_SPEED); //left
        while(abs(leftEncoder.getPulses()) < 120 || abs(rightEncoder.getPulses())< 120);
        
        leftEncoder.reset();
        rightEncoder.reset();
        motors.setMotor0Speed(MAX_SPEED); //right
        motors.setMotor1Speed(MAX_SPEED); //left
        while(abs(leftEncoder.getPulses()) < 40 || abs(rightEncoder.getPulses())< 40);
        motors.stopBothMotors();
 
        wait_ms(500);
        leftTurn();
        slightleft();
        overBump(TOOLS);
    } else {
        location[0]= 77;
        leftTurn();
        wait_ms(20);
        overBump(FIRST_WAVE);
    }
 
    bt.printf("First Wavegap = %f\r\n",location[0]);
}
 
void mid_section(float* location, float &current, int* direction)
{
 
    motors.begin();
    
    if(IR.getDistance() > 20){
        direction[0]= STRAIGHT;
        return;
    }
    
    alignWithWall(MID);
 
    bt.printf("mid section current = %f\r\n",current);
    wall_follow2(LEFT,FORWARD,MID, current);
    current=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
    bt.printf("after wf2 current = %f\r\n",current);
 
    if(current != 0) {
        direction[0]= RIGHT;
        current+= location[0];
        location[1]= current;
    } else {
        current=location[0];
        direction[0]= LEFT;
        wall_follow2(LEFT,BACKWARD,MID,current);
        location[1]= location[0]- ((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
    }
 
    bt.printf("wavegap2 = %f\r\n",location[1]);
    leftTurn();
    overBump(TOOLS);
    // go forward
    leftEncoder.reset();
    rightEncoder.reset();
    motors.setMotor0Speed(0.2*127); //right
    motors.setMotor1Speed(0.2*127); //left
    while(abs(leftEncoder.getPulses())<300 || abs(rightEncoder.getPulses())<300);
    motors.stopBothMotors();
 
}
 
void mid_section2(float* location, float &current, int* direction)
{
 
    motors.begin();
    
    if(IR.getDistance() > 20){
        direction[0]= STRAIGHT;
        return;
    }
    
    alignWithWall(MID);
    wall_follow2(LEFT,FORWARD,MID, current);
    current=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
 
    if(current != 0) {
        direction[1]= RIGHT;
        current+= location[1];
        location[2]= current;
    } else {
        current=location[1];
        direction[1]= LEFT;
        wall_follow2(LEFT,BACKWARD,MID,current);
        location[2]= location[1]- ((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
    }
 
    leftTurn();
    overBump(RIGS);
}
 
void rig_section(float* location, float &current, int* direction, int rig)
{
    float loc;
    
    if(rig == 1) loc= 16;
    else if(rig == 2) loc= 37;
    else loc = 58;
    
    rightTurn();
    
    
    if(current > loc){
        UntilWall(BACKWARD);
        current-=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
        wall_follow2(RIGHT, BACKWARD, RIGS, current);
        current-=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
        
        if((current- loc)>10){
            wall_follow2(RIGHT, BACKWARD, RIGS, current);
            current-=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
        }   
    }
    else{
        UntilWall(FORWARD);
        wall_follow2(RIGHT, FORWARD, RIGS, current);
        current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
        
        if((current- loc)<-10){
            wall_follow2(RIGHT, FORWARD, RIGS, current);
            current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
        }   
    }
    
 
}