File content as of revision 0:ff94cc47fef7:

#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_TRIGGER    (p12)
#define PIN_ECHO       (p11)
#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.2*127)

float range, pid_return;
void errFunction(void);
bool cRc;

//Hardware Initialization
Serial bt(p13,p14);
Serial pc(USBTX,USBRX);
HCSR04 rangeFinder( PIN_TRIGGER, PIN_ECHO );
PID pid1(1.0,0.0,0.0,0.1);
PololuQik2 motors(p9, p10, p8, p15, errFunction, cRc);

//Functions

void wall_follow(void);
void wall_follow2(int &currentLocation);
void wall_follow3(int &currentLocation, int &WaveOpening);

void us_distance(void);


int main(void){

    pc.baud(115200);
    motors.begin();
        
    //motors.setMotor0Speed(MAX_SPEED); //left
    //motors.setMotor1Speed(MAX_SPEED); //right
    //wait(10);
    
    wall_follow();
    
    motors.stopBothMotors();
    
    
}

void errFunction(void){
    //Nothing    
}

void us_distance(void)
{
        pc.printf("Ultra Sonic\n\r");
        rangeFinder.startMeas();
        wait_us(20);
        if ( (rangeFinder.getMeas(range) == RANGE_MEAS_VALID))
        {
            pc.printf("Range = %f\n\r", range);
        }  
}

void wall_follow(void)
{
    while(1){
        
        pid1.setInputLimits(1.0, 20.0);
        pid1.setOutputLimits( -0.3*127, 0.3*127);
        pid1.setSetPoint(10.0);
        
        rangeFinder.startMeas();
        wait_ms(100);
        if ( (rangeFinder.getMeas(range) == RANGE_MEAS_VALID) && (range < 100.0) && (range > 3.0))
        {
            //pc.printf("Range = %f\n", range);
        } 
        pid1.setProcessValue(range);
        pid_return = pid1.compute(); 
        pc.printf("Range: %f\n      PID:   %f", range, pid_return);
        
        if(pid_return > 0){
            motors.setMotor0Speed(MAX_SPEED - pid_return);//left
            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);
        }
     }
}

/* MODIFIED WALL_FOLLOW FOR NAVIGATION */

void wall_follow2(int &currentLocation)
{
    int SeeWaveGap = false;
    while(1){
        
        pid1.setInputLimits(4.0, 20.0);
        pid1.setOutputLimits( -0.6, 0.6);
        pid1.setSetPoint(20.0);
        
        rangeFinder.startMeas();
        wait_ms(100);
        if ( (rangeFinder.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 && !SeeWaveGap){
            currentLocation++;
            SeeWaveGap = true;
        } else {
            // 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);
        }
     }
}


/* 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(4.0, 20.0);
        pid1.setOutputLimits( -0.6, 0.6);
        pid1.setSetPoint(20.0);
        
        rangeFinder.startMeas();
        wait_ms(100);
        if ( (rangeFinder.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);
        }
     }
}