Yeah
Dependencies: HCSR04 PID PololuQik2 QEI Sharp mbed-rtos
Fork of NavigationTest by
main.cpp
- Committer:
- Fairy_Paolina
- Date:
- 2014-03-13
- Revision:
- 1:801f0b9a862a
- Parent:
- 0:ff94cc47fef7
- Child:
- 2:3d0be48abcf2
File content as of revision 1:801f0b9a862a:
#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.3*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(2.0,2.0,0.0,0.02); PololuQik2 motors(p9, p10, p8, p15, errFunction, cRc); QEI leftEncoder(p17,p18,NC,1192,QEI::X4_ENCODING); //QEI rightEncoder(p19,p20,NC,1192,QEI::X4_ENCODING); //InterruptIn encoder(p29); //Functions void wall_follow(void); void wall_follow2(int *currentLocation); void wall_follow3(int ¤tLocation, int &WaveOpening); void leftTurn(void); void rightTurn(void); void us_distance(void); //Variables int main(void){ int location=0; pc.baud(115200); bt.baud(115200); motors.begin(); /*motors.setMotor0Speed(MAX_SPEED); //left motors.setMotor1Speed(MAX_SPEED); //right wait_ms(350); */ //wall_follow(); //wall_follow2(&location); pc.printf("%d\n\r",location); motors.stopBothMotors(); leftTurn(); wait(1); rightTurn(); } 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(5.75, 6); pid1.setOutputLimits( -MAX_SPEED, MAX_SPEED); pid1.setSetPoint(6.0); rangeFinder.startMeas(); wait_ms(20); if ( (rangeFinder.getMeas(range) == RANGE_MEAS_VALID) && (range < 100.0) && (range > 3.0)) { pc.printf("Range = %f\n\r", range); } pid1.setProcessValue(range); pid_return = pid1.compute(); pc.printf("Range: %f\n PID: %f\r\n", 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; int count=0; while(1){ pid1.setInputLimits(0.0, 6.0); pid1.setOutputLimits( -MAX_SPEED, MAX_SPEED); pid1.setSetPoint(6.0); rangeFinder.startMeas(); wait_ms(20); 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 60 ms the ultrasonic still sees 20+ cm */ /* then robot is at wave opening */ pc.printf("range %f\r\n",range); if(range > 20){ currentLocation++; bt.printf("saw gap \r\n"); if(SeeWaveGap){ motors.stopBothMotors(); bt.printf("wavegap\r\n"); // AT WAVE OPENING!!!! break; } SeeWaveGap = true; } pid1.setProcessValue(range); pid_return = pid1.compute(); bt.printf("Range: %f\n PID: %f\r\n", 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(5.75, 6); pid1.setOutputLimits( -MAX_SPEED, MAX_SPEED); pid1.setSetPoint(6.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); } } } void rightTurn(void) { leftEncoder.reset(); //rightEncoder.reset(); motors.setMotor0Speed(-0.4*127); motors.setMotor1Speed(0.4*127); while(leftEncoder.getPulses()<1400); motors.stopBothMotors(); } void leftTurn(void) { leftEncoder.reset(); //rightEncoder.reset(); motors.setMotor0Speed(0.4*127); motors.setMotor1Speed(-0.4*127); while(leftEncoder.getPulses()>-1500); motors.stopBothMotors(); }