Thomas Ashworth / theRobot

For IEEE Robotics

Dependencies:   Adafruit-16-Ch-PWM-Servo-Driver HCSR04 PID PololuQik2 QEI Sharp mbed-rtos

main.cpp@7:8fb4204f9600, 2014-03-19 (annotated)

Committer:
tashworth
Date:
Wed Mar 19 17:08:25 2014 +0000
Revision:
7:8fb4204f9600
Parent:
6:75259c3306dd
Child:
8:77a57909aa15
3-19-2014

Who changed what in which revision?

UserRevisionLine numberNew contents of line
tashworth 0:1b64a0cedc5d 1 #include "mbed.h"
tashworth 0:1b64a0cedc5d 2 #include "Adafruit_PWMServoDriver.h"
tashworth 0:1b64a0cedc5d 3 #include "ShapeDetect.h"
tashworth 0:1b64a0cedc5d 4
tashworth 6:75259c3306dd 5 DigitalOut myled1(LED1);
tashworth 6:75259c3306dd 6 DigitalOut myled2(LED2);
tashworth 6:75259c3306dd 7 DigitalOut myled3(LED3);
tashworth 6:75259c3306dd 8 DigitalOut myled4(LED4);
tashworth 6:75259c3306dd 9
tashworth 6:75259c3306dd 10 Serial pc(USBTX,USBRX); //USB Comm
tashworth 6:75259c3306dd 11 Adafruit_PWMServoDriver pwm(p28,p27); //Servo Control PWM
tashworth 6:75259c3306dd 12 DigitalOut ServoOutputDisable(p8); //Servo Control Output Enable/Disable
tashworth 6:75259c3306dd 13 extern Serial lrf; //Laser Range Finder lrf(p13,p14)
tashworth 6:75259c3306dd 14
tashworth 6:75259c3306dd 15 //States
tashworth 6:75259c3306dd 16 #define START 0
tashworth 6:75259c3306dd 17 #define OILRIG1_POS 1
tashworth 6:75259c3306dd 18 #define OILRIG2_POS 2
tashworth 6:75259c3306dd 19 #define GOTO_TOOLS 3
tashworth 6:75259c3306dd 20 #define IDENTIFY_TOOLS 4
tashworth 6:75259c3306dd 21 #define AQUIRE_TOOL1 5
tashworth 6:75259c3306dd 22 #define AQUIRE_TOOL2 6
tashworth 6:75259c3306dd 23 #define AQUIRE_TOOL3 7
tashworth 6:75259c3306dd 24 #define NAVIGATE_WAVES_ROW1 8
tashworth 6:75259c3306dd 25 #define NAVIGATE_WAVES_ROW2 9
tashworth 6:75259c3306dd 26 #define NAVIGATE_WAVES_ROW3 10
tashworth 6:75259c3306dd 27 #define NAVIGATE_TO_SQUARE_RIG 11
tashworth 6:75259c3306dd 28 #define NAVIGATE_TO_TRIANGLE_RIG 12
tashworth 6:75259c3306dd 29 #define NAVIGATE_TO_CIRCLE_RIG 13
tashworth 6:75259c3306dd 30 #define RIG_ALIGN 14
tashworth 6:75259c3306dd 31 #define INSERT_TOOL 15
tashworth 6:75259c3306dd 32 #define END 16
tashworth 6:75259c3306dd 33
tashworth 0:1b64a0cedc5d 34
tashworth 6:75259c3306dd 35 //Servo Static Positions
tashworth 6:75259c3306dd 36 #define STORE_POSITION 0
tashworth 6:75259c3306dd 37 #define OIL_RIG1 1
tashworth 6:75259c3306dd 38 #define OIL_RIG2 2
tashworth 6:75259c3306dd 39 #define OIL_RIG3 3
tashworth 6:75259c3306dd 40 #define DRIVE_POSITION_NOTOOL 4
tashworth 6:75259c3306dd 41 #define TOOL_1 5
tashworth 6:75259c3306dd 42 #define TOOL_2 6
tashworth 6:75259c3306dd 43 #define TOOL_3 7
tashworth 6:75259c3306dd 44 #define DRIVE_POSITION_TOOL 8
tashworth 6:75259c3306dd 45 #define ORIENT_TOOL 9
tashworth 2:4e082e4c255d 46
tashworth 6:75259c3306dd 47 //Rig definitions
tashworth 6:75259c3306dd 48 #define SQUARE 1
tashworth 6:75259c3306dd 49 #define TRIANGLE 2
tashworth 6:75259c3306dd 50 #define CIRCLE 3
tashworth 6:75259c3306dd 51
tashworth 6:75259c3306dd 52 //*********************//
tashworth 6:75259c3306dd 53 //******* TODO ********//
tashworth 6:75259c3306dd 54 //*********************//
tashworth 6:75259c3306dd 55 //Oil Rig distance thresholds
tashworth 6:75259c3306dd 56 #define OILRIG1_MAX 1200
tashworth 6:75259c3306dd 57 #define OILRIG1_MIN 1200
tashworth 6:75259c3306dd 58 #define OILRIG2_MAX 1200
tashworth 6:75259c3306dd 59 #define OILRIG2_MIN 1200
tashworth 6:75259c3306dd 60 #define OILRIG3_MAX 1200
tashworth 6:75259c3306dd 61 #define OILRIG3_MIN 1200
tashworth 6:75259c3306dd 62
tashworth 6:75259c3306dd 63 //for servo normalization
tashworth 3:b7b4780a7f6e 64 #define MIN_SERVO_PULSE 900
tashworth 3:b7b4780a7f6e 65 #define MAX_SERVO_PULSE 2100
tashworth 3:b7b4780a7f6e 66 #define SERVO_MAX_ANGLE 180
tashworth 3:b7b4780a7f6e 67
tashworth 6:75259c3306dd 68 /***************
tashworth 6:75259c3306dd 69 local servo functions
tashworth 6:75259c3306dd 70 ****************/
tashworth 1:fe4a0b47ff25 71 void servoBegin(void);
tashworth 0:1b64a0cedc5d 72 void initServoDriver(void);
tashworth 3:b7b4780a7f6e 73 void setServoPulse(uint8_t n, int angle);
tashworth 3:b7b4780a7f6e 74 void setServoPulseNo_delay(uint8_t n, int angle);
tashworth 0:1b64a0cedc5d 75 void servoPosition(int set);
tashworth 3:b7b4780a7f6e 76 void setGripper(int open);
tashworth 6:75259c3306dd 77 int fire_checker(int rig);
tashworth 6:75259c3306dd 78
tashworth 6:75259c3306dd 79 /************
tashworth 6:75259c3306dd 80 Main Variables
tashworth 6:75259c3306dd 81 *************/
tashworth 6:75259c3306dd 82 int state = START;
tashworth 6:75259c3306dd 83 int fire = 0;
tashworth 6:75259c3306dd 84 int tool_needed = 0;
tashworth 6:75259c3306dd 85 int shape_detected = 0;
tashworth 0:1b64a0cedc5d 86
tashworth 0:1b64a0cedc5d 87 /************
tashworth 0:1b64a0cedc5d 88 Variables for Servos
tashworth 0:1b64a0cedc5d 89 *************/
tashworth 3:b7b4780a7f6e 90 int servoNum, servoAngle, outputDisabled, posNum, testVal;
tashworth 3:b7b4780a7f6e 91 int currentPosition[7];
tashworth 0:1b64a0cedc5d 92
tashworth 3:b7b4780a7f6e 93 typedef struct {
tashworth 6:75259c3306dd 94 int arm_position_name; //for organization only (STORE, OILRIG1, OILRIG2...)
tashworth 3:b7b4780a7f6e 95 int base_rotate;
tashworth 3:b7b4780a7f6e 96 int base_arm;
tashworth 3:b7b4780a7f6e 97 int big_arm;
tashworth 3:b7b4780a7f6e 98 int claw_arm;
tashworth 3:b7b4780a7f6e 99 int claw_rotate;
tashworth 3:b7b4780a7f6e 100 int claw_open;
tashworth 3:b7b4780a7f6e 101 } Coord;
tashworth 3:b7b4780a7f6e 102
tashworth 6:75259c3306dd 103 /********************
tashworth 6:75259c3306dd 104 Static Arm Positions
tashworth 6:75259c3306dd 105 *********************/
tashworth 6:75259c3306dd 106
tashworth 3:b7b4780a7f6e 107 Coord Arm_Table[] = {
tashworth 6:75259c3306dd 108
tashworth 3:b7b4780a7f6e 109 // POSITION ODER:
tashworth 6:75259c3306dd 110 // base_rotate, base_arm, int big_arm, int claw_arm, int claw_rotate, int claw_open
tashworth 3:b7b4780a7f6e 111
tashworth 6:75259c3306dd 112 {STORE_POSITION, 90, 0, 0, 0, 90, 90}, // storing position
tashworth 6:75259c3306dd 113 {OIL_RIG1, 90, 60, 75, 60, 90, 90}, // point laser at oilrig1
tashworth 6:75259c3306dd 114 {OIL_RIG2, 120, 60, 75, 60, 90, 90}, // point laser at oilrig2
tashworth 6:75259c3306dd 115 {OIL_RIG3, 135, 60, 75, 60, 90, 90}, // point laser at oilrig2
tashworth 6:75259c3306dd 116 {DRIVE_POSITION_NOTOOL, 135, 60, 75, 60, 90, 90}, // Drive through course
tashworth 6:75259c3306dd 117 {TOOL_1, 135, 60, 75, 60, 90, 90}, // Look over first tool
tashworth 6:75259c3306dd 118 {TOOL_2, 135, 60, 75, 60, 90, 90}, // Look over second tool
tashworth 6:75259c3306dd 119 {TOOL_3, 135, 60, 75, 60, 90, 90}, // Look over third tool
tashworth 6:75259c3306dd 120 {DRIVE_POSITION_TOOL, 135, 60, 75, 60, 90, 90}, // Drive with tool loaded
tashworth 6:75259c3306dd 121 {ORIENT_TOOL, 135, 60, 75, 60, 90, 90}, // position tool to be inserted
tashworth 3:b7b4780a7f6e 122 };
tashworth 3:b7b4780a7f6e 123
tashworth 3:b7b4780a7f6e 124
tashworth 3:b7b4780a7f6e 125 /* Variables for imageprocessing and distance */
tashworth 3:b7b4780a7f6e 126 int x_coord;
tashworth 3:b7b4780a7f6e 127 int y_coord;
tashworth 3:b7b4780a7f6e 128 int area;
tashworth 3:b7b4780a7f6e 129 int shape;
tashworth 3:b7b4780a7f6e 130
tashworth 6:75259c3306dd 131 /* Variables for distance sensor*/
tashworth 6:75259c3306dd 132 int dist;
tashworth 6:75259c3306dd 133 int fire_detected = 0;
tashworth 6:75259c3306dd 134 int fire_not_detected = 0;
tashworth 3:b7b4780a7f6e 135
tashworth 0:1b64a0cedc5d 136
tashworth 3:b7b4780a7f6e 137 int main()
tashworth 3:b7b4780a7f6e 138 {
tashworth 3:b7b4780a7f6e 139
tashworth 3:b7b4780a7f6e 140 /*****************
tashworth 3:b7b4780a7f6e 141 INITIALIZATIONS
tashworth 3:b7b4780a7f6e 142 *******************/
tashworth 3:b7b4780a7f6e 143 pc.baud(115200);
tashworth 6:75259c3306dd 144 //Laser Range Finder Initialization
tashworth 3:b7b4780a7f6e 145 lrf_baudCalibration();
tashworth 3:b7b4780a7f6e 146
tashworth 6:75259c3306dd 147 //Servo initialization
tashworth 3:b7b4780a7f6e 148 initServoDriver();
tashworth 6:75259c3306dd 149 servoBegin(); //initiates servos to start position
tashworth 6:75259c3306dd 150 //ServoOutputDisable = 0;
tashworth 6:75259c3306dd 151
tashworth 6:75259c3306dd 152 /*******************
tashworth 6:75259c3306dd 153 WHILE LOOP FOR TESTING
tashworth 6:75259c3306dd 154 ********************/
tashworth 6:75259c3306dd 155 while(1) {
tashworth 6:75259c3306dd 156 //pc.scanf("%d %d", &servoNum, &servoAngle);
tashworth 6:75259c3306dd 157 //setServoPulse(servoNum, servoAngle);
tashworth 6:75259c3306dd 158
tashworth 6:75259c3306dd 159 pc.scanf("%d", &posNum);
tashworth 6:75259c3306dd 160 servoPosition(posNum);
tashworth 5:429e9a998bab 161 }
tashworth 3:b7b4780a7f6e 162
tashworth 7:8fb4204f9600 163 /********************************
tashworth 7:8fb4204f9600 164 MAIN WHILE LOOP FOR COMPETITION
tashworth 7:8fb4204f9600 165 *********************************/
tashworth 3:b7b4780a7f6e 166 while(1) {
tashworth 6:75259c3306dd 167 switch (state) {
tashworth 7:8fb4204f9600 168
tashworth 7:8fb4204f9600 169 /**************************************************
tashworth 7:8fb4204f9600 170 * STAGE 0
tashworth 7:8fb4204f9600 171 *
tashworth 7:8fb4204f9600 172 * - START OF THE COMETITION
tashworth 7:8fb4204f9600 173 *
tashworth 7:8fb4204f9600 174 **************************************************/
tashworth 6:75259c3306dd 175 case START :
tashworth 6:75259c3306dd 176 myled1 = 1;
tashworth 6:75259c3306dd 177 wait(5);
tashworth 6:75259c3306dd 178 myled1 = 0;
tashworth 6:75259c3306dd 179 state = OILRIG1_POS;
tashworth 3:b7b4780a7f6e 180 break;
tashworth 3:b7b4780a7f6e 181
tashworth 3:b7b4780a7f6e 182
tashworth 6:75259c3306dd 183 /**************************************************
tashworth 6:75259c3306dd 184 * STAGE 1
tashworth 6:75259c3306dd 185 *
tashworth 6:75259c3306dd 186 * - DETERMINE OIL RIG ON FIRE
tashworth 6:75259c3306dd 187 *
tashworth 6:75259c3306dd 188 **************************************************/
tashworth 6:75259c3306dd 189 case OILRIG1_POS: //aims arm at square oil rig
tashworth 6:75259c3306dd 190 servoPosition(OIL_RIG1); //position arm to point at first oilrig
tashworth 6:75259c3306dd 191 wait(1); //wait for servos to settle
tashworth 6:75259c3306dd 192 fire = fire_checker(OIL_RIG1); //determines if oil rig is on fire
tashworth 6:75259c3306dd 193
tashworth 6:75259c3306dd 194 //determines what tool is needed
tashworth 6:75259c3306dd 195 if (fire == 1) {
tashworth 6:75259c3306dd 196 tool_needed = SQUARE;
tashworth 6:75259c3306dd 197 state = GOTO_TOOLS;
tashworth 6:75259c3306dd 198 } else {
tashworth 6:75259c3306dd 199 state = OILRIG2_POS;
tashworth 6:75259c3306dd 200 }
tashworth 6:75259c3306dd 201 break;
tashworth 6:75259c3306dd 202
tashworth 6:75259c3306dd 203 case OILRIG2_POS:
tashworth 6:75259c3306dd 204 servoPosition(OIL_RIG2); //position arm to point at first oilrig
tashworth 6:75259c3306dd 205 wait(1); //wait for servos to settle
tashworth 6:75259c3306dd 206 fire = fire_checker(OIL_RIG2);
tashworth 6:75259c3306dd 207 if (fire == 1) {
tashworth 6:75259c3306dd 208 tool_needed = TRIANGLE;
tashworth 6:75259c3306dd 209 state = GOTO_TOOLS;
tashworth 6:75259c3306dd 210 } else {
tashworth 6:75259c3306dd 211 tool_needed = CIRCLE;
tashworth 6:75259c3306dd 212 state = GOTO_TOOLS;
tashworth 6:75259c3306dd 213 }
tashworth 6:75259c3306dd 214 break;
tashworth 6:75259c3306dd 215
tashworth 6:75259c3306dd 216 /**************************************************
tashworth 6:75259c3306dd 217 * STAGE 2
tashworth 6:75259c3306dd 218 *
tashworth 6:75259c3306dd 219 * - TRAVEL TO TOOLS
tashworth 6:75259c3306dd 220 *
tashworth 6:75259c3306dd 221 **************************************************/
tashworth 6:75259c3306dd 222 case GOTO_TOOLS:
tashworth 6:75259c3306dd 223 servoPosition(DRIVE_POSITION_NOTOOL); //drive position without a tool
tashworth 6:75259c3306dd 224 wait(1); //wait for servos to settle
tashworth 6:75259c3306dd 225
tashworth 6:75259c3306dd 226 //*********************//
tashworth 6:75259c3306dd 227 //******* TODO ********//
tashworth 6:75259c3306dd 228 //*********************//
tashworth 6:75259c3306dd 229 //CODE TO NAVIGATE TO TOOLS
tashworth 6:75259c3306dd 230
tashworth 6:75259c3306dd 231 state = IDENTIFY_TOOLS;
tashworth 6:75259c3306dd 232 break;
tashworth 3:b7b4780a7f6e 233
tashworth 6:75259c3306dd 234 /**************************************************
tashworth 6:75259c3306dd 235 * STAGE 3
tashworth 6:75259c3306dd 236 *
tashworth 6:75259c3306dd 237 * - Determine order of tools
tashworth 6:75259c3306dd 238 * - Aquire appropriate tool
tashworth 6:75259c3306dd 239 *
tashworth 6:75259c3306dd 240 **************************************************/
tashworth 6:75259c3306dd 241 case IDENTIFY_TOOLS:
tashworth 6:75259c3306dd 242 servoPosition(TOOL_1); //arm/camera looks over tool
tashworth 6:75259c3306dd 243 wait(1); //wait for servos to settle
tashworth 6:75259c3306dd 244 centerCamWithTool(); //centers camera over tool
tashworth 6:75259c3306dd 245 shape_detected = shapeDetection(); //determines the shape
tashworth 3:b7b4780a7f6e 246
tashworth 6:75259c3306dd 247 //either goes to aquire the tool or look at the next shape
tashworth 6:75259c3306dd 248 if(shape_detected == tool_needed) {
tashworth 6:75259c3306dd 249 state = AQUIRE_TOOL1;
tashworth 6:75259c3306dd 250 } else {
tashworth 6:75259c3306dd 251 servoPosition(TOOL_2);
tashworth 6:75259c3306dd 252 wait(1); //wait for servos to settle
tashworth 6:75259c3306dd 253 centerCamWithTool();
tashworth 6:75259c3306dd 254 shape_detected = shapeDetection();
tashworth 6:75259c3306dd 255 if (shape_detected == tool_needed) {
tashworth 6:75259c3306dd 256 state = AQUIRE_TOOL2;
tashworth 6:75259c3306dd 257 } else {
tashworth 6:75259c3306dd 258 servoPosition(TOOL_3);
tashworth 6:75259c3306dd 259 wait(1); //wait for servos to settle
tashworth 6:75259c3306dd 260 centerCamWithTool();
tashworth 6:75259c3306dd 261 state = AQUIRE_TOOL3;
tashworth 6:75259c3306dd 262 }
tashworth 6:75259c3306dd 263 }
tashworth 6:75259c3306dd 264 break;
tashworth 6:75259c3306dd 265 case AQUIRE_TOOL1:
tashworth 6:75259c3306dd 266 //*********************//
tashworth 6:75259c3306dd 267 //******* TODO ********//
tashworth 6:75259c3306dd 268 //*********************//
tashworth 6:75259c3306dd 269 // CODE TO GRAB TOOL1
tashworth 6:75259c3306dd 270 servoPosition(DRIVE_POSITION_TOOL); //arm position for driving with the tool
tashworth 6:75259c3306dd 271 state = NAVIGATE_WAVES_ROW1;
tashworth 6:75259c3306dd 272 break;
tashworth 6:75259c3306dd 273 case AQUIRE_TOOL2:
tashworth 6:75259c3306dd 274 //*********************//
tashworth 6:75259c3306dd 275 //******* TODO ********//
tashworth 6:75259c3306dd 276 //*********************//
tashworth 6:75259c3306dd 277 // CODE TO GRAB TOOL2
tashworth 6:75259c3306dd 278 servoPosition(DRIVE_POSITION_TOOL); //arm position for driving with the tool
tashworth 6:75259c3306dd 279 state = NAVIGATE_WAVES_ROW1;
tashworth 6:75259c3306dd 280 break;
tashworth 6:75259c3306dd 281 case AQUIRE_TOOL3:
tashworth 6:75259c3306dd 282 //*********************//
tashworth 6:75259c3306dd 283 //******* TODO ********//
tashworth 6:75259c3306dd 284 //*********************//
tashworth 6:75259c3306dd 285 // CODE TO GRAB TOOL3
tashworth 6:75259c3306dd 286 servoPosition(DRIVE_POSITION_TOOL); //arm position for driving with the tool
tashworth 6:75259c3306dd 287 state = NAVIGATE_WAVES_ROW1;
tashworth 6:75259c3306dd 288 break;
tashworth 3:b7b4780a7f6e 289
tashworth 0:1b64a0cedc5d 290
tashworth 6:75259c3306dd 291 /**************************************************
tashworth 6:75259c3306dd 292 * STAGE 4
tashworth 6:75259c3306dd 293 *
tashworth 6:75259c3306dd 294 * - Navigate through the ocean
tashworth 6:75259c3306dd 295 *
tashworth 6:75259c3306dd 296 **************************************************/
tashworth 6:75259c3306dd 297
tashworth 6:75259c3306dd 298 case NAVIGATE_WAVES_ROW1:
tashworth 6:75259c3306dd 299 //*********************//
tashworth 6:75259c3306dd 300 //******* TODO ********//
tashworth 6:75259c3306dd 301 //*********************//
tashworth 6:75259c3306dd 302 // CODE TO NAVIGATE ROW1
tashworth 6:75259c3306dd 303 state = NAVIGATE_WAVES_ROW2;
tashworth 6:75259c3306dd 304 break;
tashworth 6:75259c3306dd 305
tashworth 6:75259c3306dd 306 case NAVIGATE_WAVES_ROW2:
tashworth 6:75259c3306dd 307 //*********************//
tashworth 6:75259c3306dd 308 //******* TODO ********//
tashworth 6:75259c3306dd 309 //*********************//
tashworth 6:75259c3306dd 310 // CODE TO NAVIGATE ROW2
tashworth 6:75259c3306dd 311 state = NAVIGATE_WAVES_ROW3;
tashworth 6:75259c3306dd 312 break;
tashworth 3:b7b4780a7f6e 313
tashworth 6:75259c3306dd 314 case NAVIGATE_WAVES_ROW3:
tashworth 6:75259c3306dd 315 //*********************//
tashworth 6:75259c3306dd 316 //******* TODO ********//
tashworth 6:75259c3306dd 317 //*********************//
tashworth 6:75259c3306dd 318 // CODE TO NAVIGATE ROW3
tashworth 7:8fb4204f9600 319
tashworth 6:75259c3306dd 320 //goes to appropriate rig
tashworth 6:75259c3306dd 321 if(shape_detected == 1) {
tashworth 6:75259c3306dd 322 state = NAVIGATE_TO_SQUARE_RIG;
tashworth 6:75259c3306dd 323 } else if(shape_detected == 2) {
tashworth 6:75259c3306dd 324 state = NAVIGATE_TO_TRIANGLE_RIG;
tashworth 6:75259c3306dd 325 } else {
tashworth 6:75259c3306dd 326 state = NAVIGATE_TO_CIRCLE_RIG;
tashworth 6:75259c3306dd 327 }
tashworth 6:75259c3306dd 328 break;
tashworth 6:75259c3306dd 329
tashworth 6:75259c3306dd 330 /**************************************************
tashworth 6:75259c3306dd 331 * STAGE 5
tashworth 6:75259c3306dd 332 *
tashworth 6:75259c3306dd 333 * - Travel to appropriate rig
tashworth 6:75259c3306dd 334 *
tashworth 6:75259c3306dd 335 **************************************************/
tashworth 6:75259c3306dd 336 case NAVIGATE_TO_SQUARE_RIG:
tashworth 6:75259c3306dd 337 //NAVIGATION CODE HERE
tashworth 6:75259c3306dd 338 state = RIG_ALIGN;
tashworth 6:75259c3306dd 339 break;
tashworth 6:75259c3306dd 340 case NAVIGATE_TO_TRIANGLE_RIG:
tashworth 6:75259c3306dd 341 //NAVIGATION CODE HERE
tashworth 6:75259c3306dd 342 state = RIG_ALIGN;
tashworth 6:75259c3306dd 343 break;
tashworth 6:75259c3306dd 344 case NAVIGATE_TO_CIRCLE_RIG:
tashworth 6:75259c3306dd 345 //NAVIGATION CODE HERE
tashworth 6:75259c3306dd 346 state = RIG_ALIGN;
tashworth 6:75259c3306dd 347 break;
tashworth 3:b7b4780a7f6e 348
tashworth 6:75259c3306dd 349 /**************************************************
tashworth 6:75259c3306dd 350 * STAGE 6
tashworth 6:75259c3306dd 351 *
tashworth 6:75259c3306dd 352 * - Align with appropriate rig
tashworth 6:75259c3306dd 353 *
tashworth 6:75259c3306dd 354 **************************************************/
tashworth 6:75259c3306dd 355 case RIG_ALIGN:
tashworth 7:8fb4204f9600 356
tashworth 6:75259c3306dd 357 //*********************//
tashworth 6:75259c3306dd 358 //******* TODO ********//
tashworth 6:75259c3306dd 359 //*********************//
tashworth 6:75259c3306dd 360 // CODE TO ALIGN ROBOT WITH RIG
tashworth 7:8fb4204f9600 361
tashworth 6:75259c3306dd 362 servoPosition(ORIENT_TOOL);
tashworth 6:75259c3306dd 363 wait(1); //wait for servos to settle
tashworth 6:75259c3306dd 364 state = INSERT_TOOL;
tashworth 6:75259c3306dd 365 break;
tashworth 3:b7b4780a7f6e 366
tashworth 6:75259c3306dd 367 /**************************************************
tashworth 6:75259c3306dd 368 * STAGE 7
tashworth 6:75259c3306dd 369 *
tashworth 6:75259c3306dd 370 * - Insert Tool
tashworth 6:75259c3306dd 371 * - Extenguish fire
tashworth 6:75259c3306dd 372 * - win contest
tashworth 6:75259c3306dd 373 *
tashworth 6:75259c3306dd 374 **************************************************/
tashworth 7:8fb4204f9600 375
tashworth 6:75259c3306dd 376 case INSERT_TOOL:
tashworth 6:75259c3306dd 377 //*********************//
tashworth 6:75259c3306dd 378 //******* TODO ********//
tashworth 6:75259c3306dd 379 //*********************//
tashworth 6:75259c3306dd 380 // CODE TO INSERT TOOL
tashworth 6:75259c3306dd 381 break;
tashworth 3:b7b4780a7f6e 382
tashworth 6:75259c3306dd 383 /**************************************************
tashworth 6:75259c3306dd 384 * STAGE 8
tashworth 6:75259c3306dd 385 *
tashworth 6:75259c3306dd 386 * - END COMPETITION
tashworth 6:75259c3306dd 387 *
tashworth 6:75259c3306dd 388 **************************************************/
tashworth 6:75259c3306dd 389 case END:
tashworth 6:75259c3306dd 390 servoPosition(STORE_POSITION);
tashworth 6:75259c3306dd 391 myled1 = 1;
tashworth 6:75259c3306dd 392 wait(.2);
tashworth 6:75259c3306dd 393 myled2 = 1;
tashworth 6:75259c3306dd 394 wait(.2);
tashworth 6:75259c3306dd 395 myled3 = 1;
tashworth 6:75259c3306dd 396 wait(.2);
tashworth 6:75259c3306dd 397 myled4 = 1;
tashworth 6:75259c3306dd 398 wait(.2);
tashworth 6:75259c3306dd 399 break;
tashworth 6:75259c3306dd 400 default:
tashworth 3:b7b4780a7f6e 401
tashworth 6:75259c3306dd 402 break;
tashworth 6:75259c3306dd 403 }
tashworth 6:75259c3306dd 404 }
tashworth 6:75259c3306dd 405
tashworth 0:1b64a0cedc5d 406
tashworth 0:1b64a0cedc5d 407 }
tashworth 0:1b64a0cedc5d 408
tashworth 0:1b64a0cedc5d 409
tashworth 0:1b64a0cedc5d 410
tashworth 0:1b64a0cedc5d 411 /************
tashworth 0:1b64a0cedc5d 412
tashworth 0:1b64a0cedc5d 413 Servo Functions
tashworth 0:1b64a0cedc5d 414
tashworth 0:1b64a0cedc5d 415 **************/
tashworth 0:1b64a0cedc5d 416
tashworth 3:b7b4780a7f6e 417 void setServoPulse(uint8_t n, int angle)
tashworth 3:b7b4780a7f6e 418 {
tashworth 3:b7b4780a7f6e 419 int pulse;
tashworth 3:b7b4780a7f6e 420 pulse = MIN_SERVO_PULSE + (( angle * (MAX_SERVO_PULSE - MIN_SERVO_PULSE)) / SERVO_MAX_ANGLE);
tashworth 1:fe4a0b47ff25 421 float pulselength = 20000; // 20,000 us per second
tashworth 1:fe4a0b47ff25 422 int i = currentPosition[n];
tashworth 3:b7b4780a7f6e 423 pc.printf("ServoNumber: %d Begining Pulse: %d\n\r",n,currentPosition[n]);
tashworth 3:b7b4780a7f6e 424 int pulse2;
tashworth 3:b7b4780a7f6e 425 if(currentPosition[n] < pulse) {
tashworth 3:b7b4780a7f6e 426 for(i; i < pulse; i++) {
tashworth 3:b7b4780a7f6e 427 pulse2 = 4094 * i / pulselength;
tashworth 3:b7b4780a7f6e 428 pwm.setPWM(n, 0, pulse2);
tashworth 3:b7b4780a7f6e 429 wait_ms(3);
tashworth 3:b7b4780a7f6e 430 }
tashworth 1:fe4a0b47ff25 431 } else if (currentPosition[n] > pulse) {
tashworth 3:b7b4780a7f6e 432 for(i; i > pulse; i--) {
tashworth 3:b7b4780a7f6e 433 pulse2 = 4094 * i / pulselength;
tashworth 3:b7b4780a7f6e 434 pwm.setPWM(n, 0, pulse2);
tashworth 3:b7b4780a7f6e 435 wait_ms(3);
tashworth 3:b7b4780a7f6e 436 }
tashworth 1:fe4a0b47ff25 437 }
tashworth 1:fe4a0b47ff25 438 currentPosition[n] = i;
tashworth 3:b7b4780a7f6e 439 pc.printf("\nEND: pulse: %d, angle: %d\n\n", i, angle);
tashworth 0:1b64a0cedc5d 440 }
tashworth 0:1b64a0cedc5d 441
tashworth 3:b7b4780a7f6e 442 void initServoDriver(void)
tashworth 3:b7b4780a7f6e 443 {
tashworth 0:1b64a0cedc5d 444 pwm.begin();
tashworth 0:1b64a0cedc5d 445 //pwm.setPWMFreq(100); //This dosen't work well because of uncertain clock speed. Use setPrescale().
tashworth 0:1b64a0cedc5d 446 pwm.setPrescale(140); //This value is decided for 20ms interval.
tashworth 0:1b64a0cedc5d 447 pwm.setI2Cfreq(400000); //400kHz
tashworth 3:b7b4780a7f6e 448
tashworth 3:b7b4780a7f6e 449 }
tashworth 3:b7b4780a7f6e 450
tashworth 3:b7b4780a7f6e 451 void servoBegin(void)
tashworth 3:b7b4780a7f6e 452 {
tashworth 3:b7b4780a7f6e 453 pc.printf("Setting Initial Position\n\r");
tashworth 3:b7b4780a7f6e 454 setServoPulseNo_delay(0, 90);
tashworth 5:429e9a998bab 455 setServoPulseNo_delay(1, 0);
tashworth 5:429e9a998bab 456 setServoPulseNo_delay(2, 177);
tashworth 5:429e9a998bab 457 setServoPulseNo_delay(3, 0);
tashworth 5:429e9a998bab 458 setServoPulseNo_delay(4, 0);
tashworth 3:b7b4780a7f6e 459 setServoPulseNo_delay(5, 90);
tashworth 3:b7b4780a7f6e 460 setGripper(1);
tashworth 0:1b64a0cedc5d 461 }
tashworth 0:1b64a0cedc5d 462
tashworth 3:b7b4780a7f6e 463 void setServoPulseNo_delay(uint8_t n, int angle)
tashworth 3:b7b4780a7f6e 464 {
tashworth 3:b7b4780a7f6e 465 int pulse = MIN_SERVO_PULSE + (( angle * (MAX_SERVO_PULSE - MIN_SERVO_PULSE)) / SERVO_MAX_ANGLE);
tashworth 1:fe4a0b47ff25 466 float pulselength = 20000; // 20,000 us per second
tashworth 1:fe4a0b47ff25 467 currentPosition[n] = pulse;
tashworth 3:b7b4780a7f6e 468 pc.printf("ServoNumber: %d Pulsewidth: %d Angle: %d \n\r",n,pulse, angle);
tashworth 1:fe4a0b47ff25 469 pulse = 4094 * pulse / pulselength;
tashworth 1:fe4a0b47ff25 470 pwm.setPWM(n, 0, pulse);
tashworth 3:b7b4780a7f6e 471
tashworth 1:fe4a0b47ff25 472 }
tashworth 3:b7b4780a7f6e 473
tashworth 3:b7b4780a7f6e 474 void setGripper(int open)
tashworth 3:b7b4780a7f6e 475 {
tashworth 3:b7b4780a7f6e 476 if (open) {
tashworth 3:b7b4780a7f6e 477 pc.printf("Gripper Open\r");
tashworth 3:b7b4780a7f6e 478 setServoPulseNo_delay(6, 10);
tashworth 3:b7b4780a7f6e 479 } else {
tashworth 3:b7b4780a7f6e 480 pc.printf("Gripper Closed\n\r");
tashworth 3:b7b4780a7f6e 481 setServoPulseNo_delay(6, 170);
tashworth 3:b7b4780a7f6e 482 }
tashworth 3:b7b4780a7f6e 483 }
tashworth 3:b7b4780a7f6e 484
tashworth 3:b7b4780a7f6e 485 void servoPosition(int set)
tashworth 3:b7b4780a7f6e 486 {
tashworth 3:b7b4780a7f6e 487 //moves to current position
tashworth 3:b7b4780a7f6e 488 setServoPulse(0, Arm_Table[set].base_rotate);
tashworth 3:b7b4780a7f6e 489 setServoPulse(1, Arm_Table[set].base_arm);
tashworth 6:75259c3306dd 490 setServoPulse(2, Arm_Table[set].big_arm);
tashworth 6:75259c3306dd 491 setServoPulse(3, Arm_Table[set].claw_arm);
tashworth 6:75259c3306dd 492 setServoPulse(4, Arm_Table[set].claw_rotate);
tashworth 6:75259c3306dd 493 setServoPulse(5, Arm_Table[set].claw_open);
tashworth 6:75259c3306dd 494 }
tashworth 6:75259c3306dd 495
tashworth 6:75259c3306dd 496 int fire_checker(int rig)
tashworth 6:75259c3306dd 497 {
tashworth 6:75259c3306dd 498 switch (rig) {
tashworth 6:75259c3306dd 499
tashworth 6:75259c3306dd 500 case 1:
tashworth 6:75259c3306dd 501 for (int i = 0; i<5; i++) {
tashworth 6:75259c3306dd 502 dist = getDistance();
tashworth 6:75259c3306dd 503 if ((dist < OILRIG1_MAX)
tashworth 6:75259c3306dd 504 || (dist > OILRIG1_MIN)) {
tashworth 6:75259c3306dd 505 fire_detected++;
tashworth 6:75259c3306dd 506 } else {
tashworth 6:75259c3306dd 507 fire_not_detected++;
tashworth 6:75259c3306dd 508 }
tashworth 6:75259c3306dd 509 }
tashworth 6:75259c3306dd 510 case 2:
tashworth 6:75259c3306dd 511 for (int i = 0; i<5; i++) {
tashworth 6:75259c3306dd 512 dist = getDistance();
tashworth 6:75259c3306dd 513 if ((dist < OILRIG2_MAX)
tashworth 6:75259c3306dd 514 || (dist > OILRIG2_MIN)) {
tashworth 6:75259c3306dd 515 fire_detected++;
tashworth 6:75259c3306dd 516 } else {
tashworth 6:75259c3306dd 517 fire_not_detected++;
tashworth 6:75259c3306dd 518 }
tashworth 6:75259c3306dd 519 }
tashworth 6:75259c3306dd 520 case 3:
tashworth 6:75259c3306dd 521 for (int i = 0; i<5; i++) {
tashworth 6:75259c3306dd 522 dist = getDistance();
tashworth 6:75259c3306dd 523 if ((dist < OILRIG3_MAX)
tashworth 6:75259c3306dd 524 || (dist > OILRIG3_MIN)) {
tashworth 6:75259c3306dd 525 fire_detected++;
tashworth 6:75259c3306dd 526 } else {
tashworth 6:75259c3306dd 527 fire_not_detected++;
tashworth 6:75259c3306dd 528 }
tashworth 6:75259c3306dd 529 }
tashworth 6:75259c3306dd 530 default:
tashworth 6:75259c3306dd 531 for (int i = 0; i<5; i++) {
tashworth 6:75259c3306dd 532 dist = getDistance();
tashworth 6:75259c3306dd 533 if ((dist < OILRIG1_MAX)
tashworth 6:75259c3306dd 534 || (dist > OILRIG1_MIN)) {
tashworth 6:75259c3306dd 535 fire_detected++;
tashworth 6:75259c3306dd 536 } else {
tashworth 6:75259c3306dd 537 fire_not_detected++;
tashworth 6:75259c3306dd 538 }
tashworth 6:75259c3306dd 539 }
tashworth 6:75259c3306dd 540 }
tashworth 6:75259c3306dd 541
tashworth 6:75259c3306dd 542 if (fire_detected > fire_not_detected) {
tashworth 6:75259c3306dd 543 return 1;
tashworth 6:75259c3306dd 544 } else {
tashworth 6:75259c3306dd 545 return 0;
tashworth 6:75259c3306dd 546 }
tashworth 0:1b64a0cedc5d 547 }
tashworth 0:1b64a0cedc5d 548
tashworth 0:1b64a0cedc5d 549
tashworth 0:1b64a0cedc5d 550