uyvug
Dependencies: Adafruit-16-Ch-PWM-Servo-Driver HCSR04 PID PololuQik2 QEI Sharp mbed-rtos
Fork of theRobot by
Diff: main.cpp
- Revision:
- 33:a41981e18a7d
- Parent:
- 32:4a42f61f64a6
- Child:
- 34:cd4980573159
--- a/main.cpp Mon Apr 21 19:23:26 2014 +0000 +++ b/main.cpp Tue Apr 22 15:24:56 2014 +0000 @@ -118,7 +118,7 @@ //Serial bt(p13,p14); HCSR04 rangeFinderFront( PIN_TRIGGERL, PIN_ECHOL ); HCSR04 rangeFinderRight( PIN_TRIGGERR, PIN_ECHOR ); -PID pid1(25.0,0.0,0.0,0.0001); +PID pid1(25.0,2.0,1.0,0.0001); PololuQik2 motors(p9, p10, p8, p11, &errFunction, cRc); QEI rightEncoder(p22,p21,NC,PPR,QEI::X4_ENCODING); QEI leftEncoder(p23,p24,NC,PPR,QEI::X4_ENCODING); @@ -166,7 +166,8 @@ void overBump(int section); void alignWithWall(int section); void UntilWall(int dir); -void alignWithGap(); +void alignWithGap(void); +void alignWithRig(void); void testSensors(void); float normd(int* pop, int count, int threshold); @@ -329,9 +330,9 @@ case START : myled1 = 1; - current=75; - //state = NAVIGATE_WAVES_ROW1; - state = OILRIG1_POS; + current=68; + state = NAVIGATE_WAVES_ROW1; + //state = OILRIG1_POS; break; @@ -432,7 +433,7 @@ * **************************************************/ case IDENTIFY_TOOLS: - + //wait(5); servoPosition(TOOL_2); //arm/camera looks over tool wait(5); //wait for servos to settle @@ -669,7 +670,7 @@ break; case NAVIGATE_WAVES_ROW3: - shape_detected = 1; + tool_needed = 2; if(tool_needed == 1) { rig_section(location, current, direction, 1); //state = NAVIGATE_TO_SQUARE_RIG; @@ -1060,25 +1061,6 @@ motors.stopBothMotors(127); wait_ms(300); return; - /* - rangeFinderRight.startMeas(); - wait_ms(20); - rangeFinderRight.getMeas(range); - - if(range>15){ - // turning left - leftEncoder.reset(); - rightEncoder.reset(); - motors.setMotor0Speed(-MAX_SPEED); //right - motors.setMotor1Speed(MAX_SPEED); //left - while(abs(rightEncoder.getPulses()) < 50 || abs(leftEncoder.getPulses()) < 50); - motors.stopBothMotors(127); - return; - } - */ - // turning left - //motors.setMotor0Speed(0.9*MAX_SPEED); //right - //motors.setMotor1Speed(-0.9*MAX_SPEED); //left } else if(section == RIGS) { @@ -1098,76 +1080,23 @@ motors.setMotor1Speed(-0.25*127); //left while(abs(leftEncoder.getPulses()) < 150 || abs(rightEncoder.getPulses()) < 150); motors.stopBothMotors(127); - wait(2); + wait(1); - // turn right away from wall - leftEncoder.reset(); - rightEncoder.reset(); - motors.setMotor0Speed(-MAX_SPEED); //right - motors.setMotor1Speed(MAX_SPEED); //left - while(abs(rightEncoder.getPulses()) < 25 || abs(leftEncoder.getPulses()) < 25 ); - + if(IRLeftFront.getIRDistance() > 35 && IRLeftBack.getIRDistance() > 35) return; + // align with wave + while(IRLeftFront.getIRDistance() > 35) { + pc.printf("front sensor sees gap\r\n"); + motors.setMotor0Speed(-0.1*127);//right + motors.setMotor1Speed(-0.1*127);//left + } + while(IRLeftBack.getIRDistance() >35 ) { + pc.printf("back sensor sees gap\r\n"); + motors.setMotor0Speed(0.1*127);//right + motors.setMotor1Speed(0.1*127);//left + } motors.stopBothMotors(127); - } else if(section == MID2) { - - // check distance to wall - - if(IRLeftFront.getIRDistance() > 4) { - - leftEncoder.reset(); - rightEncoder.reset(); - motors.setMotor0Speed(-1.2*MAX_SPEED); //right - motors.setMotor1Speed(0.4*MAX_SPEED); //left - while(rightEncoder.getPulses()>-1000); - motors.stopBothMotors(0); - - //go backwards toward wall - leftEncoder.reset(); - rightEncoder.reset(); - motors.setMotor0Speed(-0.25*127); //right - motors.setMotor1Speed(-0.25*127); //left - while(abs(leftEncoder.getPulses()) < 150 || abs(rightEncoder.getPulses()) < 150); - - // turn left towards wall - leftEncoder.reset(); - rightEncoder.reset(); - motors.setMotor0Speed(0.4*127); //right - motors.setMotor1Speed(-0.4*127); //left - while(abs(rightEncoder.getPulses()) < 65 || abs(leftEncoder.getPulses()) < 65); - motors.stopBothMotors(127); - - slightMove(FORWARD,100); - } - return; - - } else { // MID - //pc.printf("in mid section align\r\n"); - // turn right towards wall - rightTurn(); - // turning left towards wall - //motors.setMotor0Speed(0.9*MAX_SPEED); //right - //motors.setMotor1Speed(-0.9*MAX_SPEED); //left - - } - - usValue = 0; - /* while(1) { - if(section == 10) { // CURENTLY NOT USED (WAS RIGS) - rangeFinderRight.startMeas(); - wait_ms(20); - rangeFinderRight.getMeas(range); - } else { - range = IRLeftFront.getIRDistance(); - } - //pc.printf("Range %f \t OldValue %f\n\r",range, usValue); - if(range > usValue && usValue != 0 && range < 25) { - break; - } else { - usValue = range; - } - } - motors.stopBothMotors(0);*/ + } } void rightTurn(void) @@ -1607,12 +1536,18 @@ if(rig == 1) loc= 15; else if(rig == 2) loc= 45; else loc = 75; - + // Backwards until the gap is hit. + motors.setMotor1Speed(-0.1*127);//left + motors.setMotor0Speed(-0.1*127);//right + wait_ms(100); + while(motors.GetMotor0Current()<5 || motors.GetMotor1Current() <5 ); + motors.stopBothMotors(127); + wait_ms(300); + // Slight forward for turn slightMove(FORWARD,150); wait_ms(100); leftTurn(); - //slightright(); if(current > loc) { @@ -1625,12 +1560,11 @@ current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); } wait(1); - alignWithWall(MID2); - current+=4; wall_follow2(LEFT, FORWARD, RIGS, current, rig); current-=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); - slightMove(FORWARD, 75); - current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); + wait(1); + + alignWithRig(); } void tools_section_return(float* location, float ¤t) @@ -1647,7 +1581,7 @@ { if(direction[0] == RIGHT) { rightTurn(); - alignWithWall(MID); + //alignWithWall(MID); wall_follow2(LEFT, FORWARD, MID, current,0); current-=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); slightMove(FORWARD, 50); @@ -1683,7 +1617,7 @@ void rig_section_return(float* location, float ¤t, int* direction) { - alignWithWall(RIGS); + //alignWithWall(RIGS); if(location[2] > current) { wall_follow2(LEFT, BACKWARD, RETURN, current,0); @@ -1811,29 +1745,29 @@ void alignWithGap() { pc.printf("\r\n Align with gap."); + wait(1); - if(IRLeftBack.getIRDistance() > 35) { + + if(IRLeftBack.getIRDistance() > 30) { motors.setMotor0Speed(-0.1*127); motors.setMotor1Speed(-0.1*127); - } - while(IRLeftBack.getIRDistance() > 35); - pc.printf("IR left back %f \t",IRLeftBack.getIRDistance()); + while(IRLeftBack.getIRDistance() > 30); + pc.printf("IR left back %f \t",IRLeftBack.getIRDistance()); - motors.stopBothMotors(127); - wait_ms(200); - wait(1); + motors.stopBothMotors(127); + wait_ms(300); + } - if(IRLeftBack.getIRDistance() < 35) { + + if(IRLeftBack.getIRDistance() < 30 && IRLeftFront.getIRDistance() > 30) { motors.setMotor0Speed(0.1*127); motors.setMotor1Speed(0.1*127); - } - while(IRLeftBack.getIRDistance() < 35); - pc.printf("IR left back %f \t\r\n",IRLeftBack.getIRDistance()); + while(IRLeftBack.getIRDistance() < 30); - - - motors.stopBothMotors(127); - wait_ms(200); + pc.printf("IR left back %f \t\r\n",IRLeftBack.getIRDistance()); + motors.stopBothMotors(127); + wait_ms(300); + } leftEncoder.reset(); rightEncoder.reset(); @@ -1841,9 +1775,32 @@ motors.setMotor0Speed(0.1*127); motors.setMotor1Speed(0.1*127); - while(abs(leftEncoder.getPulses())<500 || rightEncoder.getPulses()<500); + while(abs(leftEncoder.getPulses())<575 || rightEncoder.getPulses()<575); motors.stopBothMotors(127); - wait_ms(200); + wait_ms(300); +} + +void alignWithRig(){ + + if(IRLeftFront.getIRDistance() < 30) { + 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); + } + + motors.setMotor0Speed(0.1*127); + motors.setMotor1Speed(0.1*127); + + while(abs(leftEncoder.getPulses())<100 || rightEncoder.getPulses()<100); + + motors.stopBothMotors(127); + wait_ms(300); + + + } \ No newline at end of file