File content as of revision 2:3d0be48abcf2:

#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 PPRL           (965)
#define PPRR           (1075)
#define LEFT           (1)
#define RIGHT          (0)

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,PPRR,QEI::X4_ENCODING);
QEI leftEncoder(p16,p15,NC,PPRR,QEI::X4_ENCODING);
//InterruptIn encoder(p29);


//Functions

void wall_follow(int side);
void wall_follow2(int side);
void wall_follow3(int &currentLocation, int &WaveOpening);
void leftTurn(void);
void rightTurn(void);
void us_distance(void);

//Variables

int main(void)
{
    float location=0;

    pc.baud(115200);
    bt.baud(115200);
    motors.begin();
    
    //motors.setMotor0Speed(MAX_SPEED); //right
    //motors.setMotor1Speed(MAX_SPEED); //left
    
    //motors.stopBothMotors();


    //wall_follow(RIGHT);
    leftEncoder.reset();
    rightEncoder.reset();
    wall_follow2(LEFT);
    location=(abs(leftEncoder.getPulses()*11.12/PPRL) + abs(rightEncoder.getPulses()*11.12/PPRR))/2;
     
    leftEncoder.reset();
    rightEncoder.reset();
    motors.setMotor0Speed(MAX_SPEED); //right
    motors.setMotor1Speed(MAX_SPEED); //left
    while(leftEncoder.getPulses()<300 || rightEncoder.getPulses()<300);
    motors.stopBothMotors();
    
    leftTurn();
    
    leftEncoder.reset();
    rightEncoder.reset();
    motors.setMotor0Speed(MAX_SPEED); //right
    motors.setMotor1Speed(MAX_SPEED); //left
    while(abs(leftEncoder.getPulses()*11.12/PPRL) + abs(rightEncoder.getPulses()*11.12/PPRR)/2 <30);
    motors.stopBothMotors();
    
    leftTurn();
    wall_follow2(RIGHT);
    rightTurn();
    


    bt.printf("LOCATION %f\n\r",location);

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

void wall_follow(int side)
{
    while(1) {

        pid1.setInputLimits(0, 5.0);
        pid1.setOutputLimits( -MAX_SPEED, MAX_SPEED);
        pid1.setSetPoint(5.0);
        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);
        }
        
        pid1.setProcessValue(range);
        pid_return = pid1.compute();

        if(pid_return > 0) {
            if(side){
                motors.setMotor0Speed(MAX_SPEED - pid_return);//right
                motors.setMotor1Speed(MAX_SPEED);//left
            }
            else{
                motors.setMotor1Speed(MAX_SPEED - pid_return);//left
                motors.setMotor0Speed(MAX_SPEED);//right
            }
        }else if(pid_return < 0) {
            if(side){
                motors.setMotor0Speed(MAX_SPEED);//right
                motors.setMotor1Speed(MAX_SPEED + pid_return);//left
            }
            else{
                motors.setMotor1Speed(MAX_SPEED);//left
                motors.setMotor0Speed(MAX_SPEED + pid_return);//right
            }
        }else {
            motors.setMotor0Speed(MAX_SPEED);//right
            motors.setMotor1Speed(MAX_SPEED);//left
        }
    }
}

/* MODIFIED WALL_FOLLOW FOR NAVIGATION */

void wall_follow2(int side)
{
    int SeeWaveGap = false;
    int count=0;

    while(1) {

        pid1.setInputLimits(0.0, 5.0);
        pid1.setOutputLimits( -MAX_SPEED, MAX_SPEED);
        pid1.setSetPoint(5.0);

        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 60 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(MAX_SPEED - pid_return);//right
                motors.setMotor1Speed(MAX_SPEED);//left
            }
            else{
                motors.setMotor1Speed(MAX_SPEED - pid_return);//left
                motors.setMotor0Speed(MAX_SPEED);//right
            }
        }else if(pid_return < 0) {
            if(side){
                motors.setMotor0Speed(MAX_SPEED);//right
                motors.setMotor1Speed(MAX_SPEED + pid_return);//left
            }
            else{
                motors.setMotor1Speed(MAX_SPEED);//left
                motors.setMotor0Speed(MAX_SPEED + pid_return);//right
            }
        } else {
            motors.setMotor0Speed(MAX_SPEED);
            motors.setMotor1Speed(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 rightTurn(void)
{
    float speedL, speedR;

    speedL=speedR= 0.4;

    leftEncoder.reset();
    rightEncoder.reset();
    motors.setMotor0Speed(-speedR*127);//right
    motors.setMotor1Speed(speedL*127);//left
    while(leftEncoder.getPulses()<1050 || rightEncoder.getPulses()>-1050);
    motors.stopBothMotors();
}

void leftTurn(void)
{
    leftEncoder.reset();
    rightEncoder.reset();
    motors.setMotor0Speed(0.4*127);// right
    motors.setMotor1Speed(-0.4*127);// left
    while(leftEncoder.getPulses()>-1200 || rightEncoder.getPulses()<1200);
    motors.stopBothMotors();
}