ksdflsjdfljas
Dependencies: Adafruit-16-Ch-PWM-Servo-Driver HCSR04 PID PololuQik2 QEI Sharp mbed-rtos
Fork of theRobot by
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; + } }