uyvug
Dependencies: Adafruit-16-Ch-PWM-Servo-Driver HCSR04 PID PololuQik2 QEI Sharp mbed-rtos
Fork of theRobot by
Diff: main.cpp
- Revision:
- 12:284be46593ae
- Parent:
- 11:8d2455e383ce
- Child:
- 13:529323807361
--- a/main.cpp Tue Apr 01 02:00:01 2014 +0000 +++ b/main.cpp Wed Apr 02 00:30:30 2014 +0000 @@ -23,19 +23,21 @@ #define PPR (4331/4) #define LEFT (1) #define RIGHT (0) +#define STRAIGHT (2) #define FORWARD (1) #define BACKWARD (0) #define TOOLS (0) #define MID (1) #define RIGS (2) -#define FIRST_WAVE (0) +#define MID2 (3) +#define RETURN (4) #define FAR (1) //States #define START 0 #define OILRIG1_POS 1 #define OILRIG2_POS 2 -#define GOTO_TOOLS 3 +#define GOTO_TOOLS1 3 #define IDENTIFY_TOOLS 4 #define AQUIRE_TOOL1 5 #define AQUIRE_TOOL2 6 @@ -49,6 +51,7 @@ #define RIG_ALIGN 14 #define INSERT_TOOL 15 #define END 16 +#define GOTO_TOOLS2 17 @@ -131,21 +134,26 @@ //Navigation Functions -float wall_follow(int side, int direction, int section); -void wall_follow2(int side, int direction, int section, float location); -void wall_follow3(int ¤tLocation, int &WaveOpening); +void wall_follow(int side, int direction, int section); +void wall_follow2(int side, int direction, int section, float location, int rig); void leftTurn(void); void slightleft(void); +void slightright(void); void rightTurn(void); -void us_distance(void); -void to_tools_section(float* location, float ¤t); +void slightMove(int direction, float pulses); +void to_tools_section1(float* location, float ¤t); +void to_tools_section2(float* location, float ¤t); void from_tools_section(float* location, float ¤t); void mid_section(float* location, float ¤t, int* direction); void mid_section2(float* location, float ¤t, int* direction); void rig_section(float* location, float ¤t, int* direction, int rig); +void tools_section_return(float* location, float ¤t); +void mid_section_return(float* location, float ¤t, int* direction); +void mid_section2_return(float* location, float ¤t, int* direction); +void rig_section_return(float* location, float ¤t, int* direction); void overBump(int section); void alignWithWall(int section); -void ledtoggle(void); +void UntilWall(int dir); /************ @@ -186,13 +194,13 @@ //increase in number 5 rotates gripper {STORE_POSITION, 85, 5, 0, 165, 175, 0}, // storing position - {OIL_RIG1, 162, 20, 60, 55, 100, 0}, // point laser at oilrig1 - {OIL_RIG2, 145, 20, 60, 55, 100, 0}, // point laser at oilrig2 - {OIL_RIG3, 130, 90, 90, 57, 100, 0}, // point laser at oilrig2 - {DRIVE_POSITION_NOTOOL, 85, 5, 0, 175, 175, 0}, // Drive through course - {TOOL_1, 95, 40, 70, 128, 175, 0}, // Look over first tool - {TOOL_2, 77, 40, 70, 128, 175, 0}, // Look over second tool - {TOOL_3, 57, 40, 70, 128, 175, 0}, // Look over third tool + {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 {ORIENT_TOOL, 135, 60, 75, 60, 90, 90}, // position tool to be inserted {PU_TOOL_1, 98, 50, 82, 118, 90, 0}, // STATIC Pickup tool POSITION @@ -222,7 +230,7 @@ /***************** INITIALIZATIONS *******************/ - float location[3], current=6; + float location[3], current=4; int direction[3]; double distance; @@ -246,8 +254,8 @@ /******************* WHILE LOOP FOR TESTING ********************/ - while(1) { - + //while(1) { +/* pc.scanf("%d %d", &servoNum, &servoAngle); if(servoAngle > 175) { servoAngle = 175; @@ -259,7 +267,7 @@ setServoPulse(servoNum, servoAngle); distLaser = getDistance(); pc.printf("Distance %d", distLaser); - +*/ /* while(pc.getc() != 'g'); servoPosition(TOOL_2); @@ -349,7 +357,7 @@ } printImageToFile(BINARY);*/ - } + // } @@ -370,7 +378,7 @@ **************************************************/ case START : myled1 = 1; - wait(1); + while(pc.getc() != 'g'); myled1 = 0; state = OILRIG1_POS; break; @@ -389,15 +397,17 @@ 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); //wait for servos to settle + wait(3); + //lrf.putc('E'); // lighting calculation + //wait(15); fire = fire_checker(OIL_RIG1); //determines if oil rig is on fire - pc.printf("FIRE: %d", fire); + + pc.printf("FIRE: SQUARE %d", fire); //determines what tool is needed if (fire == 1) { tool_needed = SQUARE; - state = GOTO_TOOLS; + state = GOTO_TOOLS1; } else { state = OILRIG2_POS; } @@ -405,16 +415,31 @@ 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); + + wait(3); //wait for servos to settle + + to_tools_section2(location, current); + servoPosition(OIL_RIG2); //position arm to point at first oilrig - wait(1); //wait for servos to settle + wait(2); + //lrf.putc('E'); // lighting calculation + //wait(15); fire = fire_checker(OIL_RIG2); - pc.printf("FIRE: %d", fire); + pc.printf("FIRE: TRIANGLE %d", fire); if (fire == 1) { tool_needed = TRIANGLE; - state = GOTO_TOOLS; + state = GOTO_TOOLS2; } else { tool_needed = CIRCLE; - state = GOTO_TOOLS; + state = GOTO_TOOLS2; } pc.printf("tool needed: %d", tool_needed); break; @@ -425,10 +450,25 @@ * - TRAVEL TO TOOLS * **************************************************/ - case GOTO_TOOLS: + 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 + 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(2, Arm_Table[DRIVE_POSITION_NOTOOL].big_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); @@ -436,12 +476,12 @@ wait(5); //wait for servos to settle - to_tools_section(location, current); + slightMove(FORWARD,3100); + current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2; state = IDENTIFY_TOOLS; - pc.printf("YES!!!!!"); - while(1); break; + /************************************************** * STAGE 3 * @@ -450,21 +490,24 @@ * **************************************************/ case IDENTIFY_TOOLS: - servoPosition(TOOL_1); //arm/camera looks over tool - wait(1); //wait for servos to settle - centerCamWithTool(); //centers camera over tool + + 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(); + //printImageToFile(BINARY); //either goes to aquire the tool or look at the next shape if(shape_detected == tool_needed) { - state = AQUIRE_TOOL1; + state = AQUIRE_TOOL2; } else { - servoPosition(TOOL_2); + servoPosition(TOOL_1); wait(1); //wait for servos to settle - centerCamWithTool(); + //centerCamWithTool(); shape_detected = shapeDetection(); if (shape_detected == tool_needed) { - state = AQUIRE_TOOL2; + state = AQUIRE_TOOL1; } else { servoPosition(TOOL_3); wait(1); //wait for servos to settle @@ -472,28 +515,29 @@ state = AQUIRE_TOOL3; } } + while(1); break; case AQUIRE_TOOL1: - //*********************// - //******* TODO ********// - //*********************// - // CODE TO GRAB 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: - //*********************// - //******* TODO ********// - //*********************// - // CODE TO GRAB 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: - //*********************// - //******* TODO ********// - //*********************// - // CODE TO GRAB TOOL3 + servoPosition(PU_TOOL_3_STAB); + wait(2); + + servoPosition(DRIVE_POSITION_TOOL); //arm position for driving with the tool state = NAVIGATE_WAVES_ROW1; break; @@ -711,6 +755,7 @@ 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) @@ -769,20 +814,9 @@ { //Nothing } - -void us_distance(void) +void wall_follow(int side, int direction, int section) { - pc.printf("Ultra Sonic\n\r"); - rangeFinderLeft.startMeas(); - wait_us(20); - if ( (rangeFinderLeft.getMeas(range) == RANGE_MEAS_VALID)) { - pc.printf("Range = %f\n\r", range); - } -} - -float wall_follow(int side, int direction, int section) -{ - float location, wavegap=0, set=5; + float location, set=6; int dir=1; pid1.reset(); @@ -795,7 +829,7 @@ location=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2; - while(location< 70) { + while(location< 66.5) { location=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2; pid1.setInputLimits(0, set); @@ -803,21 +837,20 @@ pid1.setSetPoint(set); if(side) { rangeFinderLeft.startMeas(); - wait_ms(38); + wait_ms(20); rangeFinderLeft.getMeas(range); } else { rangeFinderRight.startMeas(); - wait_ms(38); + wait_ms(20); rangeFinderRight.getMeas(range); pc.printf("%d\r\n",range); } - if(range > 20) { - wavegap=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2; + if(range > 15) { //pc.printf("wavegap %f\r\n",wavegap); // AT WAVE OPENING!!!! - motors.setMotor1Speed(dir*0.4*127);//left - motors.setMotor0Speed(dir*0.4*127);//right + motors.setMotor1Speed(dir*0.25*127);//left + motors.setMotor0Speed(dir*0.25*127);//right } else { pid1.setProcessValue(range); @@ -845,26 +878,50 @@ } } } - return wavegap; + + //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) +void wall_follow2(int side, int direction, int section, float location, int rig) { - int SeeWaveGap = false, dir=1; - float set=5, loc=0; + 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; - if(section == TOOLS)set= 5; + if(direction == BACKWARD) { + dir=-1; + limit = 100; + } + if(section == TOOLS) { + set= 6; + limit = 86; + } + if(section == RETURN) lowlim=15; leftEncoder.reset(); rightEncoder.reset(); - while(dir*loc + location <= 78) { + //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); @@ -872,54 +929,87 @@ if(side) { rangeFinderLeft.startMeas(); - wait_ms(38); + wait_ms(20); rangeFinderLeft.getMeas(range); } else { rangeFinderRight.startMeas(); - wait_ms(38); + wait_ms(20); rangeFinderRight.getMeas(range); } + if(section == RIGS) { + rangeFinderLeft.startMeas(); + wait_ms(20); + rangeFinderLeft.getMeas(range2); - /*************CHECK FOR WAVE OPENING*****************/ - /* If after 20 ms the ultrasonic still sees 20+ cm */ - /* then robot is at wave opening */ + 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) { - motors.stopBothMotors(); - pc.printf("wavegap\r\n"); - // AT WAVE OPENING!!!! - break; - } - - pid1.setProcessValue(range); - pid_return = pid1.compute(); - //pc.printf("Range: %f\n PID: %f\r\n", range, pid_return); + 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); - 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 + pc.printf("wavegap\r\n"); + // AT WAVE OPENING!!!! + break; + } } } else { - motors.setMotor0Speed(dir*MAX_SPEED); - motors.setMotor1Speed(dir*MAX_SPEED); + 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); + } } } - motors.stopBothMotors(); + + //STOP + motors.setMotor0Speed(dir*-0.3*127); //right + motors.setMotor1Speed(dir*-0.3*127); //left + wait_ms(5); + motors.stopBothMotors(0); } @@ -928,53 +1018,100 @@ 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(); + motors.stopBothMotors(0); //go backwards toward wall leftEncoder.reset(); rightEncoder.reset(); - motors.setMotor0Speed(-MAX_SPEED); //right - motors.setMotor1Speed(-MAX_SPEED); //left + motors.setMotor0Speed(-0.25*127); //right + motors.setMotor1Speed(-0.25*127); //left while(abs(leftEncoder.getPulses()) < 300 || abs(rightEncoder.getPulses()) < 300); - - motors.stopBothMotors(); + motors.stopBothMotors(0); // turn left towards wall leftEncoder.reset(); rightEncoder.reset(); motors.setMotor0Speed(MAX_SPEED); //right motors.setMotor1Speed(-MAX_SPEED); //left - while(rightEncoder.getPulses() < 10 || abs(leftEncoder.getPulses()) < 10); + 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 - motors.stopBothMotors(); + } 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); - motors.setMotor0Speed(0.7*MAX_SPEED); //right - motors.setMotor1Speed(-0.7*MAX_SPEED); //left + //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 { + pc.printf("in mid section align\r\n"); + // turn right towards wall rightTurn(); - motors.setMotor0Speed(-0.7*MAX_SPEED); //right - motors.setMotor1Speed(0.7*MAX_SPEED); //left + // turning left towards wall + motors.setMotor0Speed(0.9*MAX_SPEED); //right + motors.setMotor1Speed(-0.9*MAX_SPEED); //left } usValue = 0; while(1) { - rangeFinderLeft.startMeas(); - wait_ms(20); - rangeFinderLeft.getMeas(range); - //pc.printf("Range %f \t OldValue %f\n\r",range, usValue); + 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(); + motors.stopBothMotors(0); } void rightTurn(void) @@ -984,28 +1121,21 @@ rightEncoder.reset(); motors.setMotor0Speed(-0.5*127);//right motors.setMotor1Speed(0.5*127);//left - while(leftEncoder.getPulses()<900 || rightEncoder.getPulses()>-900); - motors.stopBothMotors(); + while(abs(leftEncoder.getPulses())<950 || abs(rightEncoder.getPulses())<950); + motors.stopBothMotors(0); } void leftTurn(void) { - /* - leftEncoder.reset(); - rightEncoder.reset(); - motors.setMotor0Speed(0.4*MAX_SPEED); //right - motors.setMotor1Speed(-MAX_SPEED); //left - while(abs(leftEncoder.getPulses())<2500); - motors.stopBothMotors(); - */ motors.begin(); leftEncoder.reset(); rightEncoder.reset(); motors.setMotor0Speed(0.5*127);// right motors.setMotor1Speed(-0.5*127);// left - while(abs(leftEncoder.getPulses())<1000 || rightEncoder.getPulses()<1000); - motors.stopBothMotors(); + while(abs(leftEncoder.getPulses())<1100 || rightEncoder.getPulses()<1100); + motors.stopBothMotors(0); } + void slightleft(void) { @@ -1013,80 +1143,123 @@ rightEncoder.reset(); motors.setMotor0Speed(0.5*127);// right motors.setMotor1Speed(-0.5*127);// left - while(abs(leftEncoder.getPulses())<50 || rightEncoder.getPulses()<50); - motors.stopBothMotors(); + 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(); - - leftEncoder.reset(); - rightEncoder.reset(); - motors.setMotor0Speed(-0.2*127); //right - motors.setMotor1Speed(-0.2*127); //left - while(abs(leftEncoder.getPulses()) < 50 || abs(rightEncoder.getPulses())< 50); - motors.stopBothMotors(); - + // slight backwards leftEncoder.reset(); rightEncoder.reset(); - motors.setMotor0Speed(0.2*127); //right - motors.setMotor1Speed(0.2*127); //left - while(/*(abs(leftEncoder.getPulses()) < 1000 || abs(rightEncoder.getPulses())< 1000)*/ IR.getIRDistance() >20 && preLeft!=0) { - preLeft=leftEncoder.getPulses(); - preRight=rightEncoder.getPulses(); - wait_ms(100); - //pc.printf(" first while left %d right %d \r\n", preLeft, preRight); - if(leftEncoder.getPulses() == preLeft || rightEncoder.getPulses()== preRight) preLeft=preRight=0; - } + 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(); - motors.begin(); - wait(2); - /* - motors.stopBothMotors(); - motors.setMotor0Speed(0.15*127); //right - motors.setMotor1Speed(0.15*127); //left - preLeft=preRight=5000 ; - leftEncoder.reset(); - rightEncoder.reset(); - */ -// while(/*(abs(leftEncoder.getPulses()) < 1000 || abs(rightEncoder.getPulses())< 1000)*/ IR.getIRDistance() >20 && preLeft!=0){ - /* preLeft=leftEncoder.getPulses(); - preRight=rightEncoder.getPulses(); - pc.printf("second while left %d right %d \r\n", preLeft, preRight); - wait_ms(200); - if(leftEncoder.getPulses() == preLeft || rightEncoder.getPulses()== preRight) preLeft=preRight=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(!out) { - preLeft=leftEncoder.getPulses(); + while((abs(leftEncoder.getPulses()) < 800 || abs(rightEncoder.getPulses()) < 800) /*&& preLeft!=0*/ && IR.getIRDistance() >15 ) { + /*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 - } - if(abs(leftEncoder.getPulses()) <1000 || abs(leftEncoder.getPulses())<1000) out=1; + wait_ms(200); + if(leftEncoder.getPulses() == preLeft || rightEncoder.getPulses()== preRight) preLeft=preRight=0;*/ } - motors.stopBothMotors(); - wait(2); + 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 ; @@ -1095,159 +1268,303 @@ motors.setMotor0Speed(.25*127); //right motors.setMotor1Speed(.25*127); //left - if(section == TOOLS || section == MID) { - while(IR.getIRDistance() > 20 ) { - //pc.printf("IR %f\r\n", IR.getIRDistance()); - //pc.printf("third while left %d right %d \r\n", preLeft, preRight); + 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(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()) < 200 || abs(rightEncoder.getPulses())< 200)); + + } else { + 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; + } + + leftEncoder.reset(); + rightEncoder.reset(); motors.setMotor0Speed(-.25*127); //right motors.setMotor1Speed(-.25*127); //left - wait_ms(10); - motors.stopBothMotors(); - wait(2); + while((abs(leftEncoder.getPulses()) < 10 || abs(rightEncoder.getPulses()) < 10)); + + motors.stopBothMotors(0); + wait_ms(20); motors.begin(); } -void to_tools_section(float* location, float ¤t) +void to_tools_section2(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; - pc.printf("current %f \r\n",current); + slightMove(FORWARD,3200); + current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2; - motors.stopBothMotors(); - - wait(2); - - motors.setMotor0Speed(-.2*127); //right - motors.setMotor1Speed(-.2*127); //left - wait_ms(5); - motors.stopBothMotors(); +} +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; } void from_tools_section(float* location, float ¤t) { + + alignWithWall(TOOLS); - + pc.printf("align\r\n"); wait_ms(100); - wall_follow2(LEFT,FORWARD,MID, current); - current= 78; + //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); + 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()) < 40 || abs(rightEncoder.getPulses())< 40); - motors.stopBothMotors(); + while(abs(leftEncoder.getPulses()) < 10 || abs(rightEncoder.getPulses())< 10); + motors.stopBothMotors(0); - wait_ms(500); - location[0]= current- ((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); - current= location[0]; + wait_ms(100); leftTurn(); - slightleft(); overBump(TOOLS); } else { - location[0]= 77; + pc.printf("else greater than 20\r\n"); + location[0]= current; leftTurn(); - wait_ms(20); - overBump(FIRST_WAVE); + overBump(TOOLS); } pc.printf("First Wavegap = %f\r\n",location[0]); + } + void mid_section(float* location, float ¤t, int* direction) { - motors.begin(); - if(IR.getIRDistance() > 20) return; - + 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); - current=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); + 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(current != 0) { + wait_ms(500); + rangeFinderLeft.startMeas(); + wait_ms(20); + rangeFinderLeft.getMeas(range); + + if(range > 20 ) { direction[0]= RIGHT; - current+= location[0]; location[1]= current; + slightMove(FORWARD,75); + //current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); } else { - current=location[0]; direction[0]= LEFT; - wall_follow2(LEFT,BACKWARD,MID,current); - location[1]= location[0]- ((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); + 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(); - overBump(TOOLS); - // go forward - leftEncoder.reset(); - rightEncoder.reset(); - motors.setMotor0Speed(0.2*127); //right - motors.setMotor1Speed(0.2*127); //left - while(abs(leftEncoder.getPulses())<300 || abs(rightEncoder.getPulses())<300); - motors.stopBothMotors(); + + wait_ms(100); + + overBump(MID); } void mid_section2(float* location, float ¤t, int* direction) { - motors.begin(); - if(IR.getIRDistance() > 20) return; + pc.printf("mid section 2\r\n"); + + if(IR.getIRDistance() > 38) { + direction[1]= STRAIGHT; + overBump(RIGS); + return; + } alignWithWall(MID); - wall_follow2(LEFT,FORWARD,MID, current); - current=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); + 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); - if(current != 0) { + pc.printf("midseection 2 after wf2 %f",current); + rangeFinderLeft.startMeas(); + wait_ms(20); + rangeFinderLeft.getMeas(range); + + if(range > 20 ) { direction[1]= RIGHT; - current+= location[1]; location[2]= current; + slightMove(FORWARD,75); + //current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); } else { - current=location[1]; direction[1]= LEFT; - wall_follow2(LEFT,BACKWARD,MID,current); - location[2]= location[1]- ((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); + 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); } 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 ledtoggle(void) +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) { - pwm.setPWM(9, 0, 4094); - wait(2); - pwm.setPWM(9, 0, 0); + 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); } + +