ksdflsjdfljas
Dependencies: Adafruit-16-Ch-PWM-Servo-Driver HCSR04 PID PololuQik2 QEI Sharp mbed-rtos
Fork of theRobot by
main.cpp
- Committer:
- tashworth
- Date:
- 2014-03-19
- Revision:
- 6:75259c3306dd
- Parent:
- 5:429e9a998bab
- Child:
- 7:8fb4204f9600
File content as of revision 6:75259c3306dd:
#include "mbed.h" #include "Adafruit_PWMServoDriver.h" #include "ShapeDetect.h" 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 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 *************/ int servoNum, servoAngle, outputDisabled, posNum, testVal; int currentPosition[7]; typedef struct { int arm_position_name; //for organization only (STORE, OILRIG1, OILRIG2...) int base_rotate; int base_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, int big_arm, int claw_arm, int claw_rotate, int claw_open {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 }; /* Variables for imageprocessing and distance */ int x_coord; int y_coord; int area; int shape; /* Variables for distance sensor*/ int dist; int fire_detected = 0; int fire_not_detected = 0; int main() { /***************** INITIALIZATIONS *******************/ pc.baud(115200); //Laser Range Finder Initialization lrf_baudCalibration(); //Servo initialization initServoDriver(); 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) { switch (state) { case START : myled1 = 1; wait(5); myled1 = 0; state = OILRIG1_POS; break; /************************************************** * 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; /************************************************** * 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 //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; 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; /************************************************** * 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; /************************************************** * STAGE 7 * * - Insert Tool * - Extenguish fire * - win contest * **************************************************/ case INSERT_TOOL: //*********************// //******* TODO ********// //*********************// // CODE TO INSERT TOOL break; /************************************************** * 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: break; } } } /************ Servo Functions **************/ void setServoPulse(uint8_t n, int angle) { int pulse; pulse = MIN_SERVO_PULSE + (( angle * (MAX_SERVO_PULSE - MIN_SERVO_PULSE)) / SERVO_MAX_ANGLE); float pulselength = 20000; // 20,000 us per second int i = currentPosition[n]; pc.printf("ServoNumber: %d Begining Pulse: %d\n\r",n,currentPosition[n]); int pulse2; if(currentPosition[n] < pulse) { for(i; i < pulse; i++) { pulse2 = 4094 * i / pulselength; pwm.setPWM(n, 0, pulse2); wait_ms(3); } } else if (currentPosition[n] > pulse) { for(i; i > pulse; i--) { pulse2 = 4094 * i / pulselength; pwm.setPWM(n, 0, pulse2); wait_ms(3); } } currentPosition[n] = i; pc.printf("\nEND: pulse: %d, angle: %d\n\n", i, angle); } void initServoDriver(void) { pwm.begin(); //pwm.setPWMFreq(100); //This dosen't work well because of uncertain clock speed. Use setPrescale(). pwm.setPrescale(140); //This value is decided for 20ms interval. pwm.setI2Cfreq(400000); //400kHz } void servoBegin(void) { pc.printf("Setting Initial Position\n\r"); setServoPulseNo_delay(0, 90); setServoPulseNo_delay(1, 0); setServoPulseNo_delay(2, 177); setServoPulseNo_delay(3, 0); setServoPulseNo_delay(4, 0); setServoPulseNo_delay(5, 90); setGripper(1); } void setServoPulseNo_delay(uint8_t n, int angle) { int pulse = MIN_SERVO_PULSE + (( angle * (MAX_SERVO_PULSE - MIN_SERVO_PULSE)) / SERVO_MAX_ANGLE); float pulselength = 20000; // 20,000 us per second currentPosition[n] = pulse; pc.printf("ServoNumber: %d Pulsewidth: %d Angle: %d \n\r",n,pulse, angle); pulse = 4094 * pulse / pulselength; pwm.setPWM(n, 0, pulse); } void setGripper(int open) { if (open) { pc.printf("Gripper Open\r"); setServoPulseNo_delay(6, 10); } else { pc.printf("Gripper Closed\n\r"); setServoPulseNo_delay(6, 170); } } void servoPosition(int set) { //moves to current position setServoPulse(0, Arm_Table[set].base_rotate); setServoPulse(1, Arm_Table[set].base_arm); 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; } }