uyvug
Dependencies: Adafruit-16-Ch-PWM-Servo-Driver HCSR04 PID PololuQik2 QEI Sharp mbed-rtos
Fork of theRobot by
Diff: main.cpp
- Revision:
- 21:0907e1f5e16c
- Parent:
- 20:55dcff40c5d9
- Child:
- 22:79c5871543b5
diff -r 55dcff40c5d9 -r 0907e1f5e16c main.cpp --- a/main.cpp Sat Apr 05 07:26:15 2014 +0000 +++ b/main.cpp Tue Apr 08 16:20:40 2014 +0000 @@ -87,11 +87,11 @@ //Oil Rig distance thresholds -#define OILRIG1_MAX 2200 +#define OILRIG1_MAX 1800 #define OILRIG1_MIN 1000 -#define OILRIG2_MAX 2200 +#define OILRIG2_MAX 1800 #define OILRIG2_MIN 1000 -#define OILRIG3_MAX 2200 +#define OILRIG3_MAX 1800 #define OILRIG3_MIN 1000 //for servo normalization @@ -209,8 +209,8 @@ //increase in number 5 rotates gripper {STORE_POSITION, 85, 10, 0, 165, 175, 0}, // storing position - {OIL_RIG1, 156, 20, 60, 47, 175, 0}, // point laser at oilrig1 - {OIL_RIG2, 162, 20, 60, 47, 175, 0}, // point laser at oilrig2 + {OIL_RIG1, 160, 20, 60, 47, 175, 0}, // point laser at oilrig1 + {OIL_RIG2, 164, 20, 60, 47, 175, 0}, // point laser at oilrig2 {OIL_RIG3, 130, 90, 90, 52, 175, 0}, // point laser at oilrig2 {DRIVE_POSITION_NOTOOL, 85, 10, 0, 165, 175, 0}, // Drive through course {TOOL_1, 101, 50, 80, 113, 90, 0}, // Look over first tool @@ -288,7 +288,6 @@ *********************************/ - while(1) { if(button_start == 1) { @@ -387,7 +386,7 @@ setServoPulse(5, Arm_Table[DRIVE_POSITION_NOTOOL].claw_open); wait(3); //wait for servos to settle - slightMove(FORWARD,3350); + slightMove(FORWARD,3250); current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2; state = IDENTIFY_TOOLS; @@ -444,7 +443,7 @@ break; } else { //printImageToFile(BINARY); - slightMove(FORWARD,60); + slightMove(FORWARD,70); current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2; servoPosition(TOOL_1); wait(5); //wait for servos to settle @@ -495,12 +494,12 @@ wait(1); setServoPulse(1, Arm_Table[PU_TOOL_1].base_arm - 6); wait(.5); - setServoPulse(5, 105); + setServoPulse(5, 140); wait(.5); setServoPulse(5, 00); setServoPulse(1, Arm_Table[PU_TOOL_1].base_arm - 10); wait(1); - setServoPulse(5, 105); + setServoPulse(5, 140); wait(1); setServoPulse(1, Arm_Table[PU_TOOL_1].base_arm); wait(2); @@ -528,12 +527,12 @@ wait(1); setServoPulse(1, Arm_Table[PU_TOOL_2].base_arm - 6); wait(2); - setServoPulse(5, 105); + setServoPulse(5, 140); wait(2); setServoPulse(5, 00); setServoPulse(1, Arm_Table[PU_TOOL_2].base_arm - 10); wait(2); - setServoPulse(5, 105); + setServoPulse(5, 140); wait(2); setServoPulse(1, Arm_Table[PU_TOOL_2].base_arm); @@ -589,12 +588,12 @@ wait(1); setServoPulse(1, Arm_Table[PU_TOOL_3].base_arm - 6); wait(.5); - setServoPulse(5, 105); + setServoPulse(5, 140); wait(.5); setServoPulse(5, 00); setServoPulse(1, Arm_Table[PU_TOOL_3].base_arm - 10); wait(1); - setServoPulse(5, 105); + setServoPulse(5, 140); wait(1); setServoPulse(1, Arm_Table[PU_TOOL_3].base_arm); @@ -1312,7 +1311,7 @@ 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 ); + while((abs(leftEncoder.getPulses()) < 900 || abs(rightEncoder.getPulses()) < 900) /*&& preLeft!=0*/ && IR.getIRDistance() >15 ); @@ -1338,7 +1337,11 @@ wait_ms(200); } } else if(section == MID || section == MID2) { - if(section == MID2) while(IR.getIRDistance() > 15 && (abs(leftEncoder.getPulses()) < 100 || abs(rightEncoder.getPulses()) < 100)); + if(section == MID2){ + motors.setMotor0Speed(.3*127); //right + motors.setMotor1Speed(.3*127); //left + while(IR.getIRDistance() > 15 && (abs(leftEncoder.getPulses()) < 300 || abs(rightEncoder.getPulses()) < 300)); + } while(IR.getIRDistance() > 15 && (abs(leftEncoder.getPulses()) < 100 || abs(rightEncoder.getPulses()) < 100) && (leftEncoder.getPulses() != preLeft || rightEncoder.getPulses() != preRight)) { @@ -1379,7 +1382,7 @@ void to_tools_section1(float* location, float ¤t) { - slightMove(FORWARD,6800); + slightMove(FORWARD,6850); current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2; } @@ -1406,7 +1409,7 @@ slightMove(BACKWARD,400); current-=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); - + rangeFinderLeft.startMeas(); wait_ms(20); rangeFinderLeft.getMeas(range); @@ -1508,6 +1511,12 @@ //current-=4; //} rightTurn(); + leftEncoder.reset(); + rightEncoder.reset(); + motors.setMotor0Speed(-0.4*127);// right + motors.setMotor1Speed(0.4*127);// left + while(abs(leftEncoder.getPulses())<50 || abs(rightEncoder.getPulses())<50); + motors.stopBothMotors(127); //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); @@ -1586,7 +1595,7 @@ if(range > 20 ) { direction[1]= RIGHT; location[2]= current; - slightMove(FORWARD,50); + slightMove(FORWARD,100); current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); } else { direction[1]= LEFT; @@ -1731,32 +1740,32 @@ } C = normd(areaArray, 10, CIRCLE_AREA); - // S = normd(areaArray, 10, SQUARE_AREA); - // T = normd(areaArray, 10, TRIANGLE_AREA); - + // S = normd(areaArray, 10, SQUARE_AREA); + // T = normd(areaArray, 10, TRIANGLE_AREA); + if((C < SQUARE_AREA) && (C > CIRCLE_AREA)) { - pc.printf("\n\nCIRCLE DETECTED\n\r"); - return CIRCLE; - } else if( ( C > SQUARE_AREA ) ) { pc.printf("\n\nSQUARE DETECTED\n\r"); return SQUARE; + } else if( ( C > SQUARE_AREA) ) { + pc.printf("\n\nCIRCLE DETECTED\n\r"); + return CIRCLE; } else { pc.printf("\n\nTRIANGLE DETECTED\n\r"); return TRIANGLE; - - - -/* - if((C < S) && (C < T)) { - pc.printf("\n\nCIRCLE DETECTED\n\r"); - return CIRCLE; - } else if( ( S<C ) && ( S<T ) ) { - pc.printf("\n\nSQUARE DETECTED\n\r"); - return SQUARE; - } else { - pc.printf("\n\nTRIANGLE DETECTED\n\r"); - return TRIANGLE; - }*/ + } + + + /* + if((C < S) && (C < T)) { + pc.printf("\n\nCIRCLE DETECTED\n\r"); + return CIRCLE; + } else if( ( S<C ) && ( S<T ) ) { + pc.printf("\n\nSQUARE DETECTED\n\r"); + return SQUARE; + } else { + pc.printf("\n\nTRIANGLE DETECTED\n\r"); + return TRIANGLE; + }*/ } float normd(int* pop, int count, int threshold)