Thomas Ashworth
For IEEE Robotics
Dependencies: Adafruit-16-Ch-PWM-Servo-Driver HCSR04 PID PololuQik2 QEI Sharp mbed-rtos
Diff: main.cpp
- Revision:
- 6:75259c3306dd
- Parent:
- 5:429e9a998bab
- Child:
- 7:8fb4204f9600
--- a/main.cpp Fri Mar 14 22:15:58 2014 +0000
+++ b/main.cpp Wed Mar 19 17:05:35 2014 +0000
@@ -2,37 +2,87 @@
#include "Adafruit_PWMServoDriver.h"
#include "ShapeDetect.h"
-Serial pc(USBTX,USBRX);
-Adafruit_PWMServoDriver pwm(p28,p27);
-DigitalOut ServoOutputDisable(p8);
-extern Serial lrf;
+DigitalOut myled1(LED1);
+DigitalOut myled2(LED2);
+DigitalOut myled3(LED3);
+DigitalOut myled4(LED4);
+
+Serial pc(USBTX,USBRX); //USB Comm
+Adafruit_PWMServoDriver pwm(p28,p27); //Servo Control PWM
+DigitalOut ServoOutputDisable(p8); //Servo Control Output Enable/Disable
+extern Serial lrf; //Laser Range Finder lrf(p13,p14)
+
+//States
+#define START 0
+#define OILRIG1_POS 1
+#define OILRIG2_POS 2
+#define GOTO_TOOLS 3
+#define IDENTIFY_TOOLS 4
+#define AQUIRE_TOOL1 5
+#define AQUIRE_TOOL2 6
+#define AQUIRE_TOOL3 7
+#define NAVIGATE_WAVES_ROW1 8
+#define NAVIGATE_WAVES_ROW2 9
+#define NAVIGATE_WAVES_ROW3 10
+#define NAVIGATE_TO_SQUARE_RIG 11
+#define NAVIGATE_TO_TRIANGLE_RIG 12
+#define NAVIGATE_TO_CIRCLE_RIG 13
+#define RIG_ALIGN 14
+#define INSERT_TOOL 15
+#define END 16
+
-//Servo Positions
-#define STORE_POSITION 0
-#define OIL_RIG1 1
-#define OIL_RIG2 2
-#define OIL_RIG3 3
-#define IMG_SHAPE_1 4
-#define IMG_SHAPE_2 5
-#define IMG_SHAPE_3 6
-#define GRASP_SHAPE_1 7
-#define GRASP_SHAPE_2 8
-#define GRASP_SHAPE_3 9
-#define INSERT_TOOL_1 10
-#define INSERT_TOOL_2 11
-#define INSERT_TOOL_3 11
+//Servo Static Positions
+#define STORE_POSITION 0
+#define OIL_RIG1 1
+#define OIL_RIG2 2
+#define OIL_RIG3 3
+#define DRIVE_POSITION_NOTOOL 4
+#define TOOL_1 5
+#define TOOL_2 6
+#define TOOL_3 7
+#define DRIVE_POSITION_TOOL 8
+#define ORIENT_TOOL 9
+//Rig definitions
+#define SQUARE 1
+#define TRIANGLE 2
+#define CIRCLE 3
+
+//*********************//
+//******* TODO ********//
+//*********************//
+//Oil Rig distance thresholds
+#define OILRIG1_MAX 1200
+#define OILRIG1_MIN 1200
+#define OILRIG2_MAX 1200
+#define OILRIG2_MIN 1200
+#define OILRIG3_MAX 1200
+#define OILRIG3_MIN 1200
+
+//for servo normalization
#define MIN_SERVO_PULSE 900
#define MAX_SERVO_PULSE 2100
#define SERVO_MAX_ANGLE 180
-
+/***************
+local servo functions
+****************/
void servoBegin(void);
void initServoDriver(void);
void setServoPulse(uint8_t n, int angle);
void setServoPulseNo_delay(uint8_t n, int angle);
void servoPosition(int set);
void setGripper(int open);
+int fire_checker(int rig);
+
+/************
+Main Variables
+*************/
+int state = START;
+int fire = 0;
+int tool_needed = 0;
+int shape_detected = 0;
/************
Variables for Servos
@@ -41,25 +91,34 @@
int currentPosition[7];
typedef struct {
- int arm_action;
+ int arm_position_name; //for organization only (STORE, OILRIG1, OILRIG2...)
int base_rotate;
int base_arm;
- int lil_arm;
int big_arm;
int claw_arm;
int claw_rotate;
int claw_open;
} Coord;
+/********************
+Static Arm Positions
+*********************/
+
Coord Arm_Table[] = {
+
// POSITION ODER:
- // base_rotate, base_arm, lil_arm, int big_arm, int claw_arm, int claw_rotate, int claw_open
+ // base_rotate, base_arm, int big_arm, int claw_arm, int claw_rotate, int claw_open
- {STORE_POSITION, 90, 0, 177, 0, 0, 90, 90}, // storing position
- {OIL_RIG1, 90, 60, 130, 75, 60, 90, 90}, // point laser at oilrig1
- {OIL_RIG2, 120, 60, 130, 75, 60, 90, 90}, // point laser at oilrig2
- {OIL_RIG3, 135, 60, 130, 75, 60, 90, 90}, // point laser at oilrig2
-
+ {STORE_POSITION, 90, 0, 0, 0, 90, 90}, // storing position
+ {OIL_RIG1, 90, 60, 75, 60, 90, 90}, // point laser at oilrig1
+ {OIL_RIG2, 120, 60, 75, 60, 90, 90}, // point laser at oilrig2
+ {OIL_RIG3, 135, 60, 75, 60, 90, 90}, // point laser at oilrig2
+ {DRIVE_POSITION_NOTOOL, 135, 60, 75, 60, 90, 90}, // Drive through course
+ {TOOL_1, 135, 60, 75, 60, 90, 90}, // Look over first tool
+ {TOOL_2, 135, 60, 75, 60, 90, 90}, // Look over second tool
+ {TOOL_3, 135, 60, 75, 60, 90, 90}, // Look over third tool
+ {DRIVE_POSITION_TOOL, 135, 60, 75, 60, 90, 90}, // Drive with tool loaded
+ {ORIENT_TOOL, 135, 60, 75, 60, 90, 90}, // position tool to be inserted
};
@@ -69,9 +128,10 @@
int area;
int shape;
-
-
-
+/* Variables for distance sensor*/
+int dist;
+int fire_detected = 0;
+int fire_not_detected = 0;
int main()
@@ -81,84 +141,260 @@
INITIALIZATIONS
*******************/
pc.baud(115200);
-//Laser Range Finder Initialization
+ //Laser Range Finder Initialization
lrf_baudCalibration();
-//Servo initialization
+ //Servo initialization
initServoDriver();
- servoBegin(); // initiates servos to start position
-//ServoOutputDisable = 0;
-while(1){
- //pc.scanf("%d %d", &servoNum, &servoAngle);
- //setServoPulse(servoNum, servoAngle);
- pc.scanf("%d", &posNum);
- servoPosition(posNum);
-
-
+ servoBegin(); //initiates servos to start position
+ //ServoOutputDisable = 0;
+
+ /*******************
+ WHILE LOOP FOR TESTING
+ ********************/
+ while(1) {
+ //pc.scanf("%d %d", &servoNum, &servoAngle);
+ //setServoPulse(servoNum, servoAngle);
+
+ pc.scanf("%d", &posNum);
+ servoPosition(posNum);
}
-/*
while(1) {
- pc.printf("PICK A TEST TO PERFORM\n");
- pc.printf("1) Distance Reading\n");
- pc.printf("2) Shape Detection\n");
- pc.printf("3) Servo Control\n");
- pc.printf("4) Motor Control\n");
+ switch (state) {
- pc.scanf("%d", &testVal);
-
- switch (testVal) {
- case 1:
- pc.printf("Distance = %d\n", getDistance());
+ case START :
+ myled1 = 1;
+ wait(5);
+ myled1 = 0;
+ state = OILRIG1_POS;
break;
- case 2:
- shape = shapeDetection_mass();
- printImageToFile(BINARY);
- break;
- case 3:
- servoBegin();
- pc.scanf("%d %d", &servoNum, &servoAngle);
- setServoPulse(servoNum, servoAngle);
- break;
- default:
- pc.printf("ERROR: NOT A VALID TEST PROCEDURE");
- break;
- }*/
- /*if(pc.readable()) {
+ /**************************************************
+ * STAGE 1
+ *
+ * - DETERMINE OIL RIG ON FIRE
+ *
+ **************************************************/
+ case OILRIG1_POS: //aims arm at square oil rig
+ servoPosition(OIL_RIG1); //position arm to point at first oilrig
+ wait(1); //wait for servos to settle
+ fire = fire_checker(OIL_RIG1); //determines if oil rig is on fire
+
+ //determines what tool is needed
+ if (fire == 1) {
+ tool_needed = SQUARE;
+ state = GOTO_TOOLS;
+ } else {
+ state = OILRIG2_POS;
+ }
+ break;
+
+ case OILRIG2_POS:
+ servoPosition(OIL_RIG2); //position arm to point at first oilrig
+ wait(1); //wait for servos to settle
+ fire = fire_checker(OIL_RIG2);
+ if (fire == 1) {
+ tool_needed = TRIANGLE;
+ state = GOTO_TOOLS;
+ } else {
+ tool_needed = CIRCLE;
+ state = GOTO_TOOLS;
+ }
+ break;
+
+ /**************************************************
+ * STAGE 2
+ *
+ * - TRAVEL TO TOOLS
+ *
+ **************************************************/
+ case GOTO_TOOLS:
+ servoPosition(DRIVE_POSITION_NOTOOL); //drive position without a tool
+ wait(1); //wait for servos to settle
+
+ //*********************//
+ //******* TODO ********//
+ //*********************//
+ //CODE TO NAVIGATE TO TOOLS
+
+ state = IDENTIFY_TOOLS;
+ break;
- pc.scanf("%d %d", &servoNum, &pulseWidth);
- setServoPulse(servoNum, pulseWidth);
+ /**************************************************
+ * STAGE 3
+ *
+ * - Determine order of tools
+ * - Aquire appropriate tool
+ *
+ **************************************************/
+ case IDENTIFY_TOOLS:
+ servoPosition(TOOL_1); //arm/camera looks over tool
+ wait(1); //wait for servos to settle
+ centerCamWithTool(); //centers camera over tool
+ shape_detected = shapeDetection(); //determines the shape
-
- // pc.scanf("%d", &posNum);
- //servoPosition(posNum);
+ //either goes to aquire the tool or look at the next shape
+ if(shape_detected == tool_needed) {
+ state = AQUIRE_TOOL1;
+ } else {
+ servoPosition(TOOL_2);
+ wait(1); //wait for servos to settle
+ centerCamWithTool();
+ shape_detected = shapeDetection();
+ if (shape_detected == tool_needed) {
+ state = AQUIRE_TOOL2;
+ } else {
+ servoPosition(TOOL_3);
+ wait(1); //wait for servos to settle
+ centerCamWithTool();
+ state = AQUIRE_TOOL3;
+ }
+ }
+ break;
+ case AQUIRE_TOOL1:
+ //*********************//
+ //******* TODO ********//
+ //*********************//
+ // CODE TO GRAB TOOL1
+ 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(DRIVE_POSITION_TOOL); //arm position for driving with the tool
+ state = NAVIGATE_WAVES_ROW1;
+ break;
+ case AQUIRE_TOOL3:
+ //*********************//
+ //******* TODO ********//
+ //*********************//
+ // CODE TO GRAB TOOL3
+ servoPosition(DRIVE_POSITION_TOOL); //arm position for driving with the tool
+ state = NAVIGATE_WAVES_ROW1;
+ break;
- }
- }*/
+ /**************************************************
+ * STAGE 4
+ *
+ * - Navigate through the ocean
+ *
+ **************************************************/
+
+ case NAVIGATE_WAVES_ROW1:
+ //*********************//
+ //******* TODO ********//
+ //*********************//
+ // CODE TO NAVIGATE ROW1
+ state = NAVIGATE_WAVES_ROW2;
+ break;
+
+ case NAVIGATE_WAVES_ROW2:
+ //*********************//
+ //******* TODO ********//
+ //*********************//
+ // CODE TO NAVIGATE ROW2
+ state = NAVIGATE_WAVES_ROW3;
+ break;
- /**************************************************
- * FIRST STAGE
- *
- * - DETERMINE OIL RIG ON FIRE
- * - DETERMINE PATH
- *
- **************************************************/
+ case NAVIGATE_WAVES_ROW3:
+ //*********************//
+ //******* TODO ********//
+ //*********************//
+ // CODE TO NAVIGATE ROW3
+
+ //goes to appropriate rig
+ if(shape_detected == 1) {
+ state = NAVIGATE_TO_SQUARE_RIG;
+ } else if(shape_detected == 2) {
+ state = NAVIGATE_TO_TRIANGLE_RIG;
+ } else {
+ state = NAVIGATE_TO_CIRCLE_RIG;
+ }
+ break;
+
+ /**************************************************
+ * STAGE 5
+ *
+ * - Travel to appropriate rig
+ *
+ **************************************************/
+ case NAVIGATE_TO_SQUARE_RIG:
+ //NAVIGATION CODE HERE
+ state = RIG_ALIGN;
+ break;
+ case NAVIGATE_TO_TRIANGLE_RIG:
+ //NAVIGATION CODE HERE
+ state = RIG_ALIGN;
+ break;
+ case NAVIGATE_TO_CIRCLE_RIG:
+ //NAVIGATION CODE HERE
+ state = RIG_ALIGN;
+ break;
- //TODO: EXTEND ARM AND FACE OILRIGS
-
- //OILRIG 1 DISTANCE READING
-
- //TODO: ROTATE ARM TO NEXT OIL RIG
+ /**************************************************
+ * STAGE 6
+ *
+ * - Align with appropriate rig
+ *
+ **************************************************/
+ case RIG_ALIGN:
+
+ //*********************//
+ //******* TODO ********//
+ //*********************//
+ // CODE TO ALIGN ROBOT WITH RIG
+
+ servoPosition(ORIENT_TOOL);
+ wait(1); //wait for servos to settle
+ state = INSERT_TOOL;
+ break;
- //OILRIG 2 DISTANCE READING
+ /**************************************************
+ * STAGE 7
+ *
+ * - Insert Tool
+ * - Extenguish fire
+ * - win contest
+ *
+ **************************************************/
+
+ case INSERT_TOOL:
+ //*********************//
+ //******* TODO ********//
+ //*********************//
+ // CODE TO INSERT TOOL
+ break;
- //ROTATE ARM TO NEXT OIL RIG
+ /**************************************************
+ * STAGE 8
+ *
+ * - END COMPETITION
+ *
+ **************************************************/
+ case END:
+ servoPosition(STORE_POSITION);
+ myled1 = 1;
+ wait(.2);
+ myled2 = 1;
+ wait(.2);
+ myled3 = 1;
+ wait(.2);
+ myled4 = 1;
+ wait(.2);
+ break;
+ default:
- //OILRIG 3 DISTANCE READING
+ break;
+ }
+ }
+
}
@@ -243,11 +479,63 @@
//moves to current position
setServoPulse(0, Arm_Table[set].base_rotate);
setServoPulse(1, Arm_Table[set].base_arm);
- setServoPulse(2, Arm_Table[set].lil_arm);
- setServoPulse(3, Arm_Table[set].big_arm);
- setServoPulse(4, Arm_Table[set].claw_arm);
- setServoPulse(5, Arm_Table[set].claw_rotate);
- setServoPulse(6, Arm_Table[set].claw_open);
+ setServoPulse(2, Arm_Table[set].big_arm);
+ setServoPulse(3, Arm_Table[set].claw_arm);
+ setServoPulse(4, Arm_Table[set].claw_rotate);
+ setServoPulse(5, Arm_Table[set].claw_open);
+}
+
+int fire_checker(int rig)
+{
+ switch (rig) {
+
+ case 1:
+ for (int i = 0; i<5; i++) {
+ dist = getDistance();
+ if ((dist < OILRIG1_MAX)
+ || (dist > OILRIG1_MIN)) {
+ fire_detected++;
+ } else {
+ fire_not_detected++;
+ }
+ }
+ case 2:
+ for (int i = 0; i<5; i++) {
+ dist = getDistance();
+ if ((dist < OILRIG2_MAX)
+ || (dist > OILRIG2_MIN)) {
+ fire_detected++;
+ } else {
+ fire_not_detected++;
+ }
+ }
+ case 3:
+ for (int i = 0; i<5; i++) {
+ dist = getDistance();
+ if ((dist < OILRIG3_MAX)
+ || (dist > OILRIG3_MIN)) {
+ fire_detected++;
+ } else {
+ fire_not_detected++;
+ }
+ }
+ default:
+ for (int i = 0; i<5; i++) {
+ dist = getDistance();
+ if ((dist < OILRIG1_MAX)
+ || (dist > OILRIG1_MIN)) {
+ fire_detected++;
+ } else {
+ fire_not_detected++;
+ }
+ }
+ }
+
+ if (fire_detected > fire_not_detected) {
+ return 1;
+ } else {
+ return 0;
+ }
}