Paolina Povolotskaya / theRobotNEW

ksdflsjdfljas

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

Fork of theRobot by Thomas Ashworth

main.cpp@6:75259c3306dd, 2014-03-19 (annotated)

Committer:
tashworth
Date:
Wed Mar 19 17:05:35 2014 +0000
Revision:
6:75259c3306dd
Parent:
5:429e9a998bab
Child:
7:8fb4204f9600
3-19-14

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 5:429e9a998bab 163
tashworth 3:b7b4780a7f6e 164 while(1) {
tashworth 6:75259c3306dd 165 switch (state) {
tashworth 3:b7b4780a7f6e 166
tashworth 6:75259c3306dd 167 case START :
tashworth 6:75259c3306dd 168 myled1 = 1;
tashworth 6:75259c3306dd 169 wait(5);
tashworth 6:75259c3306dd 170 myled1 = 0;
tashworth 6:75259c3306dd 171 state = OILRIG1_POS;
tashworth 3:b7b4780a7f6e 172 break;
tashworth 3:b7b4780a7f6e 173
tashworth 3:b7b4780a7f6e 174
tashworth 6:75259c3306dd 175 /**************************************************
tashworth 6:75259c3306dd 176 * STAGE 1
tashworth 6:75259c3306dd 177 *
tashworth 6:75259c3306dd 178 * - DETERMINE OIL RIG ON FIRE
tashworth 6:75259c3306dd 179 *
tashworth 6:75259c3306dd 180 **************************************************/
tashworth 6:75259c3306dd 181 case OILRIG1_POS: //aims arm at square oil rig
tashworth 6:75259c3306dd 182 servoPosition(OIL_RIG1); //position arm to point at first oilrig
tashworth 6:75259c3306dd 183 wait(1); //wait for servos to settle
tashworth 6:75259c3306dd 184 fire = fire_checker(OIL_RIG1); //determines if oil rig is on fire
tashworth 6:75259c3306dd 185
tashworth 6:75259c3306dd 186 //determines what tool is needed
tashworth 6:75259c3306dd 187 if (fire == 1) {
tashworth 6:75259c3306dd 188 tool_needed = SQUARE;
tashworth 6:75259c3306dd 189 state = GOTO_TOOLS;
tashworth 6:75259c3306dd 190 } else {
tashworth 6:75259c3306dd 191 state = OILRIG2_POS;
tashworth 6:75259c3306dd 192 }
tashworth 6:75259c3306dd 193 break;
tashworth 6:75259c3306dd 194
tashworth 6:75259c3306dd 195 case OILRIG2_POS:
tashworth 6:75259c3306dd 196 servoPosition(OIL_RIG2); //position arm to point at first oilrig
tashworth 6:75259c3306dd 197 wait(1); //wait for servos to settle
tashworth 6:75259c3306dd 198 fire = fire_checker(OIL_RIG2);
tashworth 6:75259c3306dd 199 if (fire == 1) {
tashworth 6:75259c3306dd 200 tool_needed = TRIANGLE;
tashworth 6:75259c3306dd 201 state = GOTO_TOOLS;
tashworth 6:75259c3306dd 202 } else {
tashworth 6:75259c3306dd 203 tool_needed = CIRCLE;
tashworth 6:75259c3306dd 204 state = GOTO_TOOLS;
tashworth 6:75259c3306dd 205 }
tashworth 6:75259c3306dd 206 break;
tashworth 6:75259c3306dd 207
tashworth 6:75259c3306dd 208 /**************************************************
tashworth 6:75259c3306dd 209 * STAGE 2
tashworth 6:75259c3306dd 210 *
tashworth 6:75259c3306dd 211 * - TRAVEL TO TOOLS
tashworth 6:75259c3306dd 212 *
tashworth 6:75259c3306dd 213 **************************************************/
tashworth 6:75259c3306dd 214 case GOTO_TOOLS:
tashworth 6:75259c3306dd 215 servoPosition(DRIVE_POSITION_NOTOOL); //drive position without a tool
tashworth 6:75259c3306dd 216 wait(1); //wait for servos to settle
tashworth 6:75259c3306dd 217
tashworth 6:75259c3306dd 218 //*********************//
tashworth 6:75259c3306dd 219 //******* TODO ********//
tashworth 6:75259c3306dd 220 //*********************//
tashworth 6:75259c3306dd 221 //CODE TO NAVIGATE TO TOOLS
tashworth 6:75259c3306dd 222
tashworth 6:75259c3306dd 223 state = IDENTIFY_TOOLS;
tashworth 6:75259c3306dd 224 break;
tashworth 3:b7b4780a7f6e 225
tashworth 6:75259c3306dd 226 /**************************************************
tashworth 6:75259c3306dd 227 * STAGE 3
tashworth 6:75259c3306dd 228 *
tashworth 6:75259c3306dd 229 * - Determine order of tools
tashworth 6:75259c3306dd 230 * - Aquire appropriate tool
tashworth 6:75259c3306dd 231 *
tashworth 6:75259c3306dd 232 **************************************************/
tashworth 6:75259c3306dd 233 case IDENTIFY_TOOLS:
tashworth 6:75259c3306dd 234 servoPosition(TOOL_1); //arm/camera looks over tool
tashworth 6:75259c3306dd 235 wait(1); //wait for servos to settle
tashworth 6:75259c3306dd 236 centerCamWithTool(); //centers camera over tool
tashworth 6:75259c3306dd 237 shape_detected = shapeDetection(); //determines the shape
tashworth 3:b7b4780a7f6e 238
tashworth 6:75259c3306dd 239 //either goes to aquire the tool or look at the next shape
tashworth 6:75259c3306dd 240 if(shape_detected == tool_needed) {
tashworth 6:75259c3306dd 241 state = AQUIRE_TOOL1;
tashworth 6:75259c3306dd 242 } else {
tashworth 6:75259c3306dd 243 servoPosition(TOOL_2);
tashworth 6:75259c3306dd 244 wait(1); //wait for servos to settle
tashworth 6:75259c3306dd 245 centerCamWithTool();
tashworth 6:75259c3306dd 246 shape_detected = shapeDetection();
tashworth 6:75259c3306dd 247 if (shape_detected == tool_needed) {
tashworth 6:75259c3306dd 248 state = AQUIRE_TOOL2;
tashworth 6:75259c3306dd 249 } else {
tashworth 6:75259c3306dd 250 servoPosition(TOOL_3);
tashworth 6:75259c3306dd 251 wait(1); //wait for servos to settle
tashworth 6:75259c3306dd 252 centerCamWithTool();
tashworth 6:75259c3306dd 253 state = AQUIRE_TOOL3;
tashworth 6:75259c3306dd 254 }
tashworth 6:75259c3306dd 255 }
tashworth 6:75259c3306dd 256 break;
tashworth 6:75259c3306dd 257 case AQUIRE_TOOL1:
tashworth 6:75259c3306dd 258 //*********************//
tashworth 6:75259c3306dd 259 //******* TODO ********//
tashworth 6:75259c3306dd 260 //*********************//
tashworth 6:75259c3306dd 261 // CODE TO GRAB TOOL1
tashworth 6:75259c3306dd 262 servoPosition(DRIVE_POSITION_TOOL); //arm position for driving with the tool
tashworth 6:75259c3306dd 263 state = NAVIGATE_WAVES_ROW1;
tashworth 6:75259c3306dd 264 break;
tashworth 6:75259c3306dd 265 case AQUIRE_TOOL2:
tashworth 6:75259c3306dd 266 //*********************//
tashworth 6:75259c3306dd 267 //******* TODO ********//
tashworth 6:75259c3306dd 268 //*********************//
tashworth 6:75259c3306dd 269 // CODE TO GRAB TOOL2
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_TOOL3:
tashworth 6:75259c3306dd 274 //*********************//
tashworth 6:75259c3306dd 275 //******* TODO ********//
tashworth 6:75259c3306dd 276 //*********************//
tashworth 6:75259c3306dd 277 // CODE TO GRAB TOOL3
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 3:b7b4780a7f6e 281
tashworth 0:1b64a0cedc5d 282
tashworth 6:75259c3306dd 283 /**************************************************
tashworth 6:75259c3306dd 284 * STAGE 4
tashworth 6:75259c3306dd 285 *
tashworth 6:75259c3306dd 286 * - Navigate through the ocean
tashworth 6:75259c3306dd 287 *
tashworth 6:75259c3306dd 288 **************************************************/
tashworth 6:75259c3306dd 289
tashworth 6:75259c3306dd 290 case NAVIGATE_WAVES_ROW1:
tashworth 6:75259c3306dd 291 //*********************//
tashworth 6:75259c3306dd 292 //******* TODO ********//
tashworth 6:75259c3306dd 293 //*********************//
tashworth 6:75259c3306dd 294 // CODE TO NAVIGATE ROW1
tashworth 6:75259c3306dd 295 state = NAVIGATE_WAVES_ROW2;
tashworth 6:75259c3306dd 296 break;
tashworth 6:75259c3306dd 297
tashworth 6:75259c3306dd 298 case NAVIGATE_WAVES_ROW2:
tashworth 6:75259c3306dd 299 //*********************//
tashworth 6:75259c3306dd 300 //******* TODO ********//
tashworth 6:75259c3306dd 301 //*********************//
tashworth 6:75259c3306dd 302 // CODE TO NAVIGATE ROW2
tashworth 6:75259c3306dd 303 state = NAVIGATE_WAVES_ROW3;
tashworth 6:75259c3306dd 304 break;
tashworth 3:b7b4780a7f6e 305
tashworth 6:75259c3306dd 306 case NAVIGATE_WAVES_ROW3:
tashworth 6:75259c3306dd 307 //*********************//
tashworth 6:75259c3306dd 308 //******* TODO ********//
tashworth 6:75259c3306dd 309 //*********************//
tashworth 6:75259c3306dd 310 // CODE TO NAVIGATE ROW3
tashworth 6:75259c3306dd 311
tashworth 6:75259c3306dd 312 //goes to appropriate rig
tashworth 6:75259c3306dd 313 if(shape_detected == 1) {
tashworth 6:75259c3306dd 314 state = NAVIGATE_TO_SQUARE_RIG;
tashworth 6:75259c3306dd 315 } else if(shape_detected == 2) {
tashworth 6:75259c3306dd 316 state = NAVIGATE_TO_TRIANGLE_RIG;
tashworth 6:75259c3306dd 317 } else {
tashworth 6:75259c3306dd 318 state = NAVIGATE_TO_CIRCLE_RIG;
tashworth 6:75259c3306dd 319 }
tashworth 6:75259c3306dd 320 break;
tashworth 6:75259c3306dd 321
tashworth 6:75259c3306dd 322 /**************************************************
tashworth 6:75259c3306dd 323 * STAGE 5
tashworth 6:75259c3306dd 324 *
tashworth 6:75259c3306dd 325 * - Travel to appropriate rig
tashworth 6:75259c3306dd 326 *
tashworth 6:75259c3306dd 327 **************************************************/
tashworth 6:75259c3306dd 328 case NAVIGATE_TO_SQUARE_RIG:
tashworth 6:75259c3306dd 329 //NAVIGATION CODE HERE
tashworth 6:75259c3306dd 330 state = RIG_ALIGN;
tashworth 6:75259c3306dd 331 break;
tashworth 6:75259c3306dd 332 case NAVIGATE_TO_TRIANGLE_RIG:
tashworth 6:75259c3306dd 333 //NAVIGATION CODE HERE
tashworth 6:75259c3306dd 334 state = RIG_ALIGN;
tashworth 6:75259c3306dd 335 break;
tashworth 6:75259c3306dd 336 case NAVIGATE_TO_CIRCLE_RIG:
tashworth 6:75259c3306dd 337 //NAVIGATION CODE HERE
tashworth 6:75259c3306dd 338 state = RIG_ALIGN;
tashworth 6:75259c3306dd 339 break;
tashworth 3:b7b4780a7f6e 340
tashworth 6:75259c3306dd 341 /**************************************************
tashworth 6:75259c3306dd 342 * STAGE 6
tashworth 6:75259c3306dd 343 *
tashworth 6:75259c3306dd 344 * - Align with appropriate rig
tashworth 6:75259c3306dd 345 *
tashworth 6:75259c3306dd 346 **************************************************/
tashworth 6:75259c3306dd 347 case RIG_ALIGN:
tashworth 6:75259c3306dd 348
tashworth 6:75259c3306dd 349 //*********************//
tashworth 6:75259c3306dd 350 //******* TODO ********//
tashworth 6:75259c3306dd 351 //*********************//
tashworth 6:75259c3306dd 352 // CODE TO ALIGN ROBOT WITH RIG
tashworth 6:75259c3306dd 353
tashworth 6:75259c3306dd 354 servoPosition(ORIENT_TOOL);
tashworth 6:75259c3306dd 355 wait(1); //wait for servos to settle
tashworth 6:75259c3306dd 356 state = INSERT_TOOL;
tashworth 6:75259c3306dd 357 break;
tashworth 3:b7b4780a7f6e 358
tashworth 6:75259c3306dd 359 /**************************************************
tashworth 6:75259c3306dd 360 * STAGE 7
tashworth 6:75259c3306dd 361 *
tashworth 6:75259c3306dd 362 * - Insert Tool
tashworth 6:75259c3306dd 363 * - Extenguish fire
tashworth 6:75259c3306dd 364 * - win contest
tashworth 6:75259c3306dd 365 *
tashworth 6:75259c3306dd 366 **************************************************/
tashworth 6:75259c3306dd 367
tashworth 6:75259c3306dd 368 case INSERT_TOOL:
tashworth 6:75259c3306dd 369 //*********************//
tashworth 6:75259c3306dd 370 //******* TODO ********//
tashworth 6:75259c3306dd 371 //*********************//
tashworth 6:75259c3306dd 372 // CODE TO INSERT TOOL
tashworth 6:75259c3306dd 373 break;
tashworth 3:b7b4780a7f6e 374
tashworth 6:75259c3306dd 375 /**************************************************
tashworth 6:75259c3306dd 376 * STAGE 8
tashworth 6:75259c3306dd 377 *
tashworth 6:75259c3306dd 378 * - END COMPETITION
tashworth 6:75259c3306dd 379 *
tashworth 6:75259c3306dd 380 **************************************************/
tashworth 6:75259c3306dd 381 case END:
tashworth 6:75259c3306dd 382 servoPosition(STORE_POSITION);
tashworth 6:75259c3306dd 383 myled1 = 1;
tashworth 6:75259c3306dd 384 wait(.2);
tashworth 6:75259c3306dd 385 myled2 = 1;
tashworth 6:75259c3306dd 386 wait(.2);
tashworth 6:75259c3306dd 387 myled3 = 1;
tashworth 6:75259c3306dd 388 wait(.2);
tashworth 6:75259c3306dd 389 myled4 = 1;
tashworth 6:75259c3306dd 390 wait(.2);
tashworth 6:75259c3306dd 391 break;
tashworth 6:75259c3306dd 392 default:
tashworth 3:b7b4780a7f6e 393
tashworth 6:75259c3306dd 394 break;
tashworth 6:75259c3306dd 395 }
tashworth 6:75259c3306dd 396 }
tashworth 6:75259c3306dd 397
tashworth 0:1b64a0cedc5d 398
tashworth 0:1b64a0cedc5d 399 }
tashworth 0:1b64a0cedc5d 400
tashworth 0:1b64a0cedc5d 401
tashworth 0:1b64a0cedc5d 402
tashworth 0:1b64a0cedc5d 403 /************
tashworth 0:1b64a0cedc5d 404
tashworth 0:1b64a0cedc5d 405 Servo Functions
tashworth 0:1b64a0cedc5d 406
tashworth 0:1b64a0cedc5d 407 **************/
tashworth 0:1b64a0cedc5d 408
tashworth 3:b7b4780a7f6e 409 void setServoPulse(uint8_t n, int angle)
tashworth 3:b7b4780a7f6e 410 {
tashworth 3:b7b4780a7f6e 411 int pulse;
tashworth 3:b7b4780a7f6e 412 pulse = MIN_SERVO_PULSE + (( angle * (MAX_SERVO_PULSE - MIN_SERVO_PULSE)) / SERVO_MAX_ANGLE);
tashworth 1:fe4a0b47ff25 413 float pulselength = 20000; // 20,000 us per second
tashworth 1:fe4a0b47ff25 414 int i = currentPosition[n];
tashworth 3:b7b4780a7f6e 415 pc.printf("ServoNumber: %d Begining Pulse: %d\n\r",n,currentPosition[n]);
tashworth 3:b7b4780a7f6e 416 int pulse2;
tashworth 3:b7b4780a7f6e 417 if(currentPosition[n] < pulse) {
tashworth 3:b7b4780a7f6e 418 for(i; i < pulse; i++) {
tashworth 3:b7b4780a7f6e 419 pulse2 = 4094 * i / pulselength;
tashworth 3:b7b4780a7f6e 420 pwm.setPWM(n, 0, pulse2);
tashworth 3:b7b4780a7f6e 421 wait_ms(3);
tashworth 3:b7b4780a7f6e 422 }
tashworth 1:fe4a0b47ff25 423 } else if (currentPosition[n] > pulse) {
tashworth 3:b7b4780a7f6e 424 for(i; i > pulse; i--) {
tashworth 3:b7b4780a7f6e 425 pulse2 = 4094 * i / pulselength;
tashworth 3:b7b4780a7f6e 426 pwm.setPWM(n, 0, pulse2);
tashworth 3:b7b4780a7f6e 427 wait_ms(3);
tashworth 3:b7b4780a7f6e 428 }
tashworth 1:fe4a0b47ff25 429 }
tashworth 1:fe4a0b47ff25 430 currentPosition[n] = i;
tashworth 3:b7b4780a7f6e 431 pc.printf("\nEND: pulse: %d, angle: %d\n\n", i, angle);
tashworth 0:1b64a0cedc5d 432 }
tashworth 0:1b64a0cedc5d 433
tashworth 3:b7b4780a7f6e 434 void initServoDriver(void)
tashworth 3:b7b4780a7f6e 435 {
tashworth 0:1b64a0cedc5d 436 pwm.begin();
tashworth 0:1b64a0cedc5d 437 //pwm.setPWMFreq(100); //This dosen't work well because of uncertain clock speed. Use setPrescale().
tashworth 0:1b64a0cedc5d 438 pwm.setPrescale(140); //This value is decided for 20ms interval.
tashworth 0:1b64a0cedc5d 439 pwm.setI2Cfreq(400000); //400kHz
tashworth 3:b7b4780a7f6e 440
tashworth 3:b7b4780a7f6e 441 }
tashworth 3:b7b4780a7f6e 442
tashworth 3:b7b4780a7f6e 443 void servoBegin(void)
tashworth 3:b7b4780a7f6e 444 {
tashworth 3:b7b4780a7f6e 445 pc.printf("Setting Initial Position\n\r");
tashworth 3:b7b4780a7f6e 446 setServoPulseNo_delay(0, 90);
tashworth 5:429e9a998bab 447 setServoPulseNo_delay(1, 0);
tashworth 5:429e9a998bab 448 setServoPulseNo_delay(2, 177);
tashworth 5:429e9a998bab 449 setServoPulseNo_delay(3, 0);
tashworth 5:429e9a998bab 450 setServoPulseNo_delay(4, 0);
tashworth 3:b7b4780a7f6e 451 setServoPulseNo_delay(5, 90);
tashworth 3:b7b4780a7f6e 452 setGripper(1);
tashworth 0:1b64a0cedc5d 453 }
tashworth 0:1b64a0cedc5d 454
tashworth 3:b7b4780a7f6e 455 void setServoPulseNo_delay(uint8_t n, int angle)
tashworth 3:b7b4780a7f6e 456 {
tashworth 3:b7b4780a7f6e 457 int pulse = MIN_SERVO_PULSE + (( angle * (MAX_SERVO_PULSE - MIN_SERVO_PULSE)) / SERVO_MAX_ANGLE);
tashworth 1:fe4a0b47ff25 458 float pulselength = 20000; // 20,000 us per second
tashworth 1:fe4a0b47ff25 459 currentPosition[n] = pulse;
tashworth 3:b7b4780a7f6e 460 pc.printf("ServoNumber: %d Pulsewidth: %d Angle: %d \n\r",n,pulse, angle);
tashworth 1:fe4a0b47ff25 461 pulse = 4094 * pulse / pulselength;
tashworth 1:fe4a0b47ff25 462 pwm.setPWM(n, 0, pulse);
tashworth 3:b7b4780a7f6e 463
tashworth 1:fe4a0b47ff25 464 }
tashworth 3:b7b4780a7f6e 465
tashworth 3:b7b4780a7f6e 466 void setGripper(int open)
tashworth 3:b7b4780a7f6e 467 {
tashworth 3:b7b4780a7f6e 468 if (open) {
tashworth 3:b7b4780a7f6e 469 pc.printf("Gripper Open\r");
tashworth 3:b7b4780a7f6e 470 setServoPulseNo_delay(6, 10);
tashworth 3:b7b4780a7f6e 471 } else {
tashworth 3:b7b4780a7f6e 472 pc.printf("Gripper Closed\n\r");
tashworth 3:b7b4780a7f6e 473 setServoPulseNo_delay(6, 170);
tashworth 3:b7b4780a7f6e 474 }
tashworth 3:b7b4780a7f6e 475 }
tashworth 3:b7b4780a7f6e 476
tashworth 3:b7b4780a7f6e 477 void servoPosition(int set)
tashworth 3:b7b4780a7f6e 478 {
tashworth 3:b7b4780a7f6e 479 //moves to current position
tashworth 3:b7b4780a7f6e 480 setServoPulse(0, Arm_Table[set].base_rotate);
tashworth 3:b7b4780a7f6e 481 setServoPulse(1, Arm_Table[set].base_arm);
tashworth 6:75259c3306dd 482 setServoPulse(2, Arm_Table[set].big_arm);
tashworth 6:75259c3306dd 483 setServoPulse(3, Arm_Table[set].claw_arm);
tashworth 6:75259c3306dd 484 setServoPulse(4, Arm_Table[set].claw_rotate);
tashworth 6:75259c3306dd 485 setServoPulse(5, Arm_Table[set].claw_open);
tashworth 6:75259c3306dd 486 }
tashworth 6:75259c3306dd 487
tashworth 6:75259c3306dd 488 int fire_checker(int rig)
tashworth 6:75259c3306dd 489 {
tashworth 6:75259c3306dd 490 switch (rig) {
tashworth 6:75259c3306dd 491
tashworth 6:75259c3306dd 492 case 1:
tashworth 6:75259c3306dd 493 for (int i = 0; i<5; i++) {
tashworth 6:75259c3306dd 494 dist = getDistance();
tashworth 6:75259c3306dd 495 if ((dist < OILRIG1_MAX)
tashworth 6:75259c3306dd 496 || (dist > OILRIG1_MIN)) {
tashworth 6:75259c3306dd 497 fire_detected++;
tashworth 6:75259c3306dd 498 } else {
tashworth 6:75259c3306dd 499 fire_not_detected++;
tashworth 6:75259c3306dd 500 }
tashworth 6:75259c3306dd 501 }
tashworth 6:75259c3306dd 502 case 2:
tashworth 6:75259c3306dd 503 for (int i = 0; i<5; i++) {
tashworth 6:75259c3306dd 504 dist = getDistance();
tashworth 6:75259c3306dd 505 if ((dist < OILRIG2_MAX)
tashworth 6:75259c3306dd 506 || (dist > OILRIG2_MIN)) {
tashworth 6:75259c3306dd 507 fire_detected++;
tashworth 6:75259c3306dd 508 } else {
tashworth 6:75259c3306dd 509 fire_not_detected++;
tashworth 6:75259c3306dd 510 }
tashworth 6:75259c3306dd 511 }
tashworth 6:75259c3306dd 512 case 3:
tashworth 6:75259c3306dd 513 for (int i = 0; i<5; i++) {
tashworth 6:75259c3306dd 514 dist = getDistance();
tashworth 6:75259c3306dd 515 if ((dist < OILRIG3_MAX)
tashworth 6:75259c3306dd 516 || (dist > OILRIG3_MIN)) {
tashworth 6:75259c3306dd 517 fire_detected++;
tashworth 6:75259c3306dd 518 } else {
tashworth 6:75259c3306dd 519 fire_not_detected++;
tashworth 6:75259c3306dd 520 }
tashworth 6:75259c3306dd 521 }
tashworth 6:75259c3306dd 522 default:
tashworth 6:75259c3306dd 523 for (int i = 0; i<5; i++) {
tashworth 6:75259c3306dd 524 dist = getDistance();
tashworth 6:75259c3306dd 525 if ((dist < OILRIG1_MAX)
tashworth 6:75259c3306dd 526 || (dist > OILRIG1_MIN)) {
tashworth 6:75259c3306dd 527 fire_detected++;
tashworth 6:75259c3306dd 528 } else {
tashworth 6:75259c3306dd 529 fire_not_detected++;
tashworth 6:75259c3306dd 530 }
tashworth 6:75259c3306dd 531 }
tashworth 6:75259c3306dd 532 }
tashworth 6:75259c3306dd 533
tashworth 6:75259c3306dd 534 if (fire_detected > fire_not_detected) {
tashworth 6:75259c3306dd 535 return 1;
tashworth 6:75259c3306dd 536 } else {
tashworth 6:75259c3306dd 537 return 0;
tashworth 6:75259c3306dd 538 }
tashworth 0:1b64a0cedc5d 539 }
tashworth 0:1b64a0cedc5d 540
tashworth 0:1b64a0cedc5d 541
tashworth 0:1b64a0cedc5d 542