Yeah
Dependencies: HCSR04 PID PololuQik2 QEI Sharp mbed-rtos
Fork of NavigationTest by
Diff: main.cpp
- Revision:
- 0:ff94cc47fef7
- Child:
- 1:801f0b9a862a
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Wed Mar 12 21:55:44 2014 +0000 @@ -0,0 +1,200 @@ +#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 ¤tLocation); +void wall_follow3(int ¤tLocation, 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 ¤tLocation) +{ + 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 ¤tLocation, 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); + } + } +}