nav fixed
Dependencies: Adafruit-16-Ch-PWM-Servo-Driver HCSR04 PID PololuQik2 QEI Sharp mbed-rtos
Fork of theRobot by
Diff: main.cpp
- Revision:
- 11:8d2455e383ce
- Parent:
- 10:1a1d52207f59
- Child:
- 12:284be46593ae
--- a/main.cpp Sat Mar 29 21:22:14 2014 +0000 +++ b/main.cpp Tue Apr 01 02:00:01 2014 +0000 @@ -63,6 +63,12 @@ #define TOOL_3 7 #define DRIVE_POSITION_TOOL 8 #define ORIENT_TOOL 9 +#define PU_TOOL_1 10 +#define PU_TOOL_2 11 +#define PU_TOOL_3 12 +#define PU_TOOL_1_STAB 13 +#define PU_TOOL_2_STAB 14 +#define PU_TOOL_3_STAB 15 //Rig definitions #define SQUARE 1 @@ -179,16 +185,22 @@ //increase in number 5 rotates gripper - {STORE_POSITION, 85, 10, 0, 175, 100, 0}, // storing position - {OIL_RIG1, 161, 90, 90, 47, 100, 0}, // point laser at oilrig1 - {OIL_RIG2, 140, 90, 90, 44, 100, 0}, // point laser at oilrig2 + {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, 10, 0, 175, 100, 0}, // Drive through course - {TOOL_1, 95, 64, 97, 79, 0, 0}, // Look over first tool - {TOOL_2, 75, 70, 102, 74, 0, 0}, // Look over second tool - {TOOL_3, 55, 70, 102, 74, 0, 0}, // Look over third tool + {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 {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 + {PU_TOOL_2, 78, 50, 82, 108, 90, 0}, // STATIC Pickup tool POSITION + {PU_TOOL_3, 53, 50, 82, 118, 90, 0}, // STATIC Pickup tool POSITION + {PU_TOOL_1_STAB, 98, 50, 90, 118, 90, 0}, // STAB TOOL 1 + {PU_TOOL_2_STAB, 78, 50, 90, 108, 90, 0}, // STAB TOOL 2 + {PU_TOOL_3_STAB, 53, 50, 90, 118, 90, 0}, // STAB TOOL 3 }; @@ -223,14 +235,19 @@ //Servo initialization initServoDriver(); servoBegin(); //initiates servos to start position - //servoPosition(0); - ServoOutputDisable = 0; + //ServoOutputDisable = 0; + + + + + + /******************* WHILE LOOP FOR TESTING ********************/ while(1) { - + pc.scanf("%d %d", &servoNum, &servoAngle); if(servoAngle > 175) { servoAngle = 175; @@ -240,21 +257,104 @@ 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){};*/ + //shape_detected = shapeDetection(); //distLaser = getDistance(); //pc.printf("Distance %d", distLaser); //ledtoggle(); + /* + int shape_alignX_done = 0; + int shape_alignY_done = 0; - //pc.scanf("%d", &posNum); - //servoPosition(posNum); - //wait(5); - //shape_detected = shapeDetection(); - distLaser = getDistance(); - pc.printf("Distance %d", distLaser); + + //pc.scanf("%d", &posNum); + + while(pc.getc() != 'g'); + servoPosition(TOOL_1); + while(pc.getc() != 'g'); + //shape_detected = shapeDetection(); + + 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_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 { + shape_alignX_done = 1; + } + + + } + printImageToFile(BINARY);*/ } + + + + /******************************** MAIN WHILE LOOP FOR COMPETITION *********************************/ @@ -284,15 +384,14 @@ case OILRIG1_POS: //aims arm at square oil rig setServoPulse(3, Arm_Table[OIL_RIG1].claw_arm); - setServoPulse(2, 50); + setServoPulse(2, Arm_Table[OIL_RIG1].big_arm); setServoPulse(1, Arm_Table[OIL_RIG1].base_arm); - setServoPulse(2, Arm_Table[OIL_RIG1].big_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); //wait for servos to settle - + fire = fire_checker(OIL_RIG1); //determines if oil rig is on fire pc.printf("FIRE: %d", fire); //determines what tool is needed @@ -327,7 +426,7 @@ * **************************************************/ case GOTO_TOOLS: - + 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); @@ -563,11 +662,11 @@ void servoBegin(void) { pc.printf("Setting Initial Position\n\r"); - setServoPulseNo_delay(3, 175); setServoPulseNo_delay(2, 0); - setServoPulseNo_delay(1, 2); + setServoPulseNo_delay(3, 162); + setServoPulseNo_delay(1, 10); setServoPulseNo_delay(0, 85); - setServoPulseNo_delay(4, 100); + setServoPulseNo_delay(4, 175); setServoPulseNo_delay(5, 0); setGripper(1); }