Tools Section Navigation Calibrated slightly
Dependencies: HCSR04 PID PololuQik2 QEI mbed-rtos
Fork of NavigationTest by
Diff: main.cpp
- Revision:
- 7:0b7897232e93
- Parent:
- 6:109f46d3cb96
- Child:
- 8:32ba0fad1689
diff -r 109f46d3cb96 -r 0b7897232e93 main.cpp --- a/main.cpp Thu Mar 20 17:21:28 2014 +0000 +++ b/main.cpp Fri Mar 21 00:02:51 2014 +0000 @@ -34,7 +34,7 @@ Serial pc(USBTX,USBRX); HCSR04 rangeFinderLeft( PIN_TRIGGERL, PIN_ECHOL ); HCSR04 rangeFinderRight( PIN_TRIGGERR, PIN_ECHOR ); -PID pid1(15.0,0.0,4.0,0.02); +PID pid1(15.0,0.0,10.0,0.02); PololuQik2 motors(p9, p10, p8, p15, errFunction, cRc); QEI rightEncoder(p17,p18,NC,PPR,QEI::X4_ENCODING); QEI leftEncoder(p16,p15,NC,PPR,QEI::X4_ENCODING); @@ -51,6 +51,7 @@ void us_distance(void); void tools_section(float *location, float ¤t); void overBump(void); +void alignWithWall(void); //Variables @@ -63,6 +64,66 @@ bt.baud(115200); motors.begin(); + /*****************************OLD CODE FOR GOING THROUGH WAVE SECTION****************/ + /* + leftEncoder.reset(); + rightEncoder.reset(); + wall_follow2(LEFT,FORWARD,TOOLS); + + motors.stopBothMotors(); + wait_ms(500); + leftEncoder.reset(); + rightEncoder.reset(); + + motors.setMotor0Speed(0.6*MAX_SPEED); //right + motors.setMotor1Speed(0.6*MAX_SPEED); //left + + wait_ms(200); + motors.stopBothMotors(); + + leftEncoder.reset(); + rightEncoder.reset(); + bt.printf("1st turn left\r\n"); + + motors.setMotor0Speed(MAX_SPEED); //right + motors.setMotor1Speed(-MAX_SPEED); //left + + while(((abs(leftEncoder.getPulses())/(PPR) + abs(rightEncoder.getPulses())/(PPR))/2) < 1); + + leftEncoder.reset(); + rightEncoder.reset(); + + motors.setMotor0Speed(MAX_SPEED); //right + motors.setMotor1Speed(MAX_SPEED); //left + + while(((abs(leftEncoder.getPulses())/(PPR) + abs(rightEncoder.getPulses())/PPR)/2) < 2); + + leftEncoder.reset(); + rightEncoder.reset(); + motors.setMotor0Speed(-MAX_SPEED); //right + motors.setMotor1Speed(MAX_SPEED); //left + float usValue = 0; + while(1){ + rangeFinderLeft.startMeas(); + wait_ms(100); + rangeFinderLeft.getMeas(range); + bt.printf("Range %f \t OldValue %f\n\r",range, usValue); + if(range > usValue && usValue != 0 && range < 10){ + break; + } else { + usValue = range; + } + } + motors.stopBothMotors(); + + leftEncoder.reset(); + rightEncoder.reset(); + + wall_follow2(LEFT,FORWARD,TOOLS); + + while(1){} + */ + /* // Very Consistent Turn leftEncoder.reset(); rightEncoder.reset(); @@ -81,9 +142,8 @@ usValue = range; } } - + */ motors.stopBothMotors(); - while(1){} /* leftEncoder.reset(); @@ -164,7 +224,7 @@ void us_distance(void) { - pc.printf("Ultra Sonic\n\r"); + rangeFinderLeft.startMeas(); wait_us(20); if ( (rangeFinderLeft.getMeas(range) == RANGE_MEAS_VALID)) { @@ -180,7 +240,7 @@ pid1.reset(); if(direction == BACKWARD) dir=-1; - if(section == TOOLS)set= 5; + if(section == TOOLS)set= 10; leftEncoder.reset(); rightEncoder.reset(); @@ -209,11 +269,12 @@ wavegap=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2; bt.printf("wavegap %f\r\n",wavegap); // AT WAVE OPENING!!!! + motors.setMotor1Speed(dir*MAX_SPEED);//left motors.setMotor0Speed(dir*MAX_SPEED);//right - motors.setMotor1Speed(dir*MAX_SPEED);//left - + } else { + pid1.setProcessValue(range); pid_return = pid1.compute(); @@ -239,7 +300,8 @@ motors.setMotor0Speed(dir*MAX_SPEED);//right motors.setMotor1Speed(dir*MAX_SPEED);//left } - }} + } + } return wavegap; } @@ -247,7 +309,8 @@ void wall_follow2(int side, int direction, int section) { - int SeeWaveGap = false, count=0, dir=1, set=5; + int SeeWaveGap = false, count=0, set=5; + float dir=1.0; pid1.reset(); @@ -378,7 +441,7 @@ rightEncoder.reset(); motors.setMotor0Speed(-speedR*127);//right motors.setMotor1Speed(speedL*127);//left - while(leftEncoder.getPulses()<1050 || rightEncoder.getPulses()>-1050); + while(leftEncoder.getPulses()<900 || rightEncoder.getPulses()>-900); motors.stopBothMotors(); } @@ -388,7 +451,7 @@ rightEncoder.reset(); motors.setMotor0Speed(0.4*127);// right motors.setMotor1Speed(-0.4*127);// left - while(leftEncoder.getPulses()>-1200 || rightEncoder.getPulses()<1200); + while(leftEncoder.getPulses()>-940 || rightEncoder.getPulses()<940); motors.stopBothMotors(); } @@ -408,24 +471,90 @@ */ motors.stopBothMotors(); } +void alignWithWall(void){ + + leftEncoder.reset(); + rightEncoder.reset(); + + motors.setMotor0Speed(-MAX_SPEED); //right + motors.setMotor1Speed(0.4*MAX_SPEED); //left + while(rightEncoder.getPulses()>-1000); + motors.stopBothMotors(); + + leftEncoder.reset(); + rightEncoder.reset(); + + motors.setMotor0Speed(-MAX_SPEED); //right + motors.setMotor1Speed(-MAX_SPEED); //left + while(abs(leftEncoder.getPulses()*11.12/PPR) < 3); + + motors.setMotor0Speed(0.1*MAX_SPEED); //right + motors.setMotor1Speed(0.1*MAX_SPEED); //left + + wait_ms(200); + + motors.stopBothMotors(); + + leftEncoder.reset(); + rightEncoder.reset(); + + motors.setMotor0Speed(MAX_SPEED); //right + motors.setMotor1Speed(-MAX_SPEED); //left + while(rightEncoder.getPulses() < 260); + + motors.setMotor0Speed(0.7*MAX_SPEED); //right + motors.setMotor1Speed(-0.7*MAX_SPEED); //left + float usValue = 0; + while(1){ + rangeFinderLeft.startMeas(); + wait_ms(100); + rangeFinderLeft.getMeas(range); + bt.printf("Range %f \t OldValue %f\n\r",range, usValue); + if(range > usValue && usValue != 0 && range < 25){ + break; + } else { + usValue = range; + } + } + + + + +} + void tools_section(float *location, float ¤t){ - location[0]=wall_follow(LEFT,FORWARD, TOOLS); //location from the left edge of the field + //location[0]=wall_follow(LEFT,FORWARD, TOOLS); //location from the left edge of the field + //Too Far from wall to see gap :( + wall_follow(LEFT,FORWARD,TOOLS); + current=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2; - bt.printf("left Encoder : %d \t right Encoder %d \r\n",leftEncoder.getPulses(),rightEncoder.getPulses()); bt.printf("wavegap %f \t current %f \r\n",location[0],current); motors.stopBothMotors(); ////////////////////////////////////////// determine shape and pick up tool /////////////////////////////////////////////////////// - - if(current >location[0]){ + alignWithWall(); + wait_ms(200); + + rangeFinderLeft.startMeas(); + wait_ms(100); + rangeFinderLeft.getMeas(range); + if(range < 10){ + wall_follow2(LEFT,BACKWARD,TOOLS); - wait_ms(100); + + leftEncoder.reset(); + rightEncoder.reset(); + motors.setMotor0Speed(-0.9*MAX_SPEED); //right + motors.setMotor1Speed(-0.9*MAX_SPEED); //left + while(abs(leftEncoder.getPulses()*11.12/PPR) < 2); + motors.stopBothMotors(); + // forward - /* leftEncoder.reset(); + /* leftEncoder.reset(); rightEncoder.reset(); motors.setMotor0Speed(-MAX_SPEED); //right motors.setMotor1Speed(-MAX_SPEED); //left @@ -434,8 +563,7 @@ wait_ms(500);*/ current = (abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2; // REMOVED += because current should just be current pulses? - } - else{/* + /* wall_follow2(LEFT,FORWARD); // backward leftEncoder.reset(); @@ -445,12 +573,18 @@ while(abs(leftEncoder.getPulses())<300 || abs(rightEncoder.getPulses())<300); motors.stopBothMotors(); current-=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;*/ - } + leftTurn(); //Go over overBump(); + } else { + + leftTurn(); + //Go over + overBump(); + } } \ No newline at end of file