Tools Section Navigation Calibrated slightly
Dependencies: HCSR04 PID PololuQik2 QEI mbed-rtos
Fork of NavigationTest by
main.cpp
- Committer:
- Fairy_Paolina
- Date:
- 2014-03-12
- Revision:
- 0:ff94cc47fef7
- Child:
- 1:801f0b9a862a
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 ¤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); } } }