ksdflsjdfljas
Dependencies: Adafruit-16-Ch-PWM-Servo-Driver HCSR04 PID PololuQik2 QEI Sharp mbed-rtos
Fork of theRobot by
Diff: main.cpp
- Revision:
- 20:b9cbaf7566e9
- Parent:
- 19:9329e9be4c66
- Child:
- 21:2936d9566213
diff -r 9329e9be4c66 -r b9cbaf7566e9 main.cpp --- a/main.cpp Thu Apr 03 23:01:50 2014 +0000 +++ b/main.cpp Fri Apr 04 18:39:38 2014 +0000 @@ -20,6 +20,7 @@ #define DIST_PER_PULSE (0.01054225722682) #define MTRS_TO_INCH (39.3701) #define MAX_SPEED (0.3*127) +#define MAX_SPEED1 (0.25*127) #define PPR (4331/4) #define LEFT (1) #define RIGHT (0) @@ -257,7 +258,18 @@ int pu, num, input; + pc.baud(115200); + /*while(1){ + rangeFinderLeft.startMeas(); + wait_ms(20); + rangeFinderLeft.getMeas(range); + rangeFinderRight.startMeas(); + wait_ms(20); + rangeFinderRight.getMeas(range2); + pc.printf("left %f \t right %f\r\n", range, range2); + + }*/ //Laser Range Finder Initialization lrf_baudCalibration(); @@ -299,25 +311,25 @@ **************************************************/ case OILRIG1_POS: //aims arm at square oil rig - +/* servoPosition(OIL_RIG1); wait(3); //wait for servo to settle before laser distance fire = fire_checker(OIL_RIG1); //determines if oil rig is on fire - +*/ fire=1; //determines what tool is needed if (fire == 1) { - pc.printf("FIRE FOUND!!!!!!!!\n\r"); + //pc.printf("FIRE FOUND!!!!!!!!\n\r"); tool_needed = SQUARE; state = GOTO_TOOLS1; } else { - pc.printf("XXXXXX FIRE NOT FOUND XXXXXX"); + //pc.printf("XXXXXX FIRE NOT FOUND XXXXXX"); state = OILRIG2_POS; } break; case OILRIG2_POS: - +/* setServoPulse(2, Arm_Table[DRIVE_POSITION_NOTOOL].big_arm); setServoPulse(3, Arm_Table[DRIVE_POSITION_NOTOOL].claw_arm); setServoPulse(1, Arm_Table[DRIVE_POSITION_NOTOOL].base_arm); @@ -325,19 +337,21 @@ setServoPulse(4, Arm_Table[DRIVE_POSITION_NOTOOL].claw_rotate); setServoPulse(5, Arm_Table[DRIVE_POSITION_NOTOOL].claw_open); wait(3); //wait for servos to settle - +*/ to_tools_section2(location, current); // moves to second rig - + wait(2); +/* servoPosition(OIL_RIG2); //position arm to point at first oilrig wait(3); fire = fire_checker(OIL_RIG2); +*/ fire=1; if (fire == 1) { - pc.printf("FIRE FOUND!!!!!!!!"); + //pc.printf("FIRE FOUND!!!!!!!!"); tool_needed = TRIANGLE; state = GOTO_TOOLS2; } else { - pc.printf("XXXXXX FIRE NOT FOUND XXXXXX"); + //pc.printf("XXXXXX FIRE NOT FOUND XXXXXX"); tool_needed = CIRCLE; state = GOTO_TOOLS2; } @@ -350,6 +364,21 @@ * **************************************************/ case GOTO_TOOLS1: + /* setServoPulse(2, Arm_Table[DRIVE_POSITION_NOTOOL].big_arm); + setServoPulse(3, Arm_Table[DRIVE_POSITION_NOTOOL].claw_arm); + setServoPulse(1, Arm_Table[DRIVE_POSITION_NOTOOL].base_arm); + setServoPulse(0, Arm_Table[DRIVE_POSITION_NOTOOL].base_rotate); + setServoPulse(4, Arm_Table[DRIVE_POSITION_NOTOOL].claw_rotate); + setServoPulse(5, Arm_Table[DRIVE_POSITION_NOTOOL].claw_open); + wait(3); //wait for servos to settle +*/ + to_tools_section1(location, current); + wait(2); + state = IDENTIFY_TOOLS; + break; + + case GOTO_TOOLS2: +/* setServoPulse(2, Arm_Table[DRIVE_POSITION_NOTOOL].big_arm); setServoPulse(3, Arm_Table[DRIVE_POSITION_NOTOOL].claw_arm); setServoPulse(1, Arm_Table[DRIVE_POSITION_NOTOOL].base_arm); @@ -357,22 +386,10 @@ setServoPulse(4, Arm_Table[DRIVE_POSITION_NOTOOL].claw_rotate); setServoPulse(5, Arm_Table[DRIVE_POSITION_NOTOOL].claw_open); wait(3); //wait for servos to settle - to_tools_section1(location, current); - state = IDENTIFY_TOOLS; - break; - - case GOTO_TOOLS2: - - setServoPulse(2, Arm_Table[DRIVE_POSITION_NOTOOL].big_arm); - setServoPulse(3, Arm_Table[DRIVE_POSITION_NOTOOL].claw_arm); - setServoPulse(1, Arm_Table[DRIVE_POSITION_NOTOOL].base_arm); - setServoPulse(0, Arm_Table[DRIVE_POSITION_NOTOOL].base_rotate); - setServoPulse(4, Arm_Table[DRIVE_POSITION_NOTOOL].claw_rotate); - setServoPulse(5, Arm_Table[DRIVE_POSITION_NOTOOL].claw_open); - wait(3); //wait for servos to settle - +*/ slightMove(FORWARD,3100); current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2; + wait(2); state = IDENTIFY_TOOLS; break; @@ -387,9 +404,13 @@ * **************************************************/ case IDENTIFY_TOOLS: - + + state = NAVIGATE_WAVES_ROW1; + break; + + wait(5); - servoPosition(TOOL_2); //arm/camera looks over tool + servoPosition(TOOL_2); //arm/camera looks over tool wait(5); //wait for servos to settle //shape_detected = shapeDetection(); //determines the shape @@ -402,7 +423,7 @@ wait(1); shape_detected = shapeDetection(); - pc.printf("Y - Adjust to center tool\n\r"); + //pc.printf("Y - Adjust to center tool\n\r"); if(get_com_y() < 50) { wait(1); @@ -704,7 +725,9 @@ break; case NAVIGATE_WAVES_ROW3: - + shape_detected=1; + + if(shape_detected == 1) { rig_section(location, current, direction, 1); while(1); @@ -752,7 +775,7 @@ //*********************// // CODE TO ALIGN ROBOT WITH RIG - servoPosition(ORIENT_TOOL); + // servoPosition(ORIENT_TOOL); wait(1); //wait for servos to settle state = INSERT_TOOL; break; @@ -815,7 +838,7 @@ pulse = MIN_SERVO_PULSE + (( angle * (MAX_SERVO_PULSE - MIN_SERVO_PULSE)) / SERVO_MAX_ANGLE); float pulselength = 20000; // 20,000 us per second int i = currentPosition[n]; - pc.printf("ServoNumber: %d Begining Pulse: %d\n\r",n,currentPosition[n]); + //pc.printf("ServoNumber: %d Begining Pulse: %d\n\r",n,currentPosition[n]); int pulse2; if(currentPosition[n] < pulse) { for(i; i < pulse; i++) { @@ -831,7 +854,7 @@ } } currentPosition[n] = i; - pc.printf("END: pulse: %d, angle: %d\n\r", i, angle); + //pc.printf("END: pulse: %d, angle: %d\n\r", i, angle); } void initServoDriver(void) @@ -845,7 +868,7 @@ void servoBegin(void) { - pc.printf("Setting Initial Servo Position\n\r"); + //pc.printf("Setting Initial Servo Position\n\r"); setServoPulseNo_delay(3, Arm_Table[STORE_POSITION].claw_arm); setServoPulseNo_delay(2, Arm_Table[STORE_POSITION].big_arm); wait(2); @@ -894,7 +917,7 @@ case 1: for (int i = 0; i<10; i++) { distLaser = getDistance(); - pc.printf("L DISTANCE: %d \n\r", distLaser); + //pc.printf("L DISTANCE: %d \n\r", distLaser); if ((distLaser < OILRIG1_MAX) && (distLaser > OILRIG1_MIN)) { fire_detected++; @@ -906,7 +929,7 @@ case 2: for (int i = 0; i<10; i++) { distLaser = getDistance(); - pc.printf("L DISTANCE: %d \n\r", distLaser); + //pc.printf("L DISTANCE: %d \n\r", distLaser); if ((distLaser < OILRIG2_MAX) && (distLaser > OILRIG2_MIN)) { fire_detected++; @@ -931,7 +954,7 @@ } void wall_follow(int side, int direction, int section) { - float location, set=6; + float location, set=4; int dir=1; pid1.reset(); @@ -958,7 +981,7 @@ rangeFinderRight.startMeas(); wait_ms(20); rangeFinderRight.getMeas(range); - pc.printf("%d\r\n",range); + //pc.printf("%d\r\n",range); } if(range > 15) { @@ -1005,8 +1028,8 @@ void wall_follow2(int side, int direction, int section, float location, int rig) { - int dir=1, limit=86, lowlim=5; - float set=6, loc=0, Rigloc=0; + int dir=1, limit=78, lowlim=4; + float set=7, loc=0, Rigloc=0; bool SeeWaveGap = false; if(rig == 1) Rigloc= 16; @@ -1019,28 +1042,32 @@ dir=-1; limit = 100; } + else if(direction == FORWARD) lowlim=-20; if(section == TOOLS) { - set= 6; + set= 7; limit = 86; } - else if(section == RIGS) set = 3; - else if(section == RETURN) lowlim=15; - + else if(section == RIGS) set = 7; + else if(section == RETURN) lowlim=4; + else if(section == MID2) limit =85; + + if(location <4) limit=80; + leftEncoder.reset(); rightEncoder.reset(); //pc.printf("before %f\r\n", location); - pc.printf("dir*loc+location %f\r\n",dir*loc + location ); - pc.printf("limit %d \r\n", limit); + //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)) { loc=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2; - pc.printf("loc %f \r\n", loc); + //pc.printf("loc %f \r\n", loc); pid1.setInputLimits(0.0, set); - pid1.setOutputLimits( -MAX_SPEED, MAX_SPEED); + pid1.setOutputLimits( -MAX_SPEED1, MAX_SPEED1); pid1.setSetPoint(set); if(side) { @@ -1058,7 +1085,7 @@ wait_ms(20); rangeFinderLeft.getMeas(range2); - if(range2< 20) { + if(range2< 15) { if( abs(dir*loc + location - Rigloc) < 10) { //STOP motors.stopBothMotors(127); @@ -1070,18 +1097,19 @@ //pc.printf("wall follow 2 range %f\r\n",range); //pc.printf("loc+location = %f\r\n", loc+location); - if(range > 20 ) { + if(range > 15 ) { if(section == RIGS || section == RETURN) { motors.setMotor0Speed(dir*0.25*127); //right motors.setMotor1Speed(dir*0.25*127); //left } else { if(!SeeWaveGap) { + wait_ms(40); SeeWaveGap=true; } else { //STOP motors.stopBothMotors(127); - pc.printf("wavegap\r\n"); + //pc.printf("wavegap\r\n"); // AT WAVE OPENING!!!! break; } @@ -1125,7 +1153,7 @@ float usValue = 0; if(section == TOOLS) { - pc.printf("tools section align\r\n"); + //pc.printf("tools section align\r\n"); // turn at an angle leftEncoder.reset(); rightEncoder.reset(); @@ -1149,11 +1177,28 @@ motors.setMotor1Speed(-MAX_SPEED); //left while(rightEncoder.getPulses() < 20 || abs(leftEncoder.getPulses()) < 20); - motors.stopBothMotors(0); - + 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 + //motors.setMotor0Speed(0.9*MAX_SPEED); //right + //motors.setMotor1Speed(-0.9*MAX_SPEED); //left } else if(section == RIGS) { // check distance to wall @@ -1161,7 +1206,7 @@ wait_ms(20); rangeFinderRight.getMeas(range); - if(range < 3 || range > 20) return; + if(range < 3) return; // turn at an angle leftEncoder.reset(); @@ -1195,17 +1240,45 @@ motors.setMotor0Speed(-0.9*MAX_SPEED); //right motors.setMotor1Speed(0.9*MAX_SPEED); //left */ - } else {// MID - pc.printf("in mid section align\r\n"); + }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 + 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) { +/* while(1) { if(section == 10) { // CURENTLY NOT USED (WAS RIGS) rangeFinderRight.startMeas(); wait_ms(20); @@ -1215,14 +1288,14 @@ wait_ms(20); rangeFinderLeft.getMeas(range); } - pc.printf("Range %f \t OldValue %f\n\r",range, usValue); + //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); + motors.stopBothMotors(0);*/ } void rightTurn(void) @@ -1232,7 +1305,7 @@ rightEncoder.reset(); motors.setMotor0Speed(-0.5*127);//right motors.setMotor1Speed(0.5*127);//left - while(abs(leftEncoder.getPulses())<1050 || abs(rightEncoder.getPulses())<1050); + while(abs(leftEncoder.getPulses())<850 || abs(rightEncoder.getPulses())<850); motors.stopBothMotors(127); } @@ -1254,7 +1327,7 @@ rightEncoder.reset(); motors.setMotor0Speed(0.5*127);// right motors.setMotor1Speed(-0.5*127);// left - while(abs(leftEncoder.getPulses())<90 || rightEncoder.getPulses()<90); + while(abs(leftEncoder.getPulses())<70 || rightEncoder.getPulses()<70); motors.stopBothMotors(127); } @@ -1318,7 +1391,7 @@ while(abs(leftEncoder.getPulses()) < 50 || abs(rightEncoder.getPulses()) < 50); motors.stopBothMotors(127); - pc.printf("slight backwards\r\n"); + //pc.printf("slight backwards\r\n"); wait_ms(200); // Over bump @@ -1326,16 +1399,11 @@ rightEncoder.reset(); motors.setMotor0Speed(0.3*127); //right motors.setMotor1Speed(0.3*127); //left - while((abs(leftEncoder.getPulses()) < 800 || abs(rightEncoder.getPulses()) < 800) /*&& preLeft!=0*/ && IR.getIRDistance() >15 ) { - /* - preLeft=leftEncoder.getPulses(); - preRight=rightEncoder.getPulses(); - wait_ms(200); - if(leftEncoder.getPulses() == preLeft || rightEncoder.getPulses()== preRight) preLeft=preRight=0; - */ - } + while((abs(leftEncoder.getPulses()) < 900 || abs(rightEncoder.getPulses()) < 900) /*&& preLeft!=0*/ && IR.getIRDistance() >15 ); + - pc.printf("forward \r\n"); + + //pc.printf("forward \r\n"); wait_ms(200); motors.stopBothMotors(0); @@ -1358,7 +1426,8 @@ } } else if(section == MID || section == MID2) { if(section == MID2) while(IR.getIRDistance() > 15 && (abs(leftEncoder.getPulses()) < 400 || abs(rightEncoder.getPulses()) < 400)); - while(IR.getIRDistance() > 15 && (abs(leftEncoder.getPulses()) < 300 || abs(rightEncoder.getPulses()) < 300) && (leftEncoder.getPulses() != preLeft || rightEncoder.getPulses() != preRight)) { + + while(IR.getIRDistance() > 15 && (abs(leftEncoder.getPulses()) < 400 || abs(rightEncoder.getPulses()) < 400) && (leftEncoder.getPulses() != preLeft || rightEncoder.getPulses() != preRight)) { if(IR.getIRDistance() > 38) break; @@ -1383,6 +1452,7 @@ if(leftEncoder.getPulses() == preLeft || rightEncoder.getPulses()== preRight) preLeft=preRight=0; } motors.stopBothMotors(127); + motors.begin(); return; } @@ -1409,43 +1479,42 @@ void from_tools_section(float* location, float ¤t) { - alignWithWall(TOOLS); - pc.printf("align\r\n"); - wait_ms(100); + //alignWithWall(TOOLS); + //current-=4; + + //slightMove(FORWARD,150); + //current+=1; + //pc.printf("align\r\n"); + //wait_ms(200); //wall_follow2(LEFT,FORWARD,MID, current); - //current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); + rangeFinderLeft.startMeas(); wait_ms(20); rangeFinderLeft.getMeas(range); - if(range < 20) { - wall_follow2(LEFT,BACKWARD,TOOLS, current,0); - pc.printf("wall follow\r\n"); + if(range < 15) { + wall_follow2(LEFT,BACKWARD,MID, current,0); + //pc.printf("wall follow\r\n"); location[0]= current - ((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); current= location[0]; - pc.printf("current %f \r\n",current); + //pc.printf("current %f \r\n",current); // go backwards - leftEncoder.reset(); - rightEncoder.reset(); - motors.setMotor0Speed(-MAX_SPEED); //right - motors.setMotor1Speed(-MAX_SPEED); //left - while(abs(leftEncoder.getPulses()) < 120 || abs(rightEncoder.getPulses())< 120); - // hard stop - motors.stopBothMotors(127); + slightMove(BACKWARD,200); wait_ms(100); leftTurn(); + slightleft(); overBump(TOOLS); } else { - pc.printf("else greater than 20\r\n"); + //pc.printf("else greater than 20\r\n"); location[0]= current; leftTurn(); overBump(TOOLS); } - pc.printf("First Wavegap = %f\r\n",location[0]); + //pc.printf("First Wavegap = %f\r\n",location[0]); } void tools_section(float* location, float ¤t) @@ -1464,9 +1533,9 @@ wait(2); //////////////////////////////////////////////////////////////////// After tool is aquired - alignWithWall(TOOLS); - pc.printf("align\r\n"); - wait_ms(100); + //alignWithWall(TOOLS); + //pc.printf("align\r\n"); + //wait_ms(100); //wall_follow2(LEFT,FORWARD,MID, current); //current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); @@ -1477,10 +1546,10 @@ if(range < 20) { wall_follow2(LEFT,BACKWARD,TOOLS, current,0); - pc.printf("wall follow\r\n"); + //pc.printf("wall follow\r\n"); location[0]= current - ((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); current= location[0]; - pc.printf("current %f \r\n",current); + //pc.printf("current %f \r\n",current); // go backwards leftEncoder.reset(); rightEncoder.reset(); @@ -1495,32 +1564,36 @@ leftTurn(); overBump(TOOLS); } else { - pc.printf("else greater than 20\r\n"); + //pc.printf("else greater than 20\r\n"); location[0]= current; leftTurn(); overBump(TOOLS); } - pc.printf("First Wavegap = %f\r\n",location[0]); + //pc.printf("First Wavegap = %f\r\n",location[0]); } void mid_section(float* location, float ¤t, int* direction) { - motors.begin(); - if(IR.getIRDistance() > 38) { direction[0]= STRAIGHT; overBump(MID); return; } - pc.printf("before align with wall \r\n"); - alignWithWall(MID); - wait_ms(100); - - pc.printf("mid section current = %f\r\n",current); + //pc.printf("before align with wall \r\n"); + //alignWithWall(MID); + //current-=4; + //wait_ms(200); + + //if(current > 20){ + //alignWithWall(MID2); + //current-=4; + //} + rightTurn(); + //pc.printf("mid section current = %f\r\n",current); wall_follow2(LEFT,FORWARD,MID, current,0); current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); - pc.printf("after wf2 current = %f\r\n",current); + //pc.printf("after wf2 current = %f\r\n",current); wait_ms(500); rangeFinderLeft.startMeas(); @@ -1530,22 +1603,22 @@ if(range > 20 ) { direction[0]= RIGHT; location[1]= current; - slightMove(FORWARD,75); + slightMove(FORWARD,100); //current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); } else { direction[0]= LEFT; wall_follow2(LEFT,BACKWARD,MID,current,0); location[1]= current- ((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); current= location[1]; - + if(location[1] < 18) { - slightMove(FORWARD, 50); - current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); + slightMove(FORWARD, 75); } + slightMove(BACKWARD,100); } - pc.printf("wavegap2 = %f\r\n",location[1]); + //pc.printf("wavegap2 = %f\r\n",location[1]); leftTurn(); wait_ms(100); @@ -1556,9 +1629,7 @@ void mid_section2(float* location, float ¤t, int* direction) { - motors.begin(); - - pc.printf("mid section 2\r\n"); + //pc.printf("mid section 2\r\n"); if(IR.getIRDistance() > 38) { direction[1]= STRAIGHT; @@ -1566,15 +1637,19 @@ return; } - alignWithWall(MID); - pc.printf("midsection 2 alignt with wall mid \r\n"); + //alignWithWall(MID); + wait_ms(100); + rightTurn(); + wait_ms(100); + + wall_follow2(LEFT,FORWARD,MID, current,0); current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); wait_ms(500); - pc.printf("midseection 2 after wf2 %f",current); + //pc.printf("midseection 2 after wf2 %f",current); rangeFinderLeft.startMeas(); wait_ms(20); rangeFinderLeft.getMeas(range); @@ -1589,12 +1664,20 @@ wall_follow2(LEFT,BACKWARD,MID,current,0); location[2]= current- ((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); current=location[2]; - //slightMove(FORWARD,100); + slightMove(BACKWARD,100); } - leftTurn(); + //LEFT turn + motors.begin(); + leftEncoder.reset(); + rightEncoder.reset(); + motors.setMotor0Speed(0.5*127);// right + motors.setMotor1Speed(-0.5*127);// left + while(abs(leftEncoder.getPulses())<900 || rightEncoder.getPulses()<900); + motors.stopBothMotors(127); + overBump(RIGS); - pc.printf("overbump rigs\r\n"); + //pc.printf("overbump rigs\r\n"); } void rig_section(float* location, float ¤t, int* direction, int rig) @@ -1604,30 +1687,34 @@ if(rig == 1) loc= 15; else if(rig == 2) loc= 45; else loc = 75; - + // Slight forward for turn slightMove(FORWARD,100); wait_ms(100); rightTurn(); - slightright(); - wait(5); + //slightright(); + if(current > loc) { - pc.printf("RIG section %f\r\n",current); + //pc.printf("RIG section %f\r\n",current); 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); + //pc.printf("RIG section %f\r\n",current); wall_follow2(RIGHT, FORWARD, RIGS, current, rig); current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); } - - alignWithWall(RIGS); - // Go forward until Rig + + alignWithWall(MID2); + current-=4; wall_follow2(RIGHT, FORWARD, RIGS, current, rig); - // line back wheel up with side of rig - slightMove(FORWARD,300); current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); + slightMove(FORWARD, 75); + + + + + } void tools_section_return(float* location, float ¤t)