Thomas Ashworth
/
theRobot
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: Adafruit-16-Ch-PWM-Servo-Driver HCSR04 PID PololuQik2 QEI Sharp mbed-rtos
Revision 11:8d2455e383ce, committed 2014-04-01
- Comitter:
- tashworth
- Date:
- Tue Apr 01 02:00:01 2014 +0000
- Parent:
- 10:1a1d52207f59
- Child:
- 12:284be46593ae
- Commit message:
- 3-31-14 end of day
Changed in this revision
--- a/ShapeDetect.cpp Sat Mar 29 21:22:14 2014 +0000
+++ b/ShapeDetect.cpp Tue Apr 01 02:00:01 2014 +0000
@@ -208,8 +208,8 @@
void centerMass(int *xcoord, int *ycoord, int *s_area){
ImageToArray(BINARY);
- printImageToFile(BINARY);
- clearBounds();
+ //printImageToFile(BINARY);
+ //clearBounds();
int count = 0;
int sumi = 0;
int sumj = 0;
@@ -316,6 +316,20 @@
//CODE GOES HERE
- };
+ }
+int get_com_x(void)
+{
+ return xcoord_val;
+ }
+int get_com_y(void)
+{
+ return ycoord_val;
+ }
+
+int get_com_a(void)
+{
+ return s_area_val;
+}
+
--- a/ShapeDetect.h Sat Mar 29 21:22:14 2014 +0000 +++ b/ShapeDetect.h Tue Apr 01 02:00:01 2014 +0000 @@ -2,11 +2,11 @@ #define SHAPEDETECT_H_ /* theshold for setting binary output */ -#define THRESHOLD 65 +#define THRESHOLD 100 //areas from camera 11" from ground -#define TRIANGE_AREA_TRESHOLD 3300 -#define SQUARE_AREA_TRESHOLD 4900 +#define TRIANGE_AREA_TRESHOLD 3350 +#define SQUARE_AREA_TRESHOLD 3800 /* modes for image processing */ #define BINARY 1 @@ -24,7 +24,9 @@ int shapeDetection(void); int getDistance(void); void centerCamWithTool(void); - +int get_com_x(void); +int get_com_y(void); +int get_com_a(void); #endif \ No newline at end of file
--- 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);
}