uyvug
Dependencies: Adafruit-16-Ch-PWM-Servo-Driver HCSR04 PID PololuQik2 QEI Sharp mbed-rtos
Fork of theRobot by
Diff: main.cpp
- Revision:
- 34:cd4980573159
- Parent:
- 33:a41981e18a7d
diff -r a41981e18a7d -r cd4980573159 main.cpp --- a/main.cpp Tue Apr 22 15:24:56 2014 +0000 +++ b/main.cpp Thu Apr 24 23:20:43 2014 +0000 @@ -167,7 +167,8 @@ void alignWithWall(int section); void UntilWall(int dir); void alignWithGap(void); -void alignWithRig(void); +void toRig(void); +void alignWithRigWall(float ¤t); void testSensors(void); float normd(int* pop, int count, int threshold); @@ -329,7 +330,8 @@ **************************************************/ case START : myled1 = 1; - + //current=16; + //state=NAVIGATE_WAVES_ROW3; current=68; state = NAVIGATE_WAVES_ROW1; //state = OILRIG1_POS; @@ -747,7 +749,8 @@ * **************************************************/ case RETURN_TO_START: - wait(3); + wait(5); + while(1); rig_section_return(location, current, direction); mid_section2_return(location, current, direction); mid_section_return(location, current, direction); @@ -1524,7 +1527,8 @@ motors.setMotor1Speed(-0.5*127);// left while(abs(leftEncoder.getPulses())<1000 || rightEncoder.getPulses()<1000); motors.stopBothMotors(127); - + + slightMove(BACKWARD,50); overBump(RIGS); //pc.printf("overbump rigs\r\n"); } @@ -1545,26 +1549,30 @@ wait_ms(300); // Slight forward for turn - slightMove(FORWARD,150); + slightMove(FORWARD,100); wait_ms(100); - leftTurn(); + //RIGHT TURN + alignWithRigWall(current); + if(current <80) current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); + else current-=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); if(current > loc) { //pc.printf("RIG section %f\r\n",current); - wall_follow2(LEFT, FORWARD, RIGS, current, rig); + wall_follow2(RIGHT, BACKWARD, RIGS, current, rig); current-=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); } else { //pc.printf("RIG section %f\r\n",current); - wall_follow2(LEFT, BACKWARD, RIGS, current, rig); + wall_follow2(RIGHT, FORWARD, RIGS, current, rig); current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); } - wait(1); - wall_follow2(LEFT, FORWARD, RIGS, current, rig); - current-=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); - wait(1); - - alignWithRig(); + wait_ms(500); + wall_follow2(RIGHT, FORWARD, RIGS, current, rig); + current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); + wait_ms(500); + pc.printf("align with rig\r\n"); + //toRig(); + slightMove(FORWARD,785); } void tools_section_return(float* location, float ¤t) @@ -1781,26 +1789,71 @@ wait_ms(300); } -void alignWithRig(){ +void toRig(){ - if(IRLeftFront.getIRDistance() < 30) { + if(IRLeftFront.getIRDistance() < 38) { motors.setMotor0Speed(0.1*127); motors.setMotor1Speed(0.1*127); - while(IRLeftFront.getIRDistance() < 30); - pc.printf("IR left back %f \t",IRLeftFront.getIRDistance()); - - motors.stopBothMotors(127); - wait_ms(300); + while(IRLeftFront.getIRDistance() < 38); + pc.printf("IF\r\n"); } motors.setMotor0Speed(0.1*127); motors.setMotor1Speed(0.1*127); + while(IRLeftBack.getIRDistance() > 38); + + //while(abs(leftEncoder.getPulses())<100 || rightEncoder.getPulses()<100); + // forward until back sensor does not see the rig + while(IRLeftBack.getIRDistance() < 38); + pc.printf("align\r\n"); + motors.stopBothMotors(127); + wait_ms(300); - while(abs(leftEncoder.getPulses())<100 || rightEncoder.getPulses()<100); +} + +void alignWithRigWall(float ¤t){ + + motors.begin(); + leftEncoder.reset(); + rightEncoder.reset(); + motors.setMotor0Speed(-0.5*127);//right + motors.setMotor1Speed(0.5*127);//left + while(abs(leftEncoder.getPulses())<950 || abs(rightEncoder.getPulses())<950); + motors.stopBothMotors(127); + wait_ms(300); + + leftEncoder.reset(); + rightEncoder.reset(); + + if(current <80){ + //forward until back sensor is in front of the wave + while(IRRightBack.getIRDistance() >35); + motors.setMotor0Speed(0.2*127);//right + motors.setMotor1Speed(0.2*127);//left + } + else{ + //motors.setMotor0Speed(-0.1*127);//right + //motors.setMotor1Speed(-0.1*127);//left + } + + /*while(abs(leftEncoder.getPulses())<800 || abs(rightEncoder.getPulses())<800); + while(IRRightBack.getIRDistance() >35); + */ + motors.stopBothMotors(127); + wait_ms(300); + + // Align with Gap + while(IRRightFront.getIRDistance() - IRRightBack.getIRDistance() > 0.5) { + pc.printf("turn right\r\n"); + motors.setMotor0Speed(-0.3*127);//right + motors.setMotor1Speed(0.3*127);// left + } + while(IRRightFront.getIRDistance() - IRRightBack.getIRDistance() < -0.5) { + pc.printf("turn left\r\n"); + motors.setMotor0Speed(0.3*127);// right + motors.setMotor1Speed(-0.3*127);// left + } motors.stopBothMotors(127); wait_ms(300); - - - } \ No newline at end of file