nav fixed
Dependencies: Adafruit-16-Ch-PWM-Servo-Driver HCSR04 PID PololuQik2 QEI Sharp mbed-rtos
Fork of theRobot by
Diff: main.cpp
- Revision:
- 19:d4d967a885dc
- Parent:
- 18:a0ea7ecaf4fe
- Child:
- 20:55dcff40c5d9
diff -r a0ea7ecaf4fe -r d4d967a885dc main.cpp --- a/main.cpp Thu Apr 03 22:41:24 2014 +0000 +++ b/main.cpp Fri Apr 04 01:09:49 2014 +0000 @@ -168,6 +168,7 @@ int tool_needed = 0; int shape_detected = 0; float range, range2, pid_return; +int num, input; /************ @@ -198,18 +199,18 @@ //increase in number 5 rotates gripper {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_RIG1, 162, 20, 60, 50, 175, 0}, // point laser at oilrig1 + {OIL_RIG2, 165, 20, 60, 50, 175, 0}, // point laser at oilrig2 {OIL_RIG3, 130, 90, 90, 52, 175, 0}, // point laser at oilrig2 {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 + {TOOL_1, 101, 50, 80, 113, 90, 0}, // Look over first tool + {TOOL_2, 82, 50, 80, 113, 90, 0}, // Look over second tool + {TOOL_3, 62, 50, 80, 112, 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, 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, 99, 46, 78, 110, 170, 0}, // STATIC Pickup tool POSITION + {PU_TOOL_2, 76, 47, 80, 112, 170, 0}, // STATIC Pickup tool POSITION + {PU_TOOL_3, 59, 44, 76, 110, 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 @@ -244,9 +245,16 @@ } + + + + + + int main() { + /***************** INITIALIZATIONS *******************/ @@ -273,6 +281,7 @@ /******************************** MAIN WHILE LOOP FOR COMPETITION *********************************/ + while(1) { if(button_start == 1) { @@ -1006,7 +1015,7 @@ void wall_follow2(int side, int direction, int section, float location, int rig) { int dir=1, limit=86, lowlim=5; - float set=7, loc=0, Rigloc=0; + float set=6, loc=0, Rigloc=0; bool SeeWaveGap = false; if(rig == 1) Rigloc= 16; @@ -1119,6 +1128,9 @@ } + + + void alignWithWall(int section) { float usValue = 0; @@ -1180,19 +1192,20 @@ motors.stopBothMotors(0); wait(2); - // turn left towards wall + // turn right towards wall leftEncoder.reset(); rightEncoder.reset(); motors.setMotor0Speed(-MAX_SPEED); //right motors.setMotor1Speed(MAX_SPEED); //left - while(abs(rightEncoder.getPulses()) < 20 || abs(leftEncoder.getPulses()) < 20); + while(abs(rightEncoder.getPulses()) < 50 || abs(leftEncoder.getPulses()) < 50); + + motors.stopBothMotors(127); + /* wait(2); - motors.stopBothMotors(0); - wait(2); - - // turning left - motors.setMotor0Speed(-0.9*MAX_SPEED); //right - motors.setMotor1Speed(0.9*MAX_SPEED); //left + // turning left + motors.setMotor0Speed(-0.9*MAX_SPEED); //right + motors.setMotor1Speed(0.9*MAX_SPEED); //left + */ } else {// MID pc.printf("in mid section align\r\n"); // turn right towards wall @@ -1204,7 +1217,7 @@ usValue = 0; while(1) { - if(section == RIGS) { + if(section == 10) { // CURENTLY NOT USED (WAS RIGS) rangeFinderRight.startMeas(); wait_ms(20); rangeFinderRight.getMeas(range); @@ -1392,7 +1405,7 @@ } void to_tools_section1(float* location, float ¤t) { - slightMove(FORWARD,6700); + slightMove(FORWARD,6600); current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2; } @@ -1621,6 +1634,11 @@ } alignWithWall(RIGS); + // Go forward until Rig + 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); } void tools_section_return(float* location, float ¤t)