File content as of revision 5:70ccef3734ae:

#include "rtos.h"
#include "PID.h"
#include "PololuQik2.h"
#include "QEI.h"
#include "mbed.h"
#include "HCSR04.h"
#include "stdio.h"
#include "LPC17xx.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 FORWARD        (1)
#define BACKWARD       (0) 
#define TOOLS          (0)
#define MID            (1)
#define RIGS           (2)  
 
float range, 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);
//InterruptIn encoder(p29);
 
 
//Functions
 
float wall_follow(int side, int direction, int section);
void wall_follow2(int side, int direction, int section);
void wall_follow3(int &currentLocation, int &WaveOpening);
void leftTurn(void);
void rightTurn(void);
void us_distance(void);
void tools_section(float* location, float &current);
void overBump(void);
void align(float speed);
 
//Variables
 
int main(void)
{
    float location[3], current=0;
    int direction[3];
 
    pc.baud(115200);
    bt.baud(115200);
    motors.begin();
    
    
    //leftEncoder.reset();
    //rightEncoder.reset();
    //motors.setMotor0Speed(MAX_SPEED); //right
    //motors.setMotor1Speed(MAX_SPEED); //left
    
    //while((abs(leftEncoder.getPulses())/(PPR) + abs(rightEncoder.getPulses())/(PPR))/2 < 3)
 
    
    
    //Go to tools
    tools_section(location, current);
   
    //////////////////////////////// without predefined wavegaps//////////////////////////////////////////////
 /*   current=0;
    if(location[0]< 75){
        rightTurn();
        current=wall_follow(LEFT,FORWARD,MID);
        if(current != 0) direction[0]= RIGHT;
        else{
            direction[0]= LEFT;
            wall_follow2(LEFT,BACKWARD,MID);
            location[1]=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
            
            leftEncoder.reset();
            rightEncoder.reset();
            motors.setMotor0Speed(-MAX_SPEED); //right
            motors.setMotor1Speed(-MAX_SPEED); //left
            while(abs(leftEncoder.getPulses())<75 || abs(rightEncoder.getPulses())<75);
            motors.stopBothMotors();
        }
        leftTurn();
    }
    else{  
        leftTurn();
        direction[0]= LEFT;
        wall_follow2(RIGHT,FORWARD,MID);
        location[1]=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;  
        rightTurn();
    }
        
    overBump();
    
    */
    
    
    // left or right
    
    
    
    
    
    
     
    
    
    //leftTurn();
    //wall_follow2(RIGHT);
   // rightTurn();
    
 
 
    bt.printf("LOCATION %f\n\r",location);
 
    //overBump();
    //motors.stopBothMotors();
   //leftTurn();
   //wait(1);
   //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< 75) {
        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(20);
            rangeFinderLeft.getMeas(range);
        }
        else{
            rangeFinderRight.startMeas();
            wait_ms(20);
            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!!!!
        }
        
        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)
{
    int SeeWaveGap = false, dir=1;
    float set=5;
    
    pid1.reset();
    
    if(direction == BACKWARD) dir=-1;
    if(section == TOOLS)set= 6.5;
    
    leftEncoder.reset();
    rightEncoder.reset();
 
    while(1) {
 
        pid1.setInputLimits(0.0, set);
        pid1.setOutputLimits( -MAX_SPEED, MAX_SPEED);
        pid1.setSetPoint(set);
 
        if(side){
            rangeFinderLeft.startMeas();
            wait_ms(20);
            rangeFinderLeft.getMeas(range);
        }
        else{
            rangeFinderRight.startMeas();
            wait_ms(20);
            rangeFinderRight.getMeas(range);
        }
 
 
        /*************CHECK FOR WAVE OPENING*****************/
        /* If after 20 ms the ultrasonic still sees 20+ cm */
        /*      then robot is at wave opening               */
 
        pc.printf("range %f\r\n",range);
        if(range > 20) {
            motors.stopBothMotors();
            bt.printf("wavegap\r\n");
            // AT WAVE OPENING!!!!
            break;
        }
 
        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);
        }
    }
}
 
 
/* MODIFIED WALL_FOLLOW FOR NAVIGATION WITH WAVE OPENINGS PASSED IN */
/* MEANT FOR RETURNING FROM OIL RIGS */
 
void wall_follow3(int &currentLocation, int &WaveOpening)
{
    while(1) {
 
 
        pid1.setInputLimits(0, 5);
        pid1.setOutputLimits( -MAX_SPEED, MAX_SPEED);
        pid1.setSetPoint(5.0);
 
        rangeFinderLeft.startMeas();
        wait_ms(100);
        if ( (rangeFinderLeft.getMeas(range) == RANGE_MEAS_VALID) && (range < 100.0) && (range > 3.0)) {
            //bt.printf("Range = %f\n", range);
        }
 
        /*************CHECK FOR WAVE OPENING*****************/
        /* If after 100 ms the ultrasonic still sees 20+ cm */
        /*      then robot is at wave opening               */
 
 
        if(range > 20 ) {
            currentLocation--;
        }
 
        if( currentLocation == WaveOpening) {
            // AT WAVE OPENING!!!!
 
            break;
        }
 
 
        pid1.setProcessValue(range);
        pid_return = pid1.compute();
        bt.printf("Range: %f\n      PID:   %f", range, pid_return);
 
        if(pid_return > 0) {
            motors.setMotor0Speed(MAX_SPEED - pid_return);
            motors.setMotor1Speed(MAX_SPEED);
        } else if(pid_return < 0) {
            motors.setMotor0Speed(MAX_SPEED);
            motors.setMotor1Speed(MAX_SPEED + pid_return);
        } else {
            motors.setMotor0Speed(MAX_SPEED);
            motors.setMotor1Speed(MAX_SPEED);
        }
    }
}
 
void align(float speed){
    float left, right;  
    left=right=speed;
    
    if(abs(leftEncoder.getPulses())> rightEncoder.getPulses()){
        if(speed>0)left-=.01;
        else left+=.01;
    }
    else{
        if(speed>0) right+=.01;
        else right-=.01;
    }
    motors.setMotor0Speed(right*127);//right
    motors.setMotor1Speed(left*127);//left
    pc.printf("speed left %f right %f\r\n\n",left, right);
    
}
 
void rightTurn(void)
{
    leftEncoder.reset();
    rightEncoder.reset();
    motors.setMotor0Speed(-0.4*127);//right
    motors.setMotor1Speed(0.4*127);//left
    while(leftEncoder.getPulses()<1030 || rightEncoder.getPulses()>-1030);
    motors.stopBothMotors();
}
 
void leftTurn(void)
{ 
    leftEncoder.reset();
    rightEncoder.reset();
    motors.setMotor0Speed(0.4*127);// right
    motors.setMotor1Speed(-0.4*127);// left
    while(abs(leftEncoder.getPulses())<1120 || rightEncoder.getPulses()<1120); //align(0.4);
    motors.stopBothMotors();
}
 
void slightRight(void)
{
    leftEncoder.reset();
    rightEncoder.reset();
    motors.setMotor0Speed(-0.4*127);//right
    motors.setMotor1Speed(0.4*127);//left
    while(leftEncoder.getPulses()<515 || rightEncoder.getPulses()>-515);
    motors.stopBothMotors();
}
void slightLeft(void)
{ 
    leftEncoder.reset();
    rightEncoder.reset();
    motors.setMotor0Speed(0.4*127);// right
    motors.setMotor1Speed(-0.4*127);// left
    while(abs(leftEncoder.getPulses())<400 || rightEncoder.getPulses()< 400); //align(0.4);
    motors.stopBothMotors();
}

void overBump(void){
    
    leftEncoder.reset();
    rightEncoder.reset();
    motors.setMotor0Speed(MAX_SPEED); //right
    motors.setMotor1Speed(MAX_SPEED); //left
    while(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR)/2 <9)//align(MAX_SPEED);
    
    leftEncoder.reset();
    rightEncoder.reset();
    motors.setMotor0Speed(MAX_SPEED); //right
    motors.setMotor1Speed(MAX_SPEED); //left
    while(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR)/2 <6);
    
    motors.stopBothMotors();
}
 
void tools_section(float* location, float &current){
    
    location[0]=wall_follow(LEFT,FORWARD, TOOLS); //location from the left edge of the field
    current=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
    bt.printf("wavegap %f \t current %f \r\n",location[0],current);
    
    motors.stopBothMotors();
    
    slightRight();
    //backward
    leftEncoder.reset();
    rightEncoder.reset();
    motors.setMotor0Speed(-MAX_SPEED);//right
    motors.setMotor1Speed(-MAX_SPEED);//left
    while(abs(leftEncoder.getPulses())<500 || abs(rightEncoder.getPulses())<500);
    motors.stopBothMotors();
    
    slightLeft();
    
    /*leftEncoder.reset();
    rightEncoder.reset();
    motors.setMotor0Speed(MAX_SPEED);//right
    motors.setMotor1Speed(MAX_SPEED);//left
    while(abs(leftEncoder.getPulses())<100 || abs(rightEncoder.getPulses())<100);*/
    
    
    
    ////////////////////////////////////////// determine shape and pick up tool ///////////////////////////////////////////////////////
    
    wall_follow2(LEFT,BACKWARD,TOOLS);
    /*
    if(current >location[0]){
        wall_follow2(LEFT,BACKWARD,TOOLS);
        wait_ms(1000);
        // back 
        leftEncoder.reset();
        rightEncoder.reset();
        motors.setMotor0Speed(-MAX_SPEED); //right
        motors.setMotor1Speed(-MAX_SPEED); //left
        while(abs(leftEncoder.getPulses())<75 || abs(rightEncoder.getPulses())<75);
        motors.stopBothMotors();
        current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
    }
    else{
        wall_follow2(LEFT,FORWARD,TOOLS);   
        // backward 
        wait_ms(1000);
        leftEncoder.reset();
        rightEncoder.reset();
        motors.setMotor0Speed(-MAX_SPEED); //right
        motors.setMotor1Speed(-MAX_SPEED); //left
        while(abs(leftEncoder.getPulses())<150 || abs(rightEncoder.getPulses())<150);
        motors.stopBothMotors();
        current-=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
    }*/
    wait_ms(1000);
    leftTurn();
    
    //Go over 
    overBump();
    
}