Thomas Ashworth
For IEEE Robotics
Dependencies: Adafruit-16-Ch-PWM-Servo-Driver HCSR04 PID PololuQik2 QEI Sharp mbed-rtos
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);
}