Yeah
Dependencies: HCSR04 PID PololuQik2 QEI Sharp mbed-rtos
Fork of NavigationTest by
Diff: main.cpp
- Revision:
- 1:801f0b9a862a
- Parent:
- 0:ff94cc47fef7
- Child:
- 2:3d0be48abcf2
--- a/main.cpp Wed Mar 12 21:55:44 2014 +0000 +++ b/main.cpp Thu Mar 13 20:45:56 2014 +0000 @@ -13,7 +13,7 @@ #define WHEEL_CIRCUM (12.56637) #define DIST_PER_PULSE (0.01054225722682) #define MTRS_TO_INCH (39.3701) -#define MAX_SPEED (0.2*127) +#define MAX_SPEED (0.3*127) float range, pid_return; void errFunction(void); @@ -23,31 +23,43 @@ Serial bt(p13,p14); Serial pc(USBTX,USBRX); HCSR04 rangeFinder( PIN_TRIGGER, PIN_ECHO ); -PID pid1(1.0,0.0,0.0,0.1); +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 ¤tLocation); +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); - motors.begin(); + bt.baud(115200); + motors.begin(); - //motors.setMotor0Speed(MAX_SPEED); //left - //motors.setMotor1Speed(MAX_SPEED); //right - //wait(10); - - wall_follow(); + /*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(); } @@ -70,19 +82,19 @@ { while(1){ - pid1.setInputLimits(1.0, 20.0); - pid1.setOutputLimits( -0.3*127, 0.3*127); - pid1.setSetPoint(10.0); + pid1.setInputLimits(5.75, 6); + pid1.setOutputLimits( -MAX_SPEED, MAX_SPEED); + pid1.setSetPoint(6.0); rangeFinder.startMeas(); - wait_ms(100); + wait_ms(20); if ( (rangeFinder.getMeas(range) == RANGE_MEAS_VALID) && (range < 100.0) && (range > 3.0)) { - //pc.printf("Range = %f\n", range); + pc.printf("Range = %f\n\r", range); } pid1.setProcessValue(range); pid_return = pid1.compute(); - pc.printf("Range: %f\n PID: %f", range, pid_return); + pc.printf("Range: %f\n PID: %f\r\n", range, pid_return); if(pid_return > 0){ motors.setMotor0Speed(MAX_SPEED - pid_return);//left @@ -99,39 +111,48 @@ /* MODIFIED WALL_FOLLOW FOR NAVIGATION */ -void wall_follow2(int ¤tLocation) +void wall_follow2(int *currentLocation) { int SeeWaveGap = false; + int count=0; + while(1){ - pid1.setInputLimits(4.0, 20.0); - pid1.setOutputLimits( -0.6, 0.6); - pid1.setSetPoint(20.0); + pid1.setInputLimits(0.0, 6.0); + pid1.setOutputLimits( -MAX_SPEED, MAX_SPEED); + pid1.setSetPoint(6.0); rangeFinder.startMeas(); - wait_ms(100); + wait_ms(20); if ( (rangeFinder.getMeas(range) == RANGE_MEAS_VALID) && (range < 100.0) && (range > 3.0)) { - //bt.printf("Range = %f\n", range); + bt.printf("Range = %f\n", range); } /*************CHECK FOR WAVE OPENING*****************/ - /* If after 100 ms the ultrasonic still sees 20+ cm */ + /* If after 60 ms the ultrasonic still sees 20+ cm */ /* then robot is at wave opening */ - - if(range > 20 && !SeeWaveGap){ + 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; - } else { - // AT WAVE OPENING!!!! - break; } + pid1.setProcessValue(range); pid_return = pid1.compute(); - bt.printf("Range: %f\n PID: %f", range, pid_return); + bt.printf("Range: %f\n PID: %f\r\n", range, pid_return); if(pid_return > 0){ motors.setMotor0Speed(MAX_SPEED - pid_return); @@ -155,9 +176,9 @@ while(1){ - pid1.setInputLimits(4.0, 20.0); - pid1.setOutputLimits( -0.6, 0.6); - pid1.setSetPoint(20.0); + pid1.setInputLimits(5.75, 6); + pid1.setOutputLimits( -MAX_SPEED, MAX_SPEED); + pid1.setSetPoint(6.0); rangeFinder.startMeas(); wait_ms(100); @@ -198,3 +219,23 @@ } } } + +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(); +}