nav fixed
Dependencies: Adafruit-16-Ch-PWM-Servo-Driver HCSR04 PID PololuQik2 QEI Sharp mbed-rtos
Fork of theRobot by
Diff: main.cpp
- Revision:
- 17:a5bb85ee205d
- Parent:
- 16:8bb212df81b7
- Child:
- 18:a0ea7ecaf4fe
--- a/main.cpp Wed Apr 02 23:54:05 2014 +0000 +++ b/main.cpp Thu Apr 03 22:05:57 2014 +0000 @@ -78,9 +78,7 @@ #define TRIANGLE 2 #define CIRCLE 3 -//*********************// -//******* TODO ********// -//*********************// + //Oil Rig distance thresholds #define OILRIG1_MAX 2200 #define OILRIG1_MIN 1000 @@ -94,6 +92,9 @@ #define MAX_SERVO_PULSE 2100 #define SERVO_MAX_ANGLE 180 +#define X_CENTER 80 +#define Y_CENTER 60 + DigitalOut myled1(LED1); DigitalOut myled2(LED2); @@ -128,6 +129,7 @@ void servoBegin(void); void initServoDriver(void); void setServoPulse(uint8_t n, int angle); +void setServoPulse2(uint8_t n, float angle); //float precision void setServoPulseNo_delay(uint8_t n, int angle); void servoPosition(int set); int fire_checker(int rig); @@ -195,19 +197,19 @@ //increase in number 5 rotates gripper - {STORE_POSITION, 85, 10, 0, 165, 90, 0}, // storing position + {STORE_POSITION, 85, 10, 0, 165, 175, 0}, // storing position {OIL_RIG1, 160, 20, 60, 45, 175, 0}, // point laser at oilrig1 {OIL_RIG2, 163, 20, 60, 45, 175, 0}, // point laser at oilrig2 {OIL_RIG3, 130, 90, 90, 52, 175, 0}, // point laser at oilrig2 - {DRIVE_POSITION_NOTOOL, 85, 5, 0, 165, 175, 0}, // Drive through course - {TOOL_1, 94, 50, 80, 109, 175, 0}, // Look over first tool - {TOOL_2, 77, 50, 80, 110, 175, 0}, // Look over second tool - {TOOL_3, 57, 50, 80, 109, 175, 0}, // Look over third tool - {DRIVE_POSITION_TOOL, 55, 70, 102, 74, 0, 0}, // Drive with tool loaded + {DRIVE_POSITION_NOTOOL, 85, 10, 0, 165, 175, 0}, // Drive through course + {TOOL_1, 96, 50, 80, 109, 90, 0}, // Look over first tool + {TOOL_2, 79, 50, 80, 110, 90, 0}, // Look over second tool + {TOOL_3, 59, 50, 80, 109, 90, 0}, // Look over third tool + {DRIVE_POSITION_TOOL, 85, 10, 0, 165, 90, 105}, // Drive with tool loaded {ORIENT_TOOL, 135, 60, 75, 60, 90, 90}, // position tool to be inserted - {PU_TOOL_1, 96, 46, 78, 104, 90, 0}, // STATIC Pickup tool POSITION - {PU_TOOL_2, 74, 47, 80, 105, 90, 0}, // STATIC Pickup tool POSITION - {PU_TOOL_3, 57, 44, 76, 104, 90, 0}, // STATIC Pickup tool POSITION + {PU_TOOL_1, 96, 46, 78, 102, 170, 0}, // STATIC Pickup tool POSITION + {PU_TOOL_2, 74, 47, 80, 104, 170, 0}, // STATIC Pickup tool POSITION + {PU_TOOL_3, 57, 44, 76, 102, 170, 0}, // STATIC Pickup tool POSITION {PU_TOOL_1_STAB, 98, 50, 90, 118, 90, 0}, // STAB TOOL 1 {PU_TOOL_2_STAB, 78, 50, 90, 108, 90, 0}, // STAB TOOL 2 {PU_TOOL_3_STAB, 53, 50, 90, 118, 90, 0}, // STAB TOOL 3 @@ -221,6 +223,7 @@ int shape; int shape_alignX_done = 0; int shape_alignY_done = 0; +float deltaX = 0; /* Variables for distance sensor*/ @@ -251,11 +254,15 @@ int direction[3]; double distance; + int pu, num, input; + pc.baud(115200); //Laser Range Finder Initialization lrf_baudCalibration(); + motors.begin(); + startBtn.rise(&button_int); //Servo initialization @@ -263,8 +270,6 @@ servoBegin(); //initiates servos to start position //ServoOutputDisable = 0; - - /******************************** MAIN WHILE LOOP FOR COMPETITION *********************************/ @@ -392,6 +397,46 @@ //printImageToFile(BINARY); shape_alignX_done = 0; shape_alignY_done = 0; + /* + while( shape_alignY_done == 0) { + wait(1); + shape_detected = shapeDetection(); + + pc.printf("Y - Adjust to center tool\n\r"); + + if(get_com_y() < 50) { + wait(1); + slightMove(FORWARD,25); + current-=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2; + + } else if(get_com_y() > 70) { + wait(1); + slightMove(BACKWARD,25); + current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2; + + } else { + shape_alignY_done = 1; + } + }*/ + + + for(int i = 0; i < 4; i++) { + shape_detected = shapeDetection(); + wait(2); + if(get_com_x() > X_CENTER ) { + deltaX = get_com_x()-X_CENTER; + setServoPulse2(0,Arm_Table[TOOL_2].base_rotate += (deltaX/60.0) ); + Arm_Table[PU_TOOL_2].base_rotate = Arm_Table[TOOL_2].base_rotate; + } + if(get_com_x() < X_CENTER) { + deltaX = get_com_x()-X_CENTER; + setServoPulse2(0,Arm_Table[TOOL_2].base_rotate += (deltaX/60.0) ); + Arm_Table[PU_TOOL_2].base_rotate = Arm_Table[TOOL_2].base_rotate; + } + } + + + /* while( shape_alignX_done == 0) { wait(1); @@ -407,14 +452,15 @@ } else { shape_alignX_done = 1; } - } + }*/ + //printImageToFile(BINARY); //either goes to aquire the tool or look at the next shape if(shape_detected == tool_needed) { state = AQUIRE_TOOL2; break; } else { - slightMove(FORWARD,25); + slightMove(FORWARD,40); current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2; @@ -422,19 +468,19 @@ wait(5); //wait for servos to settle shape_alignX_done = 0; shape_alignY_done = 0; - + /* while( shape_alignY_done == 0) { wait(1); shape_detected = shapeDetection(); pc.printf("Y - Adjust to center tool\n\r"); - if(get_com_y() < 40) { + if(get_com_y() < 50) { wait(1); slightMove(FORWARD,25); current-=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2; - } else if(get_com_y() > 80) { + } else if(get_com_y() > 70) { wait(1); slightMove(BACKWARD,25); current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2; @@ -442,23 +488,41 @@ } else { shape_alignY_done = 1; } + }*/ + + + for(int i = 0; i < 4; i++) { + shape_detected = shapeDetection(); + wait(2); + if (get_com_x() > X_CENTER) { + deltaX = get_com_x()-X_CENTER; + setServoPulse2(0,Arm_Table[TOOL_1].base_rotate += (deltaX/60.0) ); + Arm_Table[PU_TOOL_1].base_rotate = Arm_Table[TOOL_1].base_rotate; + } + if (get_com_x() < X_CENTER) { + deltaX = get_com_x()-X_CENTER; + setServoPulse2(0,Arm_Table[TOOL_1].base_rotate += (deltaX/60.0) ); + Arm_Table[PU_TOOL_1].base_rotate = Arm_Table[TOOL_1].base_rotate; + } } - while( shape_alignX_done == 0) { - wait(1); - shape_detected = shapeDetection(); - pc.printf("X - Adjust to center tool\n\r"); - if(get_com_x() > 100) { - setServoPulse(0,Arm_Table[TOOL_1].base_rotate+=1); + /* + while( shape_alignX_done == 0) { + wait(1); + shape_detected = shapeDetection(); - } else if(get_com_x() < 60) { - setServoPulse(0,Arm_Table[TOOL_1].base_rotate-=1); + pc.printf("X - Adjust to center tool\n\r"); + if(get_com_x() > 100) { + setServoPulse(0,Arm_Table[TOOL_1].base_rotate+=1); - } else { - shape_alignX_done = 1; - } - } + } else if(get_com_x() < 60) { + setServoPulse(0,Arm_Table[TOOL_1].base_rotate-=1); + + } else { + shape_alignX_done = 1; + } + }*/ if (shape_detected == tool_needed) { state = AQUIRE_TOOL1; @@ -489,52 +553,108 @@ setServoPulse(5, 105); wait(.5); setServoPulse(5, 00); - setServoPulse(1, Arm_Table[PU_TOOL_1].base_arm - 8); + setServoPulse(1, Arm_Table[PU_TOOL_1].base_arm - 10); wait(1); setServoPulse(5, 105); wait(1); setServoPulse(1, Arm_Table[PU_TOOL_1].base_arm); + wait(2); - while(1); + setServoPulse(2, Arm_Table[DRIVE_POSITION_TOOL].big_arm); + setServoPulse(3, Arm_Table[DRIVE_POSITION_TOOL].claw_arm); + setServoPulse(1, Arm_Table[DRIVE_POSITION_TOOL].base_arm); + setServoPulse(0, Arm_Table[DRIVE_POSITION_TOOL].base_rotate); + setServoPulse(4, Arm_Table[DRIVE_POSITION_TOOL].claw_rotate); + setServoPulse(5, Arm_Table[DRIVE_POSITION_TOOL].claw_open); - servoPosition(DRIVE_POSITION_TOOL); //arm position for driving with the tool + state = NAVIGATE_WAVES_ROW1; break; case AQUIRE_TOOL2: servoPosition(PU_TOOL_2); - setServoPulse(4, 170); + setServoPulse(4, 175); wait(5); setServoPulse(0, Arm_Table[PU_TOOL_2].base_rotate ); wait(1); - setServoPulse(1, Arm_Table[PU_TOOL_2].base_arm - 5); + setServoPulse(1, Arm_Table[PU_TOOL_2].base_arm - 2); + wait(1); + setServoPulse(3, Arm_Table[PU_TOOL_2].claw_arm - 2); wait(1); setServoPulse(5, 105); - wait(.5); + wait(2); setServoPulse(5, 00); setServoPulse(1, Arm_Table[PU_TOOL_2].base_arm - 6); - wait(.5); + wait(2); setServoPulse(5, 105); - wait(.5); + wait(2); setServoPulse(5, 00); - setServoPulse(1, Arm_Table[PU_TOOL_2].base_arm - 8); - wait(1); + setServoPulse(1, Arm_Table[PU_TOOL_2].base_arm - 10); + wait(2); setServoPulse(5, 105); - wait(1); + wait(2); setServoPulse(1, Arm_Table[PU_TOOL_2].base_arm); - while(1); + + wait(2); - servoPosition(DRIVE_POSITION_TOOL); //arm position for driving with the tool + setServoPulse(2, Arm_Table[DRIVE_POSITION_TOOL].big_arm); + setServoPulse(3, Arm_Table[DRIVE_POSITION_TOOL].claw_arm); + setServoPulse(1, Arm_Table[DRIVE_POSITION_TOOL].base_arm); + setServoPulse(0, Arm_Table[DRIVE_POSITION_TOOL].base_rotate); + setServoPulse(4, Arm_Table[DRIVE_POSITION_TOOL].claw_rotate); + setServoPulse(5, Arm_Table[DRIVE_POSITION_TOOL].claw_open); + + state = NAVIGATE_WAVES_ROW1; break; case AQUIRE_TOOL3: - servoPosition(PU_TOOL_3); + /* + while( shape_alignY_done == 0) { + wait(1); + + servoPosition(PU_TOOL_3); + shape_detected = shapeDetection(); + wait(2); + + pc.printf("Y - Adjust to center tool\n\r"); + + if(get_com_y() < 50) { + wait(1); + slightMove(FORWARD,25); + current-=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2; + + } else if(get_com_y() > 70) { + wait(1); + slightMove(BACKWARD,25); + current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2; + + } else { + shape_alignY_done = 1; + } + } + + */ + for(int i = 0; i < 4; i++) { + shape_detected = shapeDetection(); + wait(2); + if (get_com_x() > X_CENTER) { + deltaX = get_com_x()-X_CENTER; + setServoPulse2(0,Arm_Table[PU_TOOL_3].base_rotate += (deltaX/60.0) ); + } + if (get_com_x() < X_CENTER) { + deltaX = get_com_x()-X_CENTER; + setServoPulse2(0,Arm_Table[PU_TOOL_3].base_rotate += (deltaX/60.0) ); + } + } + setServoPulse(4, 130); wait(5); setServoPulse(0, Arm_Table[PU_TOOL_3].base_rotate ); wait(1); - setServoPulse(1, Arm_Table[PU_TOOL_3].base_arm - 5); + setServoPulse(1, Arm_Table[PU_TOOL_2].base_arm - 2); + wait(1); + setServoPulse(3, Arm_Table[PU_TOOL_2].claw_arm - 3); wait(1); setServoPulse(5, 105); wait(.5); @@ -544,14 +664,21 @@ setServoPulse(5, 105); wait(.5); setServoPulse(5, 00); - setServoPulse(1, Arm_Table[PU_TOOL_3].base_arm - 8); + setServoPulse(1, Arm_Table[PU_TOOL_3].base_arm - 10); wait(1); setServoPulse(5, 105); wait(1); setServoPulse(1, Arm_Table[PU_TOOL_3].base_arm); - while(1); + + wait(2); - servoPosition(DRIVE_POSITION_TOOL); //arm position for driving with the tool + setServoPulse(2, Arm_Table[DRIVE_POSITION_TOOL].big_arm); + setServoPulse(3, Arm_Table[DRIVE_POSITION_TOOL].claw_arm); + setServoPulse(1, Arm_Table[DRIVE_POSITION_TOOL].base_arm); + setServoPulse(0, Arm_Table[DRIVE_POSITION_TOOL].base_rotate); + setServoPulse(4, Arm_Table[DRIVE_POSITION_TOOL].claw_rotate); + setServoPulse(5, Arm_Table[DRIVE_POSITION_TOOL].claw_open); + state = NAVIGATE_WAVES_ROW1; break; @@ -564,33 +691,31 @@ **************************************************/ case NAVIGATE_WAVES_ROW1: - //*********************// - //******* TODO ********// - //*********************// - // CODE TO NAVIGATE ROW1 + from_tools_section(location,current); + + mid_section(location, current, direction); + state = NAVIGATE_WAVES_ROW2; break; case NAVIGATE_WAVES_ROW2: - //*********************// - //******* TODO ********// - //*********************// - // CODE TO NAVIGATE ROW2 + mid_section2(location, current, direction); state = NAVIGATE_WAVES_ROW3; break; case NAVIGATE_WAVES_ROW3: - //*********************// - //******* TODO ********// - //*********************// - // CODE TO NAVIGATE ROW3 - //goes to appropriate rig if(shape_detected == 1) { + rig_section(location, current, direction, 1); + while(1); state = NAVIGATE_TO_SQUARE_RIG; } else if(shape_detected == 2) { + rig_section(location, current, direction, 2); + while(1); state = NAVIGATE_TO_TRIANGLE_RIG; } else { + rig_section(location, current, direction, 3); + while(1); state = NAVIGATE_TO_CIRCLE_RIG; } break; @@ -740,6 +865,13 @@ pwm.setPWM(n, 0, pulse); } +void setServoPulse2(uint8_t n, float angle) +{ + float pulse = MIN_SERVO_PULSE + (( angle * (MAX_SERVO_PULSE - MIN_SERVO_PULSE)) / SERVO_MAX_ANGLE); + float pulselength = 20000; // 10,000 us per second + pulse = 4094 * pulse / pulselength; + pwm.setPWM(n, 0, pulse); +} @@ -874,7 +1006,7 @@ 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; + float set=7, loc=0, Rigloc=0; bool SeeWaveGap = false; if(rig == 1) Rigloc= 16; @@ -928,10 +1060,7 @@ if(range2< 20) { if( abs(dir*loc + location - Rigloc) < 10) { //STOP - motors.setMotor0Speed(dir*-0.25*127); //right - motors.setMotor1Speed(dir*-0.25*127); //left - wait_ms(5); - motors.stopBothMotors(0); + motors.stopBothMotors(127); break; } } @@ -949,10 +1078,7 @@ SeeWaveGap=true; } else { //STOP - motors.setMotor0Speed(dir*-0.25*127); //right - motors.setMotor1Speed(dir*-0.25*127); //left - wait_ms(5); - motors.stopBothMotors(0); + motors.stopBothMotors(127); pc.printf("wavegap\r\n"); // AT WAVE OPENING!!!! @@ -989,10 +1115,7 @@ } //STOP - motors.setMotor0Speed(dir*-0.3*127); //right - motors.setMotor1Speed(dir*-0.3*127); //left - wait_ms(5); - motors.stopBothMotors(0); + motors.stopBothMotors(127); } @@ -1031,29 +1154,31 @@ motors.setMotor0Speed(0.9*MAX_SPEED); //right motors.setMotor1Speed(-0.9*MAX_SPEED); //left - } else if( section == RIGS) { + } else if(section == RIGS) { // check distance to wall rangeFinderRight.startMeas(); wait_ms(20); rangeFinderRight.getMeas(range); - if(range < 4 || range > 20) return; + if(range < 3 || range > 20) return; // turn 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())<1000); + 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()) < 150 || abs(rightEncoder.getPulses()) < 150); + while(abs(leftEncoder.getPulses()) < 200 || abs(rightEncoder.getPulses()) < 200); motors.stopBothMotors(0); + wait(2); // turn left towards wall leftEncoder.reset(); @@ -1063,11 +1188,12 @@ while(abs(rightEncoder.getPulses()) < 20 || abs(leftEncoder.getPulses()) < 20); motors.stopBothMotors(0); + wait(2); // turning left motors.setMotor0Speed(-0.9*MAX_SPEED); //right motors.setMotor1Speed(0.9*MAX_SPEED); //left - } else { + } else {// MID pc.printf("in mid section align\r\n"); // turn right towards wall rightTurn(); @@ -1104,8 +1230,8 @@ 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(0); + while(abs(leftEncoder.getPulses())<1050 || abs(rightEncoder.getPulses())<1050); + motors.stopBothMotors(127); } void leftTurn(void) @@ -1116,7 +1242,7 @@ motors.setMotor0Speed(0.5*127);// right motors.setMotor1Speed(-0.5*127);// left while(abs(leftEncoder.getPulses())<1100 || rightEncoder.getPulses()<1100); - motors.stopBothMotors(0); + motors.stopBothMotors(127); } void slightleft(void) @@ -1127,7 +1253,7 @@ motors.setMotor0Speed(0.5*127);// right motors.setMotor1Speed(-0.5*127);// left while(abs(leftEncoder.getPulses())<90 || rightEncoder.getPulses()<90); - motors.stopBothMotors(0); + motors.stopBothMotors(127); } void slightright(void) @@ -1137,8 +1263,8 @@ rightEncoder.reset(); motors.setMotor0Speed(-0.4*127);// right motors.setMotor1Speed(0.4*127);// left - while(abs(leftEncoder.getPulses())<90 || abs(rightEncoder.getPulses())<90); - motors.stopBothMotors(0); + while(abs(leftEncoder.getPulses())<50 || abs(rightEncoder.getPulses())<50); + motors.stopBothMotors(127); } void slightMove(int direction, float pulses) @@ -1153,9 +1279,6 @@ motors.setMotor1Speed(dir*0.25*127); //left while(abs(leftEncoder.getPulses()) < pulses || abs(rightEncoder.getPulses()) < pulses); - motors.setMotor0Speed(dir*-0.25*127); //right - motors.setMotor1Speed(dir*-0.25*127); //left - wait_ms(10); motors.stopBothMotors(127); } @@ -1177,10 +1300,7 @@ rangeFinderRight.getMeas(range); } - motors.setMotor0Speed(dir*-0.2*127); //right - motors.setMotor1Speed(dir*-0.2*127); //left - wait_ms(5); - motors.stopBothMotors(0); + motors.stopBothMotors(127); } void overBump(int section) @@ -1194,53 +1314,27 @@ motors.setMotor0Speed(-0.25*127); //right motors.setMotor1Speed(-0.25*127); //left while(abs(leftEncoder.getPulses()) < 50 || abs(rightEncoder.getPulses()) < 50); - motors.stopBothMotors(0); + motors.stopBothMotors(127); pc.printf("slight backwards\r\n"); wait_ms(200); + // Over bump leftEncoder.reset(); 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(); + /* + preLeft=leftEncoder.getPulses(); preRight=rightEncoder.getPulses(); wait_ms(200); - if(leftEncoder.getPulses() == preLeft || rightEncoder.getPulses()== preRight) preLeft=preRight=0;*/ + if(leftEncoder.getPulses() == preLeft || rightEncoder.getPulses()== preRight) preLeft=preRight=0; + */ } pc.printf("forward \r\n"); wait_ms(200); - /* - motors.stopBothMotors(0); - motors.begin(); - - leftEncoder.reset(); - rightEncoder.reset(); - motors.setMotor0Speed(0.3*127); //right - motors.setMotor1Speed(0.3*127); //left - - while(!out) { - preLeft=leftEncoder.getPulses(); - preRight=rightEncoder.getPulses(); - - rangeFinderLeft.startMeas(); - rangeFinderRight.startMeas(); - wait_ms(20); - rangeFinderLeft.getMeas(range); - rangeFinderRight.getMeas(range2); - if(range < 10 || range2 < 10) out=1; - - if(leftEncoder.getPulses() == preLeft || rightEncoder.getPulses()== preRight) { - motors.setMotor0Speed(0.4*127); //right - motors.setMotor1Speed(0.4*127); //left - wait_ms(50); - out=1; - } - if(abs(leftEncoder.getPulses()) >1000 || abs(leftEncoder.getPulses())>1000) out=1; - } - */ motors.stopBothMotors(0); motors.begin(); @@ -1252,7 +1346,7 @@ motors.setMotor1Speed(.25*127); //left if(section == TOOLS) { - while(IR.getIRDistance() > 10 && (abs(leftEncoder.getPulses()) < 300 || abs(rightEncoder.getPulses()) < 300) && (leftEncoder.getPulses() != preLeft || rightEncoder.getPulses() != preRight)) { + while(IR.getIRDistance() > 10 && (abs(leftEncoder.getPulses()) < 200 || abs(rightEncoder.getPulses()) < 200) && (leftEncoder.getPulses() != preLeft || rightEncoder.getPulses() != preRight)) { if(IR.getIRDistance() > 38) break; @@ -1262,7 +1356,7 @@ } } 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()) < 400 || abs(rightEncoder.getPulses()) < 400) && (leftEncoder.getPulses() != preLeft || rightEncoder.getPulses() != preRight)) { + while(IR.getIRDistance() > 15 && (abs(leftEncoder.getPulses()) < 300 || abs(rightEncoder.getPulses()) < 300) && (leftEncoder.getPulses() != preLeft || rightEncoder.getPulses() != preRight)) { if(IR.getIRDistance() > 38) break; @@ -1271,9 +1365,10 @@ wait_ms(200); } - } else { - while(abs(leftEncoder.getPulses()) < 100 || abs(rightEncoder.getPulses()) < 100); + } else {// RIGS + while(abs(leftEncoder.getPulses()) < 220 || abs(rightEncoder.getPulses()) < 220); + // go backwards to line up with bump leftEncoder.reset(); rightEncoder.reset(); @@ -1285,35 +1380,19 @@ wait_ms(200); if(leftEncoder.getPulses() == preLeft || rightEncoder.getPulses()== preRight) preLeft=preRight=0; } - - leftEncoder.reset(); - rightEncoder.reset(); - - motors.setMotor0Speed(0.25*127); //right - motors.setMotor1Speed(0.25*127); //left - while((abs(leftEncoder.getPulses()) < 10 || abs(rightEncoder.getPulses()) < 10)); - - motors.stopBothMotors(0); + motors.stopBothMotors(127); return; } - leftEncoder.reset(); - rightEncoder.reset(); - - motors.setMotor0Speed(-.25*127); //right - motors.setMotor1Speed(-.25*127); //left - while((abs(leftEncoder.getPulses()) < 10 || abs(rightEncoder.getPulses()) < 10)); - - motors.stopBothMotors(0); + motors.stopBothMotors(127); wait_ms(20); motors.begin(); } - void to_tools_section1(float* location, float ¤t) { - slightMove(FORWARD,6600); + slightMove(FORWARD,6700); current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2; } @@ -1328,7 +1407,6 @@ void from_tools_section(float* location, float ¤t) { - alignWithWall(TOOLS); pc.printf("align\r\n"); wait_ms(100); @@ -1353,12 +1431,7 @@ motors.setMotor1Speed(-MAX_SPEED); //left while(abs(leftEncoder.getPulses()) < 120 || abs(rightEncoder.getPulses())< 120); // hard stop - leftEncoder.reset(); - rightEncoder.reset(); - motors.setMotor0Speed(MAX_SPEED); //right - motors.setMotor1Speed(MAX_SPEED); //left - while(abs(leftEncoder.getPulses()) < 10 || abs(rightEncoder.getPulses())< 10); - motors.stopBothMotors(0); + motors.stopBothMotors(127); wait_ms(100); leftTurn(); @@ -1373,6 +1446,61 @@ pc.printf("First Wavegap = %f\r\n",location[0]); } +void tools_section(float* location, float ¤t) +{ + wall_follow(LEFT,FORWARD, TOOLS); + // current position in reference to the starting position + current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2; + + //////////////////////////////// determine tool + wait(2); + /////////////////////////////////////////////////////////////////////////////////////// + // Move Forward + slightMove(FORWARD, 100); + + //////////////////////////////////////////Tool aquiring + wait(2); + //////////////////////////////////////////////////////////////////// After tool is aquired + + 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); + + rangeFinderLeft.startMeas(); + wait_ms(20); + rangeFinderLeft.getMeas(range); + + if(range < 20) { + wall_follow2(LEFT,BACKWARD,TOOLS, 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); + // 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); + + wait_ms(100); + leftTurn(); + overBump(TOOLS); + } else { + pc.printf("else greater than 20\r\n"); + location[0]= current; + leftTurn(); + overBump(TOOLS); + } + + pc.printf("First Wavegap = %f\r\n",location[0]); +} void mid_section(float* location, float ¤t, int* direction) { @@ -1452,14 +1580,14 @@ if(range > 20 ) { direction[1]= RIGHT; location[2]= current; - slightMove(FORWARD,75); - //current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); + slightMove(FORWARD,100); + current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); } else { direction[1]= LEFT; 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,500); + //slightMove(FORWARD,100); } leftTurn(); @@ -1475,8 +1603,12 @@ else if(rig == 2) loc= 45; else loc = 75; + // Slight forward for turn + slightMove(FORWARD,100); + wait_ms(100); rightTurn(); slightright(); + wait(5); if(current > loc) { pc.printf("RIG section %f\r\n",current); @@ -1487,6 +1619,8 @@ wall_follow2(RIGHT, FORWARD, RIGS, current, rig); current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); } + + alignWithWall(RIGS); } void tools_section_return(float* location, float ¤t) @@ -1536,7 +1670,6 @@ void rig_section_return(float* location, float ¤t, int* direction) { - alignWithWall(RIGS); if(location[2] > current) { wall_follow2(RIGHT, FORWARD, MID, current,0); current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); @@ -1546,9 +1679,4 @@ } rightTurn(); overBump(MID2); -} - - - - - +} \ No newline at end of file