uyvug
Dependencies: Adafruit-16-Ch-PWM-Servo-Driver HCSR04 PID PololuQik2 QEI Sharp mbed-rtos
Fork of theRobot by
Diff: main.cpp
- Revision:
- 23:f8e806d1ffcc
- Parent:
- 22:79c5871543b5
- Child:
- 24:3369d51f6cbd
--- a/main.cpp Fri Apr 11 21:56:18 2014 +0000 +++ b/main.cpp Sat Apr 12 20:27:00 2014 +0000 @@ -303,7 +303,6 @@ **************************************************/ case START : myled1 = 1; - //state = GOTO_TOOLS1; state = OILRIG1_POS; break; @@ -376,7 +375,6 @@ wait(3); //wait for servos to settle to_tools_section1(location, current); - //state = NAVIGATE_WAVES_ROW1; state = IDENTIFY_TOOLS; break; @@ -637,12 +635,12 @@ break; case NAVIGATE_WAVES_ROW3: - shape_detected = 1; - if(shape_detected == 1) { + //shape_detected = 1; + if(tool_needed == 1) { rig_section(location, current, direction, 1); //state = NAVIGATE_TO_SQUARE_RIG; state = RETURN_TO_START; - } else if(shape_detected == 2) { + } else if(tool_needed == 2) { rig_section(location, current, direction, 2); //state = NAVIGATE_TO_TRIANGLE_RIG; state = RETURN_TO_START; @@ -714,13 +712,10 @@ * **************************************************/ case RETURN_TO_START: - wait(3); + wait(3); rig_section_return(location, current, direction); - wait(3); mid_section2_return(location, current, direction); - wait(3); mid_section_return(location, current, direction); - wait(3); tools_section_return(location,current); while(1); state = END; @@ -970,7 +965,7 @@ void wall_follow2(int side, int direction, int section, float location, int rig) { int dir=1, limit=80, lowlim=4; - float set=9, loc=0, Rigloc=0; + float set=9, loc=0, Rigloc=0, location_cal=0; bool SeeWaveGap = false; if(rig == 1) Rigloc= 16; @@ -980,10 +975,9 @@ pid1.reset(); if(section == TOOLS) { - set= 9; - limit = 86; - } else if(section == RIGS) set = 9; - else if(section == RETURN) lowlim=4; + limit = 100; + lowlim=10; + }else if(section == RIGS) set = 6; else if(section == MID2) limit =85; if(direction == BACKWARD) { @@ -1000,11 +994,15 @@ //pc.printf("dir*loc+location %f\r\n",dir*loc + location ); //pc.printf("limit %d \r\n", limit); - - while((dir*loc + location <= limit) && (dir*loc + location >= lowlim)) { + if((section == RIGS || section == RETURN) && side == LEFT)location_cal= -1*dir*loc + location; + else location_cal= dir*loc + location; + while((location_cal <= limit) && (location_cal >= lowlim)) { + loc=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2; - //pc.printf("loc %f \r\n", loc); + + if((section == RIGS || section == RETURN) && side == LEFT)location_cal= -1*dir*loc + location; + else location_cal= dir*loc + location; pid1.setInputLimits(0.0, set); pid1.setOutputLimits( -MAX_SPEED1, MAX_SPEED1); @@ -1021,12 +1019,18 @@ } if(section == RIGS) { - rangeFinderLeft.startMeas(); - wait_ms(20); - rangeFinderLeft.getMeas(range2); + if(side == LEFT){ + rangeFinderRight.startMeas(); + wait_ms(20); + rangeFinderRight.getMeas(range2); + }else{ + rangeFinderLeft.startMeas(); + wait_ms(20); + rangeFinderLeft.getMeas(range2); + } if(range2< 15) { - if( abs(dir*loc + location - Rigloc) < 10) { + if( abs(location_cal - Rigloc) < 10) { //STOP motors.stopBothMotors(127); break; @@ -1038,7 +1042,7 @@ //pc.printf("wall follow 2 range %f\r\n",range); //pc.printf("loc+location = %f\r\n", loc+location); if(range > 15 ) { - if(section == RIGS || section == RETURN) { + if(section == RIGS || section == TOOLS) { motors.setMotor0Speed(dir*0.25*127); //right motors.setMotor1Speed(dir*0.25*127); //left } else { @@ -1141,78 +1145,77 @@ //motors.setMotor1Speed(-0.9*MAX_SPEED); //left } else if(section == RIGS) { - // check distance to wall - rangeFinderRight.startMeas(); - wait_ms(20); - rangeFinderRight.getMeas(range); - if(range < 3) return; - - // turn at an angle + // turn left at an angle leftEncoder.reset(); rightEncoder.reset(); motors.setMotor1Speed(-1.2*MAX_SPEED); //left motors.setMotor0Speed(0.4*MAX_SPEED); //right while(abs(leftEncoder.getPulses())<500); motors.stopBothMotors(0); - wait(2); - - //go backwards toward wall - leftEncoder.reset(); - rightEncoder.reset(); - motors.setMotor0Speed(-0.25*127); //right - motors.setMotor1Speed(-0.25*127); //left - while(abs(leftEncoder.getPulses()) < 200 || abs(rightEncoder.getPulses()) < 200); - motors.stopBothMotors(0); - wait(2); + wait_ms(200); - // turn right towards wall - 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); - /* wait(2); - - // turning left - motors.setMotor0Speed(-0.9*MAX_SPEED); //right - motors.setMotor1Speed(0.9*MAX_SPEED); //left - */ - } else if(section == MID2) { - 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 + //go backwards away form 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); + motors.stopBothMotors(127); + wait(2); - // turn left towards wall + // turn right away from 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.setMotor0Speed(-MAX_SPEED); //right + motors.setMotor1Speed(MAX_SPEED); //left + while(abs(rightEncoder.getPulses()) < 25 || abs(leftEncoder.getPulses()) < 25 ); + motors.stopBothMotors(127); - - slightMove(FORWARD,100); + + } else if(section == MID2) { + + // check distance to wall + rangeFinderLeft.startMeas(); + wait_ms(20); + rangeFinderLeft.getMeas(range); + + if(range > 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 + } + 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 + //motors.setMotor0Speed(0.9*MAX_SPEED); //right + //motors.setMotor1Speed(-0.9*MAX_SPEED); //left } @@ -1337,8 +1340,8 @@ // Over bump leftEncoder.reset(); rightEncoder.reset(); - motors.setMotor0Speed(0.3*127); //right - motors.setMotor1Speed(0.3*127); //left + motors.setMotor0Speed(0.33*127); //right + motors.setMotor1Speed(0.33*127); //left while((abs(leftEncoder.getPulses()) < 900 || abs(rightEncoder.getPulses()) < 900) /*&& preLeft!=0*/ && IR.getIRDistance() >15 ); @@ -1366,18 +1369,18 @@ } } else if(section == MID || section == MID2) { if(section == MID2) { - motors.setMotor0Speed(.3*127); //right - motors.setMotor1Speed(.3*127); //left - while(IR.getIRDistance() > 15 && (abs(leftEncoder.getPulses()) < 300 || abs(rightEncoder.getPulses()) < 300)); + motors.setMotor0Speed(.33*127); //right + motors.setMotor1Speed(.33*127); //left + while(IR.getIRDistance() > 15 && (abs(leftEncoder.getPulses()) < 400 || abs(rightEncoder.getPulses()) < 400)); } - while(IR.getIRDistance() > 15 && (abs(leftEncoder.getPulses()) < 100 || abs(rightEncoder.getPulses()) < 100) && (leftEncoder.getPulses() != preLeft || rightEncoder.getPulses() != preRight)) { + while( IR.getIRDistance() > 15 && (abs(leftEncoder.getPulses()) < 100 || abs(rightEncoder.getPulses()) < 100) /*&& (leftEncoder.getPulses() != preLeft || rightEncoder.getPulses() != preRight)*/) { - if(IR.getIRDistance() > 38) break; + //if(IR.getIRDistance() > 38) break; - preLeft=leftEncoder.getPulses(); - preRight=rightEncoder.getPulses(); - wait_ms(200); + //preLeft=leftEncoder.getPulses(); + //preRight=rightEncoder.getPulses(); + //wait_ms(200); } } else {// RIGS @@ -1435,7 +1438,7 @@ //wall_follow2(LEFT,FORWARD,MID, current); - slightMove(BACKWARD,400); + slightMove(BACKWARD,500); current-=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); rangeFinderLeft.startMeas(); @@ -1544,11 +1547,12 @@ //current-=4; //} rightTurn(); + //right turn more leftEncoder.reset(); rightEncoder.reset(); motors.setMotor0Speed(-0.4*127);// right motors.setMotor1Speed(0.4*127);// left - while(abs(leftEncoder.getPulses())<100 || abs(rightEncoder.getPulses())<100); + while(abs(leftEncoder.getPulses())<25 || abs(rightEncoder.getPulses())<25); motors.stopBothMotors(127); //pc.printf("mid section current = %f\r\n",current); wall_follow2(LEFT,FORWARD,MID, current,0); @@ -1561,7 +1565,7 @@ rangeFinderLeft.getMeas(range); if(range > 20 ) { - wait(3); + wait(1); direction[0]= RIGHT; location[1]= current; wait_ms(300); @@ -1577,7 +1581,7 @@ slightMove(FORWARD, 75); current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); } - //slightMove(BACKWARD,100); + else slightMove(BACKWARD,75); } @@ -1589,7 +1593,7 @@ rightEncoder.reset(); motors.setMotor0Speed(0.5*127);// right motors.setMotor1Speed(-0.5*127);// left - while(abs(leftEncoder.getPulses())<1045 || rightEncoder.getPulses()<1045); + while(abs(leftEncoder.getPulses())<110 || rightEncoder.getPulses()<1100); motors.stopBothMotors(127); wait_ms(100); @@ -1633,7 +1637,7 @@ if(range > 20 ) { direction[1]= RIGHT; location[2]= current; - slightMove(FORWARD,100); + slightMove(FORWARD,150); current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); } else { direction[1]= LEFT; @@ -1649,7 +1653,7 @@ rightEncoder.reset(); motors.setMotor0Speed(0.5*127);// right motors.setMotor1Speed(-0.5*127);// left - while(abs(leftEncoder.getPulses())<1050 || rightEncoder.getPulses()<1050); + while(abs(leftEncoder.getPulses())<1115 || rightEncoder.getPulses()<1115); motors.stopBothMotors(127); overBump(RIGS); @@ -1666,26 +1670,25 @@ // Slight forward for turn slightMove(FORWARD,150); - current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); wait_ms(100); - rightTurn(); + leftTurn(); //slightright(); if(current > loc) { //pc.printf("RIG section %f\r\n",current); - wall_follow2(RIGHT, BACKWARD, RIGS, current, rig); + wall_follow2(LEFT, FORWARD, 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(RIGHT, FORWARD, RIGS, current, rig); + wall_follow2(LEFT, BACKWARD, RIGS, current, rig); current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); } - wait(2); + wait(1); alignWithWall(MID2); - current-=4; - wall_follow2(RIGHT, FORWARD, RIGS, current, rig); - current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); + 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); } @@ -1694,25 +1697,28 @@ { if(location[0] > 16) { leftTurn(); - wall_follow2(LEFT, BACKWARD, RETURN, location[0], 0); + wall_follow2(LEFT, BACKWARD, TOOLS, location[0], 0); } - motors.stopBothMotors(0); + motors.stopBothMotors(127); } void mid_section_return(float* location, float ¤t, int* direction) { if(direction[0] == RIGHT) { - leftTurn(); - alignWithWall(MID); - wall_follow2(LEFT, BACKWARD, MID, current,0); - current-=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); rightTurn(); - } else if(direction[0] == LEFT) { + 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); leftTurn(); - wall_follow2(RIGHT, FORWARD, MID, current,0); + } else if(direction[0] == LEFT) { + rightTurn(); + wall_follow2(RIGHT, BACKWARD, MID, current,0); current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); - rightTurn(); + wait_ms(200); + slightMove(BACKWARD, 100); + leftTurn(); } //ELSE and GO FORWARD overBump(RIGS); @@ -1721,15 +1727,15 @@ void mid_section2_return(float* location, float ¤t, int* direction) { if(direction[1] == RIGHT) { + rightTurn(); + wall_follow2(LEFT, FORWARD, RETURN, current,0); + current-=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); leftTurn(); - wall_follow2(LEFT, BACKWARD, MID, current,0); - current-=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); - rightTurn(); } else if(direction[1] == LEFT) { + rightTurn(); + wall_follow2(LEFT, BACKWARD, RETURN, current,0); + current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); leftTurn(); - wall_follow2(RIGHT, FORWARD, MID, current,0); - current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); - rightTurn(); } //ELSE and GO FORWARD overBump(MID); @@ -1737,14 +1743,27 @@ void rig_section_return(float* location, float ¤t, int* direction) { + alignWithWall(RIGS); + if(location[2] > current) { - wall_follow2(RIGHT, FORWARD, MID, current,0); + wall_follow2(LEFT, BACKWARD, RETURN, current,0); current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); + wait_ms(500); + //slightMove(FORWARD,50); } else { - wall_follow2(RIGHT, BACKWARD, MID, current,0); + wall_follow2(LEFT, FORWARD, RETURN, current,0); current-=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); } - rightTurn(); + + // LEFT TURN + motors.begin(); + leftEncoder.reset(); + rightEncoder.reset(); + motors.setMotor0Speed(0.5*127);// right + motors.setMotor1Speed(-0.5*127);// left + while(abs(leftEncoder.getPulses())<1100 || rightEncoder.getPulses()<110); + + motors.stopBothMotors(127); overBump(MID2); }