Yeah
Dependencies: HCSR04 PID PololuQik2 QEI Sharp mbed-rtos
Fork of NavigationTest by
Diff: main.cpp
- Revision:
- 2:3d0be48abcf2
- Parent:
- 1:801f0b9a862a
- Child:
- 3:58726d2e11f0
--- a/main.cpp Thu Mar 13 20:45:56 2014 +0000 +++ b/main.cpp Sat Mar 15 22:37:06 2014 +0000 @@ -7,13 +7,19 @@ #include "stdio.h" #include "LPC17xx.h" -#define PIN_TRIGGER (p12) -#define PIN_ECHO (p11) +#define PIN_TRIGGERL (p12) +#define PIN_ECHOL (p11) +#define PIN_TRIGGERR (p29) +#define PIN_ECHOR (p30) #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) +#define PPRL (965) +#define PPRR (1075) +#define LEFT (1) +#define RIGHT (0) float range, pid_return; void errFunction(void); @@ -22,18 +28,19 @@ //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); +HCSR04 rangeFinderLeft( PIN_TRIGGERL, PIN_ECHOL ); +HCSR04 rangeFinderRight( PIN_TRIGGERR, PIN_ECHOR ); +PID pid1(15.0,0.0,4.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); +QEI rightEncoder(p17,p18,NC,PPRR,QEI::X4_ENCODING); +QEI leftEncoder(p16,p15,NC,PPRR,QEI::X4_ENCODING); //InterruptIn encoder(p29); //Functions -void wall_follow(void); -void wall_follow2(int *currentLocation); +void wall_follow(int side); +void wall_follow2(int side); void wall_follow3(int ¤tLocation, int &WaveOpening); void leftTurn(void); void rightTurn(void); @@ -41,130 +48,184 @@ //Variables -int main(void){ - int location=0; +int main(void) +{ + float 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.begin(); + + //motors.setMotor0Speed(MAX_SPEED); //right + //motors.setMotor1Speed(MAX_SPEED); //left + //motors.stopBothMotors(); + + + //wall_follow(RIGHT); + leftEncoder.reset(); + rightEncoder.reset(); + wall_follow2(LEFT); + location=(abs(leftEncoder.getPulses()*11.12/PPRL) + abs(rightEncoder.getPulses()*11.12/PPRR))/2; + + leftEncoder.reset(); + rightEncoder.reset(); + motors.setMotor0Speed(MAX_SPEED); //right + motors.setMotor1Speed(MAX_SPEED); //left + while(leftEncoder.getPulses()<300 || rightEncoder.getPulses()<300); motors.stopBothMotors(); + leftTurn(); - wait(1); + + leftEncoder.reset(); + rightEncoder.reset(); + motors.setMotor0Speed(MAX_SPEED); //right + motors.setMotor1Speed(MAX_SPEED); //left + while(abs(leftEncoder.getPulses()*11.12/PPRL) + abs(rightEncoder.getPulses()*11.12/PPRR)/2 <30); + motors.stopBothMotors(); + + leftTurn(); + wall_follow2(RIGHT); rightTurn(); + + + bt.printf("LOCATION %f\n\r",location); + + motors.stopBothMotors(); +// leftTurn(); +// wait(1); +// rightTurn(); + + } -void errFunction(void){ - //Nothing +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); - } + pc.printf("Ultra Sonic\n\r"); + rangeFinderLeft.startMeas(); + wait_us(20); + if ( (rangeFinderLeft.getMeas(range) == RANGE_MEAS_VALID)) { + pc.printf("Range = %f\n\r", range); + } } -void wall_follow(void) +void wall_follow(int side) { - while(1){ - - pid1.setInputLimits(5.75, 6); + while(1) { + + pid1.setInputLimits(0, 5.0); pid1.setOutputLimits( -MAX_SPEED, MAX_SPEED); - pid1.setSetPoint(6.0); + pid1.setSetPoint(5.0); + if(side){ + rangeFinderLeft.startMeas(); + wait_ms(20); + rangeFinderLeft.getMeas(range); + } + else{ + rangeFinderRight.startMeas(); + wait_ms(20); + rangeFinderRight.getMeas(range); + pc.printf("%d\r\n",range); + } - 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); + pid_return = pid1.compute(); + + if(pid_return > 0) { + if(side){ + motors.setMotor0Speed(MAX_SPEED - pid_return);//right + motors.setMotor1Speed(MAX_SPEED);//left + } + else{ + motors.setMotor1Speed(MAX_SPEED - pid_return);//left + motors.setMotor0Speed(MAX_SPEED);//right + } + }else if(pid_return < 0) { + if(side){ + motors.setMotor0Speed(MAX_SPEED);//right + motors.setMotor1Speed(MAX_SPEED + pid_return);//left + } + else{ + motors.setMotor1Speed(MAX_SPEED);//left + motors.setMotor0Speed(MAX_SPEED + pid_return);//right + } + }else { + motors.setMotor0Speed(MAX_SPEED);//right + motors.setMotor1Speed(MAX_SPEED);//left } - } + } } /* MODIFIED WALL_FOLLOW FOR NAVIGATION */ -void wall_follow2(int *currentLocation) +void wall_follow2(int side) { int SeeWaveGap = false; int count=0; - - while(1){ - - pid1.setInputLimits(0.0, 6.0); + + while(1) { + + pid1.setInputLimits(0.0, 5.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); - } - + pid1.setSetPoint(5.0); + + if(side){ + rangeFinderLeft.startMeas(); + wait_ms(20); + rangeFinderLeft.getMeas(range); + } + else{ + rangeFinderRight.startMeas(); + wait_ms(20); + rangeFinderRight.getMeas(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; + if(range > 20) { + motors.stopBothMotors(); + bt.printf("wavegap\r\n"); + // AT WAVE OPENING!!!! + break; } - - - + 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{ + pid_return = pid1.compute(); + //bt.printf("Range: %f\n PID: %f\r\n", range, pid_return); + + if(pid_return > 0) { + if(side){ + motors.setMotor0Speed(MAX_SPEED - pid_return);//right + motors.setMotor1Speed(MAX_SPEED);//left + } + else{ + motors.setMotor1Speed(MAX_SPEED - pid_return);//left + motors.setMotor0Speed(MAX_SPEED);//right + } + }else if(pid_return < 0) { + if(side){ + motors.setMotor0Speed(MAX_SPEED);//right + motors.setMotor1Speed(MAX_SPEED + pid_return);//left + } + else{ + motors.setMotor1Speed(MAX_SPEED);//left + motors.setMotor0Speed(MAX_SPEED + pid_return);//right + } + } else { motors.setMotor0Speed(MAX_SPEED); motors.setMotor1Speed(MAX_SPEED); } - } + } } @@ -173,69 +234,72 @@ void wall_follow3(int ¤tLocation, int &WaveOpening) { - while(1){ - - - pid1.setInputLimits(5.75, 6); + while(1) { + + + pid1.setInputLimits(0, 5); pid1.setOutputLimits( -MAX_SPEED, MAX_SPEED); - pid1.setSetPoint(6.0); - - rangeFinder.startMeas(); + pid1.setSetPoint(5.0); + + rangeFinderLeft.startMeas(); wait_ms(100); - if ( (rangeFinder.getMeas(range) == RANGE_MEAS_VALID) && (range < 100.0) && (range > 3.0)) - { + if ( (rangeFinderLeft.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 ){ + + + if(range > 20 ) { currentLocation--; } - - if( currentLocation == WaveOpening){ + + if( currentLocation == WaveOpening) { // AT WAVE OPENING!!!! - + break; } - - + + pid1.setProcessValue(range); - pid_return = pid1.compute(); + pid_return = pid1.compute(); bt.printf("Range: %f\n PID: %f", range, pid_return); - - if(pid_return > 0){ + + if(pid_return > 0) { motors.setMotor0Speed(MAX_SPEED - pid_return); motors.setMotor1Speed(MAX_SPEED); - }else if(pid_return < 0){ + } else if(pid_return < 0) { motors.setMotor0Speed(MAX_SPEED); motors.setMotor1Speed(MAX_SPEED + pid_return); - }else{ + } else { motors.setMotor0Speed(MAX_SPEED); motors.setMotor1Speed(MAX_SPEED); } - } + } } void rightTurn(void) { + float speedL, speedR; + + speedL=speedR= 0.4; + leftEncoder.reset(); - //rightEncoder.reset(); - motors.setMotor0Speed(-0.4*127); - motors.setMotor1Speed(0.4*127); - while(leftEncoder.getPulses()<1400); + rightEncoder.reset(); + motors.setMotor0Speed(-speedR*127);//right + motors.setMotor1Speed(speedL*127);//left + while(leftEncoder.getPulses()<1050 || rightEncoder.getPulses()>-1050); motors.stopBothMotors(); } void leftTurn(void) { leftEncoder.reset(); - //rightEncoder.reset(); - motors.setMotor0Speed(0.4*127); - motors.setMotor1Speed(-0.4*127); - while(leftEncoder.getPulses()>-1500); + rightEncoder.reset(); + motors.setMotor0Speed(0.4*127);// right + motors.setMotor1Speed(-0.4*127);// left + while(leftEncoder.getPulses()>-1200 || rightEncoder.getPulses()<1200); motors.stopBothMotors(); }