uyvug
Dependencies: Adafruit-16-Ch-PWM-Servo-Driver HCSR04 PID PololuQik2 QEI Sharp mbed-rtos
Fork of theRobot by
Diff: main.cpp
- Revision:
- 13:529323807361
- Parent:
- 12:284be46593ae
- Child:
- 14:784acd735b8c
diff -r 284be46593ae -r 529323807361 main.cpp --- a/main.cpp Wed Apr 02 00:30:30 2014 +0000 +++ b/main.cpp Wed Apr 02 03:30:48 2014 +0000 @@ -129,7 +129,6 @@ void setServoPulse(uint8_t n, int angle); void setServoPulseNo_delay(uint8_t n, int angle); void servoPosition(int set); -void setGripper(int open); int fire_checker(int rig); @@ -217,6 +216,8 @@ int y_coord; int area; int shape; +int shape_alignX_done = 0; + /* Variables for distance sensor*/ int distLaser; @@ -246,118 +247,90 @@ //ServoOutputDisable = 0; - - - - - /******************* WHILE LOOP FOR TESTING ********************/ //while(1) { -/* - pc.scanf("%d %d", &servoNum, &servoAngle); - if(servoAngle > 175) { - servoAngle = 175; - } - if(servoNum > 5 ) { - servoNum = 0; - servoAngle = 90; - } - setServoPulse(servoNum, servoAngle); - distLaser = getDistance(); - pc.printf("Distance %d", distLaser); -*/ - /* - while(pc.getc() != 'g'); - servoPosition(TOOL_2); - wait(3); - //shape_detected = shapeDetection(); - //clearBounds(); - ImageToArray(GREYSCALE); - printImageToFile(GREYSCALE); - while(pc.getc() != 'g'); - servoPosition(TOOL_3); - wait(3); - shape_detected = shapeDetection(); - printImageToFile(BINARY); - while(pc.getc() != 'g'); - servoPosition(TOOL_3); - wait(3); - shape_detected = shapeDetection(); - while(1){};*/ + /* + pc.scanf("%d %d", &servoNum, &servoAngle); + if(servoAngle > 175) { + servoAngle = 175; + } + if(servoNum > 5 ) { + servoNum = 0; + servoAngle = 90; + } + setServoPulse(servoNum, servoAngle); + distLaser = getDistance(); + pc.printf("Distance %d", distLaser); + */ - - //shape_detected = shapeDetection(); - //distLaser = getDistance(); - //pc.printf("Distance %d", distLaser); - //ledtoggle(); - /* - int shape_alignX_done = 0; - int shape_alignY_done = 0; + /* + int shape_alignX_done = 0; + int shape_alignY_done = 0; - //pc.scanf("%d", &posNum); + //pc.scanf("%d", &posNum); - while(pc.getc() != 'g'); - servoPosition(TOOL_1); - while(pc.getc() != 'g'); - //shape_detected = shapeDetection(); + while(pc.getc() != 'g'); + servoPosition(TOOL_1); + while(pc.getc() != 'g'); + //shape_detected = shapeDetection(); - ImageToArray(GREYSCALE); - //clearBounds(); - printImageToFile(GREYSCALE); - while(1); + ImageToArray(GREYSCALE); + //clearBounds(); + printImageToFile(GREYSCALE); + while(1); - while(shape_alignY_done == 0) { - shape_detected = shapeDetection(); - pc.printf("\nY movement\n"); - if(get_com_y() >= 80) { - setServoPulse(1, Arm_Table[TOOL_1].base_arm-=2); - setServoPulse(2, Arm_Table[TOOL_1].big_arm-=1); - //setServoPulse(3, Arm_Table[TOOL_1].claw_arm+=2); - } else if(get_com_y() > 70 && get_com_y() < 90) { - setServoPulse(1, Arm_Table[TOOL_1].base_arm-=1); - setServoPulse(2, Arm_Table[TOOL_1].big_arm-=1); - //setServoPulse(3, Arm_Table[TOOL_1].claw_arm+=1); - } else if(get_com_y() <= 35 ) { - setServoPulse(1, Arm_Table[TOOL_1].base_arm+=2); - setServoPulse(2, Arm_Table[TOOL_1].big_arm+=1); - //setServoPulse(3, Arm_Table[TOOL_1].claw_arm-=2); - } else if(get_com_y() < 50 && get_com_y() > 20) { - setServoPulse(1, Arm_Table[TOOL_1].base_arm+=1); - setServoPulse(2, Arm_Table[TOOL_1].big_arm+=1); - //setServoPulse(3, Arm_Table[TOOL_1].claw_arm-=1); - } else { - shape_alignY_done = 1; - } - } + while(shape_alignY_done == 0) { + shape_detected = shapeDetection(); + pc.printf("\nY movement\n"); + if(get_com_y() >= 80) { + setServoPulse(1, Arm_Table[TOOL_1].base_arm-=2); + setServoPulse(2, Arm_Table[TOOL_1].big_arm-=1); + //setServoPulse(3, Arm_Table[TOOL_1].claw_arm+=2); + } else if(get_com_y() > 70 && get_com_y() < 90) { + setServoPulse(1, Arm_Table[TOOL_1].base_arm-=1); + setServoPulse(2, Arm_Table[TOOL_1].big_arm-=1); + //setServoPulse(3, Arm_Table[TOOL_1].claw_arm+=1); + } else if(get_com_y() <= 35 ) { + setServoPulse(1, Arm_Table[TOOL_1].base_arm+=2); + setServoPulse(2, Arm_Table[TOOL_1].big_arm+=1); + //setServoPulse(3, Arm_Table[TOOL_1].claw_arm-=2); + } else if(get_com_y() < 50 && get_com_y() > 20) { + setServoPulse(1, Arm_Table[TOOL_1].base_arm+=1); + setServoPulse(2, Arm_Table[TOOL_1].big_arm+=1); + //setServoPulse(3, Arm_Table[TOOL_1].claw_arm-=1); + } else { + shape_alignY_done = 1; + } + } - while( shape_alignX_done == 0){ - shape_detected = shapeDetection(); - pc.printf("\nX movement\n"); - if(get_com_x() > 95) { - Arm_Table[TOOL_1].base_rotate+=1; - setServoPulse(0, Arm_Table[TOOL_1].base_rotate); - setServoPulse(0, Arm_Table[TOOL_1].base_rotate-1); - setServoPulse(0, Arm_Table[TOOL_1].base_rotate+1); + while( shape_alignX_done == 0){ + shape_detected = shapeDetection(); + pc.printf("\nX movement\n"); + if(get_com_x() > 95) { + Arm_Table[TOOL_1].base_rotate+=1; + setServoPulse(0, Arm_Table[TOOL_1].base_rotate); + setServoPulse(0, Arm_Table[TOOL_1].base_rotate-1); + setServoPulse(0, Arm_Table[TOOL_1].base_rotate+1); - } else if(get_com_x() < 65) { - Arm_Table[TOOL_1].base_rotate-=1; - setServoPulse(0, Arm_Table[TOOL_1].base_rotate); - setServoPulse(0, Arm_Table[TOOL_1].base_rotate-1); - setServoPulse(0, Arm_Table[TOOL_1].base_rotate+1); + } else if(get_com_x() < 65) { + Arm_Table[TOOL_1].base_rotate-=1; + setServoPulse(0, Arm_Table[TOOL_1].base_rotate); + setServoPulse(0, Arm_Table[TOOL_1].base_rotate-1); + setServoPulse(0, Arm_Table[TOOL_1].base_rotate+1); - } else { - shape_alignX_done = 1; - } + } else { + shape_alignX_done = 1; + } - } - printImageToFile(BINARY);*/ + } + printImageToFile(BINARY);*/ - // } + // } @@ -378,11 +351,12 @@ **************************************************/ case START : myled1 = 1; - while(pc.getc() != 'g'); + while(pc.getc() != 'g'); //waits for the letter g before starting program myled1 = 0; state = OILRIG1_POS; break; + /************************************************** * STAGE 1 * @@ -391,57 +365,42 @@ **************************************************/ case OILRIG1_POS: //aims arm at square oil rig - setServoPulse(3, Arm_Table[OIL_RIG1].claw_arm); - setServoPulse(2, Arm_Table[OIL_RIG1].big_arm); - setServoPulse(1, Arm_Table[OIL_RIG1].base_arm); - setServoPulse(0, Arm_Table[OIL_RIG1].base_rotate); - setServoPulse(4, Arm_Table[OIL_RIG1].claw_rotate); - setServoPulse(5, Arm_Table[OIL_RIG1].claw_open); - wait(3); - //lrf.putc('E'); // lighting calculation - //wait(15); + 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 - pc.printf("FIRE: SQUARE %d", fire); //determines what tool is needed if (fire == 1) { + pc.printf("FIRE FOUND!!!!!!!!\n\r"); tool_needed = SQUARE; state = GOTO_TOOLS1; } else { + 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); - 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); - + servoPosition(DRIVE_POSITION_NOTOOL); wait(3); //wait for servos to settle - to_tools_section2(location, current); + to_tools_section2(location, current); // moves to second rig servoPosition(OIL_RIG2); //position arm to point at first oilrig - wait(2); - //lrf.putc('E'); // lighting calculation - //wait(15); + wait(3); + fire = fire_checker(OIL_RIG2); - pc.printf("FIRE: TRIANGLE %d", fire); if (fire == 1) { + pc.printf("FIRE FOUND!!!!!!!!"); tool_needed = TRIANGLE; state = GOTO_TOOLS2; } else { + pc.printf("XXXXXX FIRE NOT FOUND XXXXXX"); tool_needed = CIRCLE; state = GOTO_TOOLS2; } - pc.printf("tool needed: %d", tool_needed); break; /************************************************** @@ -452,29 +411,16 @@ **************************************************/ 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(5); //wait for servos to settle + servoPosition(DRIVE_POSITION_NOTOOL); + 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(5); //wait for servos to settle + servoPosition(DRIVE_POSITION_NOTOOL); + wait(3); //wait for servos to settle slightMove(FORWARD,3100); current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2; @@ -482,6 +428,8 @@ state = IDENTIFY_TOOLS; break; + + /************************************************** * STAGE 3 * @@ -493,1076 +441,1054 @@ servoPosition(TOOL_2); //arm/camera looks over tool wait(5); //wait for servos to settle - //centerCamWithTool(); //centers camera over tool - shape_detected = shapeDetection(); //determines the shape - clearBounds(); + + //shape_detected = shapeDetection(); //determines the shape + //clearBounds(); //printImageToFile(BINARY); - //either goes to aquire the tool or look at the next shape - if(shape_detected == tool_needed) { - state = AQUIRE_TOOL2; - } else { - servoPosition(TOOL_1); - wait(1); //wait for servos to settle - //centerCamWithTool(); + while( shape_alignX_done == 0) { shape_detected = shapeDetection(); - if (shape_detected == tool_needed) { - state = AQUIRE_TOOL1; + pc.printf("X - Adjust to center tool\n\r"); + if(get_com_x() > 95) { + Arm_Table[TOOL_1].base_rotate+=1; + + } else if(get_com_x() < 65) { + Arm_Table[TOOL_1].base_rotate-=1; + } else { - servoPosition(TOOL_3); - wait(1); //wait for servos to settle - centerCamWithTool(); - state = AQUIRE_TOOL3; + shape_alignX_done = 1; } - } - while(1); - break; - case AQUIRE_TOOL1: - - servoPosition(PU_TOOL_1_STAB); - wait(2); - - servoPosition(DRIVE_POSITION_TOOL); //arm position for driving with the tool - state = NAVIGATE_WAVES_ROW1; - break; - case AQUIRE_TOOL2: - servoPosition(PU_TOOL_2_STAB); - wait(2); - - servoPosition(DRIVE_POSITION_TOOL); //arm position for driving with the tool - state = NAVIGATE_WAVES_ROW1; - break; - - case AQUIRE_TOOL3: - servoPosition(PU_TOOL_3_STAB); - wait(2); - servoPosition(DRIVE_POSITION_TOOL); //arm position for driving with the tool - state = NAVIGATE_WAVES_ROW1; - break; + //either goes to aquire the tool or look at the next shape + if(shape_detected == tool_needed) { + state = AQUIRE_TOOL2; + } else { + servoPosition(TOOL_1); + wait(3); //wait for servos to settle + + shape_alignX_done = 0; + while( shape_alignX_done == 0) { + + shape_detected = shapeDetection(); + pc.printf("X - Adjust to center tool\n\r"); + if(get_com_x() > 95) { + Arm_Table[TOOL_1].base_rotate+=1; + + } else if(get_com_x() < 65) { + Arm_Table[TOOL_1].base_rotate-=1; + + } else { + shape_alignX_done = 1; + } + + if (shape_detected == tool_needed) { + state = AQUIRE_TOOL; + } else { + servoPosition(TOOL_3); + wait(3); //wait for servos to settle + shape_detected = shapeDetection(); + pc.printf("X - Adjust to center tool\n\r"); + if(get_com_x() > 95) { + Arm_Table[TOOL_1].base_rotate+=1; + + } else if(get_com_x() < 65) { + Arm_Table[TOOL_1].base_rotate-=1; + + } else { + shape_alignX_done = 1; + } + + state = AQUIRE_TOOL3; + } + } + + while(1); + break; + + case AQUIRE_TOOL1: + + servoPosition(PU_TOOL_1_STAB); + wait(2); + + servoPosition(DRIVE_POSITION_TOOL); //arm position for driving with the tool + state = NAVIGATE_WAVES_ROW1; + break; + + case AQUIRE_TOOL2: + servoPosition(PU_TOOL_2_STAB); + wait(2); + + servoPosition(DRIVE_POSITION_TOOL); //arm position for driving with the tool + state = NAVIGATE_WAVES_ROW1; + break; + + case AQUIRE_TOOL3: + servoPosition(PU_TOOL_3_STAB); + wait(2); + + + servoPosition(DRIVE_POSITION_TOOL); //arm position for driving with the tool + state = NAVIGATE_WAVES_ROW1; + break; - /************************************************** - * STAGE 4 - * - * - Navigate through the ocean - * - **************************************************/ + /************************************************** + * STAGE 4 + * + * - Navigate through the ocean + * + **************************************************/ - case NAVIGATE_WAVES_ROW1: - //*********************// - //******* TODO ********// - //*********************// - // CODE TO NAVIGATE ROW1 - state = NAVIGATE_WAVES_ROW2; - break; + case NAVIGATE_WAVES_ROW1: + //*********************// + //******* TODO ********// + //*********************// + // CODE TO NAVIGATE ROW1 + state = NAVIGATE_WAVES_ROW2; + break; - case NAVIGATE_WAVES_ROW2: - //*********************// - //******* TODO ********// - //*********************// - // CODE TO NAVIGATE ROW2 - state = NAVIGATE_WAVES_ROW3; - break; + case NAVIGATE_WAVES_ROW2: + //*********************// + //******* TODO ********// + //*********************// + // CODE TO NAVIGATE ROW2 + state = NAVIGATE_WAVES_ROW3; + break; - case NAVIGATE_WAVES_ROW3: - //*********************// - //******* TODO ********// - //*********************// - // CODE TO NAVIGATE ROW3 + case NAVIGATE_WAVES_ROW3: + //*********************// + //******* TODO ********// + //*********************// + // CODE TO NAVIGATE ROW3 - //goes to appropriate rig - if(shape_detected == 1) { - state = NAVIGATE_TO_SQUARE_RIG; - } else if(shape_detected == 2) { - state = NAVIGATE_TO_TRIANGLE_RIG; - } else { - state = NAVIGATE_TO_CIRCLE_RIG; - } - break; + //goes to appropriate rig + if(shape_detected == 1) { + state = NAVIGATE_TO_SQUARE_RIG; + } else if(shape_detected == 2) { + state = NAVIGATE_TO_TRIANGLE_RIG; + } else { + state = NAVIGATE_TO_CIRCLE_RIG; + } + break; - /************************************************** - * STAGE 5 - * - * - Travel to appropriate rig - * - **************************************************/ - case NAVIGATE_TO_SQUARE_RIG: - //NAVIGATION CODE HERE - state = RIG_ALIGN; - break; - case NAVIGATE_TO_TRIANGLE_RIG: - //NAVIGATION CODE HERE - state = RIG_ALIGN; - break; - case NAVIGATE_TO_CIRCLE_RIG: - //NAVIGATION CODE HERE - state = RIG_ALIGN; - break; + /************************************************** + * STAGE 5 + * + * - Travel to appropriate rig + * + **************************************************/ + case NAVIGATE_TO_SQUARE_RIG: + //NAVIGATION CODE HERE + state = RIG_ALIGN; + break; + case NAVIGATE_TO_TRIANGLE_RIG: + //NAVIGATION CODE HERE + state = RIG_ALIGN; + break; + case NAVIGATE_TO_CIRCLE_RIG: + //NAVIGATION CODE HERE + state = RIG_ALIGN; + break; - /************************************************** - * STAGE 6 - * - * - Align with appropriate rig - * - **************************************************/ - case RIG_ALIGN: + /************************************************** + * STAGE 6 + * + * - Align with appropriate rig + * + **************************************************/ + case RIG_ALIGN: - //*********************// - //******* TODO ********// - //*********************// - // CODE TO ALIGN ROBOT WITH RIG + //*********************// + //******* TODO ********// + //*********************// + // CODE TO ALIGN ROBOT WITH RIG - servoPosition(ORIENT_TOOL); - wait(1); //wait for servos to settle - state = INSERT_TOOL; - break; + servoPosition(ORIENT_TOOL); + wait(1); //wait for servos to settle + state = INSERT_TOOL; + break; - /************************************************** - * STAGE 7 - * - * - Insert Tool - * - Extenguish fire - * - win contest - * - **************************************************/ + /************************************************** + * STAGE 7 + * + * - Insert Tool + * - Extenguish fire + * - win contest + * + **************************************************/ - case INSERT_TOOL: - //*********************// - //******* TODO ********// - //*********************// - // CODE TO INSERT TOOL - break; + case INSERT_TOOL: + //*********************// + //******* TODO ********// + //*********************// + // CODE TO INSERT TOOL + break; - /************************************************** - * STAGE 8 - * - * - END COMPETITION - * - **************************************************/ - case END: - servoPosition(STORE_POSITION); - myled1 = 1; - wait(.2); - myled2 = 1; - wait(.2); - myled3 = 1; - wait(.2); - myled4 = 1; - wait(.2); - break; - default: + /************************************************** + * STAGE 8 + * + * - END COMPETITION + * + **************************************************/ + case END: + servoPosition(STORE_POSITION); + myled1 = 1; + wait(.2); + myled2 = 1; + wait(.2); + myled3 = 1; + wait(.2); + myled4 = 1; + wait(.2); + break; + default: - break; - } - } + break; + } + } -} + } -/************ - -Servo Functions - -**************/ - -void setServoPulse(uint8_t n, int angle) -{ - int pulse; - 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]); - int pulse2; - if(currentPosition[n] < pulse) { - for(i; i < pulse; i++) { - pulse2 = 4094 * i / pulselength; - pwm.setPWM(n, 0, pulse2); - wait_ms(3); - } - } else if (currentPosition[n] > pulse) { - for(i; i > pulse; i--) { - pulse2 = 4094 * i / pulselength; - pwm.setPWM(n, 0, pulse2); - wait_ms(3); - } - } - currentPosition[n] = i; - pc.printf("\nEND: pulse: %d, angle: %d\n\n", i, angle); -} + /************ -void initServoDriver(void) -{ - pwm.begin(); - //pwm.setPWMFreq(100); //This dosen't work well because of uncertain clock speed. Use setPrescale(). - pwm.setPrescale(140); //This value is decided for 20ms interval. - pwm.setI2Cfreq(400000); //400kHz - -} + Servo Functions -void servoBegin(void) -{ - pc.printf("Setting Initial Position\n\r"); - setServoPulseNo_delay(2, 0); - setServoPulseNo_delay(3, 162); - setServoPulseNo_delay(1, 10); - setServoPulseNo_delay(0, 85); - setServoPulseNo_delay(4, 175); - setServoPulseNo_delay(5, 0); - setGripper(1); -} - -void setServoPulseNo_delay(uint8_t n, int angle) -{ - int pulse = MIN_SERVO_PULSE + (( angle * (MAX_SERVO_PULSE - MIN_SERVO_PULSE)) / SERVO_MAX_ANGLE); - float pulselength = 20000; // 20,000 us per second - currentPosition[n] = pulse; - //pc.printf("ServoNumber: %d Pulsewidth: %d Angle: %d \n\r",n,pulse, angle); - pulse = 4094 * pulse / pulselength; - pwm.setPWM(n, 0, pulse); - -} + **************/ -void setGripper(int open) -{ - if (open) { - pc.printf("Gripper Open\r"); - setServoPulseNo_delay(6, 10); - } else { - pc.printf("Gripper Closed\n\r"); - setServoPulseNo_delay(6, 170); - } -} - -void servoPosition(int set) -{ - //moves to current position - setServoPulse(3, Arm_Table[set].claw_arm); - setServoPulse(1, Arm_Table[set].base_arm); - setServoPulse(2, Arm_Table[set].big_arm); - setServoPulse(0, Arm_Table[set].base_rotate); - setServoPulse(4, Arm_Table[set].claw_rotate); - setServoPulse(5, Arm_Table[set].claw_open); -} - - -int fire_checker(int rig) -{ - switch (rig) { - - case 1: - for (int i = 0; i<5; i++) { - pc.printf("1"); - distLaser = getDistance(); - pc.printf("L DISTANCE: %d \n\r", distLaser); - if ((distLaser < OILRIG1_MAX) - && (distLaser > OILRIG1_MIN)) { - fire_detected++; - } else { - fire_not_detected++; + void setServoPulse(uint8_t n, int angle) { + int pulse; + 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]); + int pulse2; + if(currentPosition[n] < pulse) { + for(i; i < pulse; i++) { + pulse2 = 4094 * i / pulselength; + pwm.setPWM(n, 0, pulse2); + wait_ms(3); } - } - break; - case 2: - for (int i = 0; i<5; i++) { - distLaser = getDistance(); - pc.printf("L DISTANCE: %d \n\r", distLaser); - if ((distLaser < OILRIG2_MAX) - && (distLaser > OILRIG2_MIN)) { - fire_detected++; - } else { - fire_not_detected++; - } - } - break; - case 3: - for (int i = 0; i<5; i++) { - distLaser = getDistance(); - if ((distLaser < OILRIG3_MAX) - && (distLaser > OILRIG3_MIN)) { - fire_detected++; - } else { - fire_not_detected++; - } - } - break; - - default: - for (int i = 0; i<5; i++) { - distLaser = getDistance(); - if ((distLaser < OILRIG1_MAX) - && (distLaser > OILRIG1_MIN)) { - fire_detected++; - } else { - fire_not_detected++; + } else if (currentPosition[n] > pulse) { + for(i; i > pulse; i--) { + pulse2 = 4094 * i / pulselength; + pwm.setPWM(n, 0, pulse2); + wait_ms(3); } } - break; - } + currentPosition[n] = i; + pc.printf("END: pulse: %d, angle: %d\n\r", i, angle); + } - if (fire_detected > fire_not_detected) { - return 1; - } else { - return 0; - } -} + void initServoDriver(void) { + pwm.begin(); + //pwm.setPWMFreq(100); //This dosen't work well because of uncertain clock speed. Use setPrescale(). + pwm.setPrescale(140); //This value is decided for 20ms interval. + pwm.setI2Cfreq(400000); //400kHz + + } -void errFunction(void) -{ - //Nothing -} -void wall_follow(int side, int direction, int section) -{ - float location, set=6; - int dir=1; + void servoBegin(void) { + pc.printf("Setting Initial Servo Position\n\r"); + servoPosition(STORE_POSITION); + } - pid1.reset(); + void setServoPulseNo_delay(uint8_t n, int angle) { + int pulse = MIN_SERVO_PULSE + (( angle * (MAX_SERVO_PULSE - MIN_SERVO_PULSE)) / SERVO_MAX_ANGLE); + float pulselength = 20000; // 20,000 us per second + currentPosition[n] = pulse; + //pc.printf("ServoNumber: %d Pulsewidth: %d Angle: %d \n\r",n,pulse, angle); + pulse = 4094 * pulse / pulselength; + pwm.setPWM(n, 0, pulse); + + } + + - if(direction == BACKWARD) dir=-1; - if(section == TOOLS)set= 10; + void servoPosition(int set) { + //moves to current position + setServoPulse(3, Arm_Table[set].claw_arm); + setServoPulse(2, Arm_Table[set].big_arm); + setServoPulse(1, Arm_Table[set].base_arm); + setServoPulse(0, Arm_Table[set].base_rotate); + setServoPulse(4, Arm_Table[set].claw_rotate); + setServoPulse(5, Arm_Table[set].claw_open); + } - leftEncoder.reset(); - rightEncoder.reset(); - location=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2; - - while(location< 66.5) { - location=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2; + int fire_checker(int rig) { + switch (rig) { - pid1.setInputLimits(0, set); - pid1.setOutputLimits( -MAX_SPEED, MAX_SPEED); - pid1.setSetPoint(set); - if(side) { - rangeFinderLeft.startMeas(); - wait_ms(20); - rangeFinderLeft.getMeas(range); - } else { - rangeFinderRight.startMeas(); - wait_ms(20); - rangeFinderRight.getMeas(range); - pc.printf("%d\r\n",range); + case 1: + for (int i = 0; i<10; i++) { + distLaser = getDistance(); + pc.printf("L DISTANCE: %d \n\r", distLaser); + if ((distLaser < OILRIG1_MAX) + && (distLaser > OILRIG1_MIN)) { + fire_detected++; + } else { + fire_not_detected++; + } + } + break; + case 2: + for (int i = 0; i<10; i++) { + distLaser = getDistance(); + pc.printf("L DISTANCE: %d \n\r", distLaser); + if ((distLaser < OILRIG2_MAX) + && (distLaser > OILRIG2_MIN)) { + fire_detected++; + } else { + fire_not_detected++; + } + } + break; + + } + + if (fire_detected > 0) { + return 1; + } else { + return 0; + } } - if(range > 15) { - //pc.printf("wavegap %f\r\n",wavegap); - // AT WAVE OPENING!!!! - motors.setMotor1Speed(dir*0.25*127);//left - motors.setMotor0Speed(dir*0.25*127);//right - } else { + void errFunction(void) { + //Nothing + } + void wall_follow(int side, int direction, int section) { + float location, set=6; + int dir=1; - pid1.setProcessValue(range); - pid_return = pid1.compute(); + pid1.reset(); + + if(direction == BACKWARD) dir=-1; + if(section == TOOLS)set= 10; - if(pid_return > 0) { - if(side) { - motors.setMotor0Speed(dir*MAX_SPEED - dir*pid_return);//right - motors.setMotor1Speed(dir*MAX_SPEED);//left - } else { - motors.setMotor1Speed(dir*MAX_SPEED - dir*pid_return);//left - motors.setMotor0Speed(dir*MAX_SPEED);//right - } - } else if(pid_return < 0) { + leftEncoder.reset(); + rightEncoder.reset(); + + location=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2; + + while(location< 66.5) { + location=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2; + + pid1.setInputLimits(0, set); + pid1.setOutputLimits( -MAX_SPEED, MAX_SPEED); + pid1.setSetPoint(set); if(side) { - motors.setMotor0Speed(dir*MAX_SPEED);//right - motors.setMotor1Speed(dir*MAX_SPEED + dir*pid_return);//left + rangeFinderLeft.startMeas(); + wait_ms(20); + rangeFinderLeft.getMeas(range); } else { - motors.setMotor1Speed(dir*MAX_SPEED);//left - motors.setMotor0Speed(dir*MAX_SPEED + dir*pid_return);//right + rangeFinderRight.startMeas(); + wait_ms(20); + rangeFinderRight.getMeas(range); + pc.printf("%d\r\n",range); } - } else { - motors.setMotor0Speed(dir*MAX_SPEED);//right - motors.setMotor1Speed(dir*MAX_SPEED);//left - } - } - } - - //STOP - motors.setMotor0Speed(dir*-0.3*127); //right - motors.setMotor1Speed(dir*-0.3*127); //left - wait_ms(10); - motors.stopBothMotors(0); -} - -/* MODIFIED WALL_FOLLOW FOR NAVIGATION */ -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; - bool SeeWaveGap = false; - - if(rig == 1) Rigloc= 16; - else if(rig == 2) Rigloc= 45; - else if(rig== 3) Rigloc = 70; + if(range > 15) { + //pc.printf("wavegap %f\r\n",wavegap); + // AT WAVE OPENING!!!! + motors.setMotor1Speed(dir*0.25*127);//left + motors.setMotor0Speed(dir*0.25*127);//right + } else { - pid1.reset(); - - if(direction == BACKWARD) { - dir=-1; - limit = 100; - } - if(section == TOOLS) { - set= 6; - limit = 86; - } - if(section == RETURN) lowlim=15; + pid1.setProcessValue(range); + pid_return = pid1.compute(); - 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); - - 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); + if(pid_return > 0) { + if(side) { + motors.setMotor0Speed(dir*MAX_SPEED - dir*pid_return);//right + motors.setMotor1Speed(dir*MAX_SPEED);//left + } else { + motors.setMotor1Speed(dir*MAX_SPEED - dir*pid_return);//left + motors.setMotor0Speed(dir*MAX_SPEED);//right + } + } else if(pid_return < 0) { + if(side) { + motors.setMotor0Speed(dir*MAX_SPEED);//right + motors.setMotor1Speed(dir*MAX_SPEED + dir*pid_return);//left + } else { + motors.setMotor1Speed(dir*MAX_SPEED);//left + motors.setMotor0Speed(dir*MAX_SPEED + dir*pid_return);//right + } + } else { + motors.setMotor0Speed(dir*MAX_SPEED);//right + motors.setMotor1Speed(dir*MAX_SPEED);//left + } + } + } - pid1.setInputLimits(0.0, set); - pid1.setOutputLimits( -MAX_SPEED, MAX_SPEED); - pid1.setSetPoint(set); - - if(side) { - rangeFinderLeft.startMeas(); - wait_ms(20); - rangeFinderLeft.getMeas(range); - } else { - rangeFinderRight.startMeas(); - wait_ms(20); - rangeFinderRight.getMeas(range); + //STOP + motors.setMotor0Speed(dir*-0.3*127); //right + motors.setMotor1Speed(dir*-0.3*127); //left + wait_ms(10); + motors.stopBothMotors(0); } - if(section == RIGS) { - rangeFinderLeft.startMeas(); - wait_ms(20); - rangeFinderLeft.getMeas(range2); + /* MODIFIED WALL_FOLLOW FOR NAVIGATION */ + + 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; + bool SeeWaveGap = false; + + if(rig == 1) Rigloc= 16; + else if(rig == 2) Rigloc= 45; + else if(rig== 3) Rigloc = 70; + + pid1.reset(); + + if(direction == BACKWARD) { + dir=-1; + limit = 100; + } + if(section == TOOLS) { + set= 6; + limit = 86; + } + if(section == RETURN) lowlim=15; + + 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); + + 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); + + pid1.setInputLimits(0.0, set); + pid1.setOutputLimits( -MAX_SPEED, MAX_SPEED); + pid1.setSetPoint(set); + + if(side) { + rangeFinderLeft.startMeas(); + wait_ms(20); + rangeFinderLeft.getMeas(range); + } else { + rangeFinderRight.startMeas(); + wait_ms(20); + rangeFinderRight.getMeas(range); + } + + if(section == RIGS) { + rangeFinderLeft.startMeas(); + wait_ms(20); + rangeFinderLeft.getMeas(range2); - 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); - break; + 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); + break; + } + } + } + + + //pc.printf("wall follow 2 range %f\r\n",range); + //pc.printf("loc+location = %f\r\n", loc+location); + if(range > 20 ) { + if(section == RIGS || section == RETURN) { + motors.setMotor0Speed(dir*0.25*127); //right + motors.setMotor1Speed(dir*0.25*127); //left + } else { + if(!SeeWaveGap) { + SeeWaveGap=true; + } else { + //STOP + motors.setMotor0Speed(dir*-0.25*127); //right + motors.setMotor1Speed(dir*-0.25*127); //left + wait_ms(5); + motors.stopBothMotors(0); + + pc.printf("wavegap\r\n"); + // AT WAVE OPENING!!!! + break; + } + } + } else { + SeeWaveGap = false; + pid1.setProcessValue(range); + pid_return = pid1.compute(); + //pc.printf("Range: %f\n PID: %f\r\n", range, pid_return); + + if(pid_return > 0) { + if(side) { + motors.setMotor0Speed(dir*MAX_SPEED - dir*pid_return);//right + motors.setMotor1Speed(dir*MAX_SPEED);//left + } else { + motors.setMotor1Speed(dir*MAX_SPEED - dir*pid_return);//left + motors.setMotor0Speed(dir*MAX_SPEED);//right + } + } else if(pid_return < 0) { + if(side) { + motors.setMotor0Speed(dir*MAX_SPEED);//right + motors.setMotor1Speed(dir*MAX_SPEED + dir*pid_return);//left + } else { + motors.setMotor1Speed(dir*MAX_SPEED);//left + motors.setMotor0Speed(dir*MAX_SPEED + dir*pid_return);//right + } + } else { + motors.setMotor0Speed(dir*MAX_SPEED); + motors.setMotor1Speed(dir*MAX_SPEED); + } } } + + //STOP + motors.setMotor0Speed(dir*-0.3*127); //right + motors.setMotor1Speed(dir*-0.3*127); //left + wait_ms(5); + motors.stopBothMotors(0); } - //pc.printf("wall follow 2 range %f\r\n",range); - //pc.printf("loc+location = %f\r\n", loc+location); - if(range > 20 ) { - if(section == RIGS || section == RETURN) { - motors.setMotor0Speed(dir*0.25*127); //right - motors.setMotor1Speed(dir*0.25*127); //left + void alignWithWall(int section) { + float usValue = 0; + + if(section == TOOLS) { + pc.printf("tools section align\r\n"); + // turn at an angle + 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()) < 300 || abs(rightEncoder.getPulses()) < 300); + motors.stopBothMotors(0); + + // turn left towards wall + leftEncoder.reset(); + rightEncoder.reset(); + motors.setMotor0Speed(MAX_SPEED); //right + motors.setMotor1Speed(-MAX_SPEED); //left + while(rightEncoder.getPulses() < 20 || abs(leftEncoder.getPulses()) < 20); + + motors.stopBothMotors(0); + + // turning left + motors.setMotor0Speed(0.9*MAX_SPEED); //right + 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 < 4 || 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); + 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); + motors.stopBothMotors(0); + + // turn left 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); + + motors.stopBothMotors(0); + + // turning left + motors.setMotor0Speed(-0.9*MAX_SPEED); //right + motors.setMotor1Speed(0.9*MAX_SPEED); //left } else { - if(!SeeWaveGap) { - SeeWaveGap=true; + 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 == RIGS) { + rangeFinderRight.startMeas(); + wait_ms(20); + rangeFinderRight.getMeas(range); } else { - //STOP - motors.setMotor0Speed(dir*-0.25*127); //right - motors.setMotor1Speed(dir*-0.25*127); //left - wait_ms(5); - motors.stopBothMotors(0); - - pc.printf("wavegap\r\n"); - // AT WAVE OPENING!!!! + rangeFinderLeft.startMeas(); + wait_ms(20); + rangeFinderLeft.getMeas(range); + } + pc.printf("Range %f \t OldValue %f\n\r",range, usValue); + if(range > usValue && usValue != 0 && range < 25) { break; + } else { + usValue = range; } } - } else { - SeeWaveGap = false; - pid1.setProcessValue(range); - pid_return = pid1.compute(); - //pc.printf("Range: %f\n PID: %f\r\n", range, pid_return); + motors.stopBothMotors(0); + } + + void rightTurn(void) { + motors.begin(); + leftEncoder.reset(); + 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); + } + + void leftTurn(void) { + 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()<1100); + motors.stopBothMotors(0); + } + + void slightleft(void) { + + leftEncoder.reset(); + rightEncoder.reset(); + motors.setMotor0Speed(0.5*127);// right + motors.setMotor1Speed(-0.5*127);// left + while(abs(leftEncoder.getPulses())<90 || rightEncoder.getPulses()<90); + motors.stopBothMotors(0); + } + + void slightright(void) { + + leftEncoder.reset(); + 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); + } + + void slightMove(int direction, float pulses) { + int dir=1; + + if(direction == BACKWARD) dir= -1; + + leftEncoder.reset(); + rightEncoder.reset(); + motors.setMotor0Speed(dir*0.25*127); //right + 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); + } + + void UntilWall(int dir) { + + if(dir == BACKWARD) dir=-1; + + leftEncoder.reset(); + rightEncoder.reset(); + motors.setMotor0Speed(dir*0.2*127); //right + motors.setMotor1Speed(dir*0.2*127); //left + + range = 30; + + while(range > 20) { + rangeFinderRight.startMeas(); + wait_ms(20); + rangeFinderRight.getMeas(range); + } - if(pid_return > 0) { - if(side) { - motors.setMotor0Speed(dir*MAX_SPEED - dir*pid_return);//right - motors.setMotor1Speed(dir*MAX_SPEED);//left - } else { - motors.setMotor1Speed(dir*MAX_SPEED - dir*pid_return);//left - motors.setMotor0Speed(dir*MAX_SPEED);//right + motors.setMotor0Speed(dir*-0.2*127); //right + motors.setMotor1Speed(dir*-0.2*127); //left + wait_ms(5); + motors.stopBothMotors(0); + } + + void overBump(int section) { + int preLeft=5000, preRight=5000, out=0; + + motors.begin(); + // slight backwards + leftEncoder.reset(); + rightEncoder.reset(); + motors.setMotor0Speed(-0.25*127); //right + motors.setMotor1Speed(-0.25*127); //left + while(abs(leftEncoder.getPulses()) < 50 || abs(rightEncoder.getPulses()) < 50); + motors.stopBothMotors(0); + + pc.printf("slight backwards\r\n"); + wait_ms(200); + + 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(); + preRight=rightEncoder.getPulses(); + wait_ms(200); + 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(); + + preLeft=preRight=5000 ; + leftEncoder.reset(); + rightEncoder.reset(); + motors.setMotor0Speed(.25*127); //right + 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)) { + + if(IR.getIRDistance() > 38) break; + + preLeft=leftEncoder.getPulses(); + preRight=rightEncoder.getPulses(); + wait_ms(200); } - } else if(pid_return < 0) { - if(side) { - motors.setMotor0Speed(dir*MAX_SPEED);//right - motors.setMotor1Speed(dir*MAX_SPEED + dir*pid_return);//left - } else { - motors.setMotor1Speed(dir*MAX_SPEED);//left - motors.setMotor0Speed(dir*MAX_SPEED + dir*pid_return);//right + } 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)) { + + if(IR.getIRDistance() > 38) break; + + preLeft=leftEncoder.getPulses(); + preRight=rightEncoder.getPulses(); + wait_ms(200); } + } else { - motors.setMotor0Speed(dir*MAX_SPEED); - motors.setMotor1Speed(dir*MAX_SPEED); + while(abs(leftEncoder.getPulses()) < 100 || abs(rightEncoder.getPulses()) < 100); + + leftEncoder.reset(); + rightEncoder.reset(); + + motors.setMotor0Speed(-.15*127); //right + motors.setMotor1Speed(-.15*127); //left + while((abs(leftEncoder.getPulses()) < 100 || abs(rightEncoder.getPulses()) < 100) && preLeft!=0 ) { + preLeft = leftEncoder.getPulses(); + preRight = rightEncoder.getPulses(); + 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); + + return; } - } - } - //STOP - motors.setMotor0Speed(dir*-0.3*127); //right - motors.setMotor1Speed(dir*-0.3*127); //left - wait_ms(5); - motors.stopBothMotors(0); -} + 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); + wait_ms(20); + motors.begin(); + + } + + void to_tools_section1(float* location, float ¤t) { + slightMove(FORWARD,6500); + current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2; + + } + + void to_tools_section2(float* location, float ¤t) { + slightMove(FORWARD,3200); + current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2; + + } + + void from_tools_section(float* location, float ¤t) { -void alignWithWall(int section) -{ - float usValue = 0; - - if(section == TOOLS) { - pc.printf("tools section align\r\n"); - // turn at an angle - 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()) < 300 || abs(rightEncoder.getPulses()) < 300); - motors.stopBothMotors(0); - - // turn left towards wall - leftEncoder.reset(); - rightEncoder.reset(); - motors.setMotor0Speed(MAX_SPEED); //right - motors.setMotor1Speed(-MAX_SPEED); //left - while(rightEncoder.getPulses() < 20 || abs(leftEncoder.getPulses()) < 20); - - motors.stopBothMotors(0); - - // turning left - motors.setMotor0Speed(0.9*MAX_SPEED); //right - 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 < 4 || range > 20) return; + alignWithWall(TOOLS); + pc.printf("align\r\n"); + wait_ms(100); - // 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); - 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); - motors.stopBothMotors(0); + //wall_follow2(LEFT,FORWARD,MID, current); + //current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); - // turn left 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); - - motors.stopBothMotors(0); - - // turning left - motors.setMotor0Speed(-0.9*MAX_SPEED); //right - motors.setMotor1Speed(0.9*MAX_SPEED); //left - } else { - 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 == RIGS) { - rangeFinderRight.startMeas(); - wait_ms(20); - rangeFinderRight.getMeas(range); - } else { rangeFinderLeft.startMeas(); wait_ms(20); rangeFinderLeft.getMeas(range); - } - 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) -{ - motors.begin(); - leftEncoder.reset(); - 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); -} - -void leftTurn(void) -{ - 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()<1100); - motors.stopBothMotors(0); -} - -void slightleft(void) -{ - - leftEncoder.reset(); - rightEncoder.reset(); - motors.setMotor0Speed(0.5*127);// right - motors.setMotor1Speed(-0.5*127);// left - while(abs(leftEncoder.getPulses())<90 || rightEncoder.getPulses()<90); - motors.stopBothMotors(0); -} - -void slightright(void) -{ - - leftEncoder.reset(); - 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); -} - -void slightMove(int direction, float pulses) -{ - int dir=1; - - if(direction == BACKWARD) dir= -1; - - leftEncoder.reset(); - rightEncoder.reset(); - motors.setMotor0Speed(dir*0.25*127); //right - 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); -} - -void UntilWall(int dir) -{ - - if(dir == BACKWARD) dir=-1; - - leftEncoder.reset(); - rightEncoder.reset(); - motors.setMotor0Speed(dir*0.2*127); //right - motors.setMotor1Speed(dir*0.2*127); //left - - range = 30; - - while(range > 20) { - rangeFinderRight.startMeas(); - wait_ms(20); - rangeFinderRight.getMeas(range); - } - motors.setMotor0Speed(dir*-0.2*127); //right - motors.setMotor1Speed(dir*-0.2*127); //left - wait_ms(5); - motors.stopBothMotors(0); -} - -void overBump(int section) -{ - int preLeft=5000, preRight=5000, out=0; - - motors.begin(); - // slight backwards - leftEncoder.reset(); - rightEncoder.reset(); - motors.setMotor0Speed(-0.25*127); //right - motors.setMotor1Speed(-0.25*127); //left - while(abs(leftEncoder.getPulses()) < 50 || abs(rightEncoder.getPulses()) < 50); - motors.stopBothMotors(0); - - pc.printf("slight backwards\r\n"); - wait_ms(200); - - 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(); - preRight=rightEncoder.getPulses(); - wait_ms(200); - 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 + 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 + 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); - 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(); + wait_ms(100); + leftTurn(); + overBump(TOOLS); + } else { + pc.printf("else greater than 20\r\n"); + location[0]= current; + leftTurn(); + overBump(TOOLS); + } - preLeft=preRight=5000 ; - leftEncoder.reset(); - rightEncoder.reset(); - motors.setMotor0Speed(.25*127); //right - 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)) { - - if(IR.getIRDistance() > 38) break; + pc.printf("First Wavegap = %f\r\n",location[0]); - preLeft=leftEncoder.getPulses(); - preRight=rightEncoder.getPulses(); - wait_ms(200); - } - } 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)) { - - if(IR.getIRDistance() > 38) break; - - preLeft=leftEncoder.getPulses(); - preRight=rightEncoder.getPulses(); - wait_ms(200); } - } else { - while(abs(leftEncoder.getPulses()) < 100 || abs(rightEncoder.getPulses()) < 100); + void mid_section(float* location, float ¤t, int* direction) { + motors.begin(); - leftEncoder.reset(); - rightEncoder.reset(); + 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); + 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); + + wait_ms(500); + rangeFinderLeft.startMeas(); + wait_ms(20); + rangeFinderLeft.getMeas(range); - motors.setMotor0Speed(-.15*127); //right - motors.setMotor1Speed(-.15*127); //left - while((abs(leftEncoder.getPulses()) < 100 || abs(rightEncoder.getPulses()) < 100) && preLeft!=0 ) { - preLeft = leftEncoder.getPulses(); - preRight = rightEncoder.getPulses(); - wait_ms(200); - if(leftEncoder.getPulses() == preLeft || rightEncoder.getPulses()== preRight) preLeft=preRight=0; + if(range > 20 ) { + direction[0]= RIGHT; + location[1]= current; + slightMove(FORWARD,75); + //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); + } + + } + + pc.printf("wavegap2 = %f\r\n",location[1]); + leftTurn(); + + wait_ms(100); + + overBump(MID); + } - 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); - - return; - } + void mid_section2(float* location, float ¤t, int* direction) { + motors.begin(); - 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); - wait_ms(20); - motors.begin(); - -} + pc.printf("mid section 2\r\n"); -void to_tools_section2(float* location, float ¤t) -{ - slightMove(FORWARD,3200); - current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2; - -} -void to_tools_section1(float* location, float ¤t) -{ - slightMove(FORWARD,6450); - current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2; - -} + if(IR.getIRDistance() > 38) { + direction[1]= STRAIGHT; + overBump(RIGS); + return; + } -void from_tools_section(float* location, float ¤t) -{ - + alignWithWall(MID); + pc.printf("midsection 2 alignt with wall mid \r\n"); - alignWithWall(TOOLS); - pc.printf("align\r\n"); - wait_ms(100); + wall_follow2(LEFT,FORWARD,MID, current,0); + current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); - //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); + wait_ms(500); - 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 - 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); - - 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]); - -} + pc.printf("midseection 2 after wf2 %f",current); + rangeFinderLeft.startMeas(); + wait_ms(20); + rangeFinderLeft.getMeas(range); -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); - 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); + 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); + } 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); + } - wait_ms(500); - rangeFinderLeft.startMeas(); - wait_ms(20); - rangeFinderLeft.getMeas(range); - - if(range > 20 ) { - direction[0]= RIGHT; - location[1]= current; - slightMove(FORWARD,75); - //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); + leftTurn(); + overBump(RIGS); + pc.printf("overbump rigs\r\n"); } - } - - pc.printf("wavegap2 = %f\r\n",location[1]); - leftTurn(); - - wait_ms(100); - - overBump(MID); - -} - -void mid_section2(float* location, float ¤t, int* direction) -{ - motors.begin(); + void rig_section(float* location, float ¤t, int* direction, int rig) { + float loc; - pc.printf("mid section 2\r\n"); - - if(IR.getIRDistance() > 38) { - direction[1]= STRAIGHT; - overBump(RIGS); - return; - } + if(rig == 1) loc= 15; + else if(rig == 2) loc= 45; + else loc = 75; - alignWithWall(MID); - pc.printf("midsection 2 alignt with wall mid \r\n"); - - wall_follow2(LEFT,FORWARD,MID, current,0); - current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); - - wait_ms(500); + rightTurn(); + slightright(); - pc.printf("midseection 2 after wf2 %f",current); - rangeFinderLeft.startMeas(); - wait_ms(20); - rangeFinderLeft.getMeas(range); + if(current > loc) { + 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); + wall_follow2(RIGHT, FORWARD, RIGS, current, rig); + current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); + } + } - 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); - } 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); - } + void tools_section_return(float* location, float ¤t) { + if(location[0] > 16) { + leftTurn(); + wall_follow2(LEFT, BACKWARD, RETURN, location[0], 0); + } + motors.stopBothMotors(0); - leftTurn(); - overBump(RIGS); - pc.printf("overbump rigs\r\n"); -} - -void rig_section(float* location, float ¤t, int* direction, int rig) -{ - float loc; - - if(rig == 1) loc= 15; - else if(rig == 2) loc= 45; - else loc = 75; - - rightTurn(); - slightright(); + } - if(current > loc) { - 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); - wall_follow2(RIGHT, FORWARD, RIGS, current, rig); - current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); - } -} - -void tools_section_return(float* location, float ¤t) -{ - if(location[0] > 16) { - leftTurn(); - wall_follow2(LEFT, BACKWARD, RETURN, location[0], 0); - } - motors.stopBothMotors(0); - -} + 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) { + 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(RIGS); + } -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) { - 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(RIGS); -} + void mid_section2_return(float* location, float ¤t, int* direction) { + if(direction[1] == RIGHT) { + 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) { + 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); + } -void mid_section2_return(float* location, float ¤t, int* direction) -{ - if(direction[1] == RIGHT) { - 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) { - 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); -} - -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); - } else { - wall_follow2(RIGHT, BACKWARD, MID, current,0); - current-=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); - } - rightTurn(); - overBump(MID2); -} + 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); + } else { + wall_follow2(RIGHT, BACKWARD, MID, current,0); + current-=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); + } + rightTurn(); + overBump(MID2); + }