revise
Dependencies: HCSR04 PID PololuQik2 QEI mbed-rtos Sharp
Diff: main.cpp
- Revision:
- 15:a467af795e57
- Parent:
- 14:3c8c4efe4786
- Child:
- 16:0888096bff60
diff -r 3c8c4efe4786 -r a467af795e57 main.cpp --- a/main.cpp Sun Mar 30 18:21:06 2014 +0000 +++ b/main.cpp Tue Apr 01 15:42:03 2014 +0000 @@ -147,7 +147,7 @@ location=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2; - while(location< 69) { + while(location< 66.5) { location=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2; pid1.setInputLimits(0, set); @@ -196,6 +196,12 @@ } } } + + //STOP + motors.setMotor0Speed(dir*-0.3*127); //right + motors.setMotor1Speed(dir*-0.3*127); //left + wait_ms(10); + motors.stopBothMotors(); } /* MODIFIED WALL_FOLLOW FOR NAVIGATION */ @@ -643,18 +649,18 @@ void tools_section(float* location, float ¤t) { wall_follow(LEFT,FORWARD, TOOLS); - //HARD STOP - motors.setMotor0Speed(-.3*127); //right - motors.setMotor1Speed(-.3*127); //left - wait_ms(5); - motors.stopBothMotors(); // current position in reference to the starting position current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2; - pc.printf("current %f \r\n",current); + + //////////////////////////////// determine tool + wait(2); + /////////////////////////////////////////////////////////////////////////////////////// + // Move Forward + slightMove(FORWARD, 100); - //Tool aquiring + //////////////////////////////////////////Tool aquiring wait(2); - // After tool is aquired + //////////////////////////////////////////////////////////////////// After tool is aquired alignWithWall(TOOLS); pc.printf("align\r\n");