Paolina Povolotskaya
/
theRobotNEW
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: Adafruit-16-Ch-PWM-Servo-Driver HCSR04 PID PololuQik2 QEI Sharp mbed-rtos
Fork of
theRobot
by 
Thomas Ashworth
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
00001 #include "mbed.h" 00002 #include "Adafruit_PWMServoDriver.h" 00003 #include "ShapeDetect.h" 00004 #include "rtos.h" 00005 #include "PID.h" 00006 #include "PololuQik2.h" 00007 #include "QEI.h" 00008 #include "HCSR04.h" 00009 #include "stdio.h" 00010 #include "LPC17xx.h" 00011 #include "Sharp.h" 00012 00013 /* Navigation Definitions */ 00014 #define PIN_TRIGGERL (p12) 00015 #define PIN_ECHOL (p11) 00016 #define PIN_TRIGGERR (p29) 00017 #define PIN_ECHOR (p30) 00018 #define PULSE_PER_REV (1192) 00019 #define WHEEL_CIRCUM (12.56637) 00020 #define DIST_PER_PULSE (0.01054225722682) 00021 #define MTRS_TO_INCH (39.3701) 00022 #define MAX_SPEED (0.3*127) 00023 #define MAX_SPEED1 (0.25*127) 00024 #define PPR (4331/4) 00025 #define LEFT (1) 00026 #define RIGHT (0) 00027 #define STRAIGHT (2) 00028 #define FORWARD (1) 00029 #define BACKWARD (0) 00030 #define TOOLS (0) 00031 #define MID (1) 00032 #define RIGS (2) 00033 #define MID2 (3) 00034 #define RETURN (4) 00035 #define FAR (1) 00036 00037 //States 00038 #define START 0 00039 #define OILRIG1_POS 1 00040 #define OILRIG2_POS 2 00041 #define GOTO_TOOLS1 3 00042 #define IDENTIFY_TOOLS 4 00043 #define AQUIRE_TOOL1 5 00044 #define AQUIRE_TOOL2 6 00045 #define AQUIRE_TOOL3 7 00046 #define NAVIGATE_WAVES_ROW1 8 00047 #define NAVIGATE_WAVES_ROW2 9 00048 #define NAVIGATE_WAVES_ROW3 10 00049 #define NAVIGATE_TO_SQUARE_RIG 11 00050 #define NAVIGATE_TO_TRIANGLE_RIG 12 00051 #define NAVIGATE_TO_CIRCLE_RIG 13 00052 #define RIG_ALIGN 14 00053 #define INSERT_TOOL 15 00054 #define END 16 00055 #define GOTO_TOOLS2 17 00056 00057 00058 00059 //Servo Static Positions 00060 #define STORE_POSITION 0 00061 #define OIL_RIG1 1 00062 #define OIL_RIG2 2 00063 #define OIL_RIG3 3 00064 #define DRIVE_POSITION_NOTOOL 4 00065 #define TOOL_1 5 00066 #define TOOL_2 6 00067 #define TOOL_3 7 00068 #define DRIVE_POSITION_TOOL 8 00069 #define ORIENT_TOOL 9 00070 #define PU_TOOL_1 10 00071 #define PU_TOOL_2 11 00072 #define PU_TOOL_3 12 00073 #define PU_TOOL_1_STAB 13 00074 #define PU_TOOL_2_STAB 14 00075 #define PU_TOOL_3_STAB 15 00076 00077 //Rig definitions 00078 #define SQUARE 1 00079 #define TRIANGLE 2 00080 #define CIRCLE 3 00081 00082 00083 //Oil Rig distance thresholds 00084 #define OILRIG1_MAX 2200 00085 #define OILRIG1_MIN 1000 00086 #define OILRIG2_MAX 2200 00087 #define OILRIG2_MIN 1000 00088 #define OILRIG3_MAX 2200 00089 #define OILRIG3_MIN 1000 00090 00091 //for servo normalization 00092 #define MIN_SERVO_PULSE 900 00093 #define MAX_SERVO_PULSE 2100 00094 #define SERVO_MAX_ANGLE 180 00095 00096 #define X_CENTER 80 00097 #define Y_CENTER 60 00098 00099 00100 DigitalOut myled1(LED1); 00101 DigitalOut myled2(LED2); 00102 DigitalOut myled3(LED3); 00103 DigitalOut myled4(LED4); 00104 InterruptIn startBtn(p7); 00105 00106 void errFunction(void); 00107 bool cRc; 00108 00109 Serial pc(USBTX,USBRX); //USB Comm 00110 Adafruit_PWMServoDriver pwm(p28,p27); //pwm(SDA,SCL) - Servo Control PWM 00111 DigitalOut ServoOutputDisable(p8); //Servo Control Output Enable/Disable 00112 extern Serial lrf; //Laser Range Finder lrf(p13,p14) 00113 //Hardware Initialization 00114 //Serial bt(p13,p14); 00115 HCSR04 rangeFinderLeft( PIN_TRIGGERL, PIN_ECHOL ); 00116 HCSR04 rangeFinderRight( PIN_TRIGGERR, PIN_ECHOR ); 00117 PID pid1(15.0,0.0,4.0,0.02); 00118 PololuQik2 motors(p9, p10, p8, p15, errFunction, cRc); 00119 QEI rightEncoder(p17,p18,NC,PPR,QEI::X4_ENCODING); 00120 QEI leftEncoder(p16,p15,NC,PPR,QEI::X4_ENCODING); 00121 Sharp IR(p20); 00122 //InterruptIn encoder(p29); 00123 00124 00125 00126 00127 /*************** 00128 local servo functions 00129 ****************/ 00130 void servoBegin(void); 00131 void initServoDriver(void); 00132 void setServoPulse(uint8_t n, int angle); 00133 void setServoPulse2(uint8_t n, float angle); //float precision 00134 void setServoPulseNo_delay(uint8_t n, int angle); 00135 void servoPosition(int set); 00136 int fire_checker(int rig); 00137 int button_start = 0; 00138 00139 00140 //Navigation Functions 00141 void wall_follow(int side, int direction, int section); 00142 void wall_follow2(int side, int direction, int section, float location, int rig); 00143 void leftTurn(void); 00144 void slightleft(void); 00145 void slightright(void); 00146 void rightTurn(void); 00147 void slightMove(int direction, float pulses); 00148 void to_tools_section1(float* location, float ¤t); 00149 void to_tools_section2(float* location, float ¤t); 00150 void from_tools_section(float* location, float ¤t); 00151 void mid_section(float* location, float ¤t, int* direction); 00152 void mid_section2(float* location, float ¤t, int* direction); 00153 void rig_section(float* location, float ¤t, int* direction, int rig); 00154 void tools_section_return(float* location, float ¤t); 00155 void mid_section_return(float* location, float ¤t, int* direction); 00156 void mid_section2_return(float* location, float ¤t, int* direction); 00157 void rig_section_return(float* location, float ¤t, int* direction); 00158 void overBump(int section); 00159 void alignWithWall(int section); 00160 void UntilWall(int dir); 00161 00162 extern "C" void mbed_reset(); 00163 00164 /************ 00165 Main Variables 00166 *************/ 00167 int state = START; 00168 int fire = 0; 00169 int tool_needed = 0; 00170 int shape_detected = 0; 00171 float range, range2, pid_return; 00172 00173 00174 /************ 00175 Variables for Servos 00176 *************/ 00177 int servoNum, servoAngle, outputDisabled, posNum, testVal; 00178 int currentPosition[7]; 00179 00180 typedef struct { 00181 int arm_position_name; //for organization only (STORE, OILRIG1, OILRIG2...) 00182 int base_rotate; 00183 int base_arm; 00184 int big_arm; 00185 int claw_arm; 00186 int claw_rotate; 00187 int claw_open; 00188 } Coord; 00189 00190 /******************** 00191 Static Arm Positions 00192 *********************/ 00193 00194 Coord Arm_Table[] = { 00195 00196 // POSITION ODER: 00197 // base_rotate, base_arm, int big_arm, int claw_arm, int claw_rotate, int claw_open 00198 00199 //increase in number 5 rotates gripper 00200 00201 {STORE_POSITION, 85, 10, 0, 165, 175, 0}, // storing position 00202 {OIL_RIG1, 160, 20, 60, 45, 175, 0}, // point laser at oilrig1 00203 {OIL_RIG2, 163, 20, 60, 45, 175, 0}, // point laser at oilrig2 00204 {OIL_RIG3, 130, 90, 90, 52, 175, 0}, // point laser at oilrig2 00205 {DRIVE_POSITION_NOTOOL, 85, 10, 0, 165, 175, 0}, // Drive through course 00206 {TOOL_1, 96, 50, 80, 109, 90, 0}, // Look over first tool 00207 {TOOL_2, 79, 50, 80, 110, 90, 0}, // Look over second tool 00208 {TOOL_3, 59, 50, 80, 109, 90, 0}, // Look over third tool 00209 {DRIVE_POSITION_TOOL, 85, 10, 0, 165, 90, 105}, // Drive with tool loaded 00210 {ORIENT_TOOL, 135, 60, 75, 60, 90, 90}, // position tool to be inserted 00211 {PU_TOOL_1, 96, 46, 78, 102, 170, 0}, // STATIC Pickup tool POSITION 00212 {PU_TOOL_2, 74, 47, 80, 104, 170, 0}, // STATIC Pickup tool POSITION 00213 {PU_TOOL_3, 57, 44, 76, 102, 170, 0}, // STATIC Pickup tool POSITION 00214 {PU_TOOL_1_STAB, 98, 50, 90, 118, 90, 0}, // STAB TOOL 1 00215 {PU_TOOL_2_STAB, 78, 50, 90, 108, 90, 0}, // STAB TOOL 2 00216 {PU_TOOL_3_STAB, 53, 50, 90, 118, 90, 0}, // STAB TOOL 3 00217 }; 00218 00219 00220 /* Variables for imageprocessing and distance */ 00221 int x_coord; 00222 int y_coord; 00223 int area; 00224 int shape; 00225 int shape_alignX_done = 0; 00226 int shape_alignY_done = 0; 00227 float deltaX = 0; 00228 00229 00230 /* Variables for distance sensor*/ 00231 int distLaser; 00232 int fire_detected = 0; 00233 int fire_not_detected = 0; 00234 00235 void button_int(void) 00236 { 00237 if(!button_start) { 00238 button_start = 1; 00239 wait(1.0); 00240 } else { 00241 button_start = 0; 00242 mbed_reset(); 00243 } 00244 return; 00245 } 00246 00247 00248 int main() 00249 { 00250 00251 /***************** 00252 INITIALIZATIONS 00253 *******************/ 00254 float location[3], current=4; 00255 int direction[3]; 00256 double distance; 00257 00258 int pu, num, input; 00259 00260 00261 00262 pc.baud(115200); 00263 /*while(1){ 00264 rangeFinderLeft.startMeas(); 00265 wait_ms(20); 00266 rangeFinderLeft.getMeas(range); 00267 rangeFinderRight.startMeas(); 00268 wait_ms(20); 00269 rangeFinderRight.getMeas(range2); 00270 pc.printf("left %f \t right %f\r\n", range, range2); 00271 00272 }*/ 00273 //Laser Range Finder Initialization 00274 lrf_baudCalibration(); 00275 00276 motors.begin(); 00277 00278 startBtn.rise(&button_int); 00279 00280 //Servo initialization 00281 initServoDriver(); 00282 servoBegin(); //initiates servos to start position 00283 //ServoOutputDisable = 0; 00284 00285 /******************************** 00286 MAIN WHILE LOOP FOR COMPETITION 00287 *********************************/ 00288 while(1) { 00289 if(button_start == 1) { 00290 00291 00292 switch (state) { 00293 00294 /************************************************** 00295 * STAGE 0 00296 * 00297 * - START OF THE COMETITION 00298 * 00299 **************************************************/ 00300 case START : 00301 myled1 = 1; 00302 state = OILRIG1_POS; 00303 break; 00304 00305 00306 /************************************************** 00307 * STAGE 1 00308 * 00309 * - DETERMINE OIL RIG ON FIRE 00310 * 00311 **************************************************/ 00312 00313 case OILRIG1_POS: //aims arm at square oil rig 00314 /* 00315 servoPosition(OIL_RIG1); 00316 wait(3); //wait for servo to settle before laser distance 00317 00318 fire = fire_checker(OIL_RIG1); //determines if oil rig is on fire 00319 */ fire=1; 00320 //determines what tool is needed 00321 if (fire == 1) { 00322 //pc.printf("FIRE FOUND!!!!!!!!\n\r"); 00323 tool_needed = SQUARE; 00324 state = GOTO_TOOLS1; 00325 } else { 00326 //pc.printf("XXXXXX FIRE NOT FOUND XXXXXX"); 00327 state = OILRIG2_POS; 00328 } 00329 break; 00330 00331 case OILRIG2_POS: 00332 /* 00333 setServoPulse(2, Arm_Table[DRIVE_POSITION_NOTOOL].big_arm); 00334 setServoPulse(3, Arm_Table[DRIVE_POSITION_NOTOOL].claw_arm); 00335 setServoPulse(1, Arm_Table[DRIVE_POSITION_NOTOOL].base_arm); 00336 setServoPulse(0, Arm_Table[DRIVE_POSITION_NOTOOL].base_rotate); 00337 setServoPulse(4, Arm_Table[DRIVE_POSITION_NOTOOL].claw_rotate); 00338 setServoPulse(5, Arm_Table[DRIVE_POSITION_NOTOOL].claw_open); 00339 wait(3); //wait for servos to settle 00340 */ 00341 to_tools_section2(location, current); // moves to second rig 00342 wait(2); 00343 /* 00344 servoPosition(OIL_RIG2); //position arm to point at first oilrig 00345 wait(3); 00346 00347 fire = fire_checker(OIL_RIG2); 00348 */ fire=1; 00349 if (fire == 1) { 00350 //pc.printf("FIRE FOUND!!!!!!!!"); 00351 tool_needed = TRIANGLE; 00352 state = GOTO_TOOLS2; 00353 } else { 00354 //pc.printf("XXXXXX FIRE NOT FOUND XXXXXX"); 00355 tool_needed = CIRCLE; 00356 state = GOTO_TOOLS2; 00357 } 00358 break; 00359 00360 /************************************************** 00361 * STAGE 2 00362 * 00363 * - TRAVEL TO TOOLS 00364 * 00365 **************************************************/ 00366 case GOTO_TOOLS1: 00367 /* setServoPulse(2, Arm_Table[DRIVE_POSITION_NOTOOL].big_arm); 00368 setServoPulse(3, Arm_Table[DRIVE_POSITION_NOTOOL].claw_arm); 00369 setServoPulse(1, Arm_Table[DRIVE_POSITION_NOTOOL].base_arm); 00370 setServoPulse(0, Arm_Table[DRIVE_POSITION_NOTOOL].base_rotate); 00371 setServoPulse(4, Arm_Table[DRIVE_POSITION_NOTOOL].claw_rotate); 00372 setServoPulse(5, Arm_Table[DRIVE_POSITION_NOTOOL].claw_open); 00373 wait(3); //wait for servos to settle 00374 */ 00375 to_tools_section1(location, current); 00376 wait(2); 00377 state = IDENTIFY_TOOLS; 00378 break; 00379 00380 case GOTO_TOOLS2: 00381 /* 00382 setServoPulse(2, Arm_Table[DRIVE_POSITION_NOTOOL].big_arm); 00383 setServoPulse(3, Arm_Table[DRIVE_POSITION_NOTOOL].claw_arm); 00384 setServoPulse(1, Arm_Table[DRIVE_POSITION_NOTOOL].base_arm); 00385 setServoPulse(0, Arm_Table[DRIVE_POSITION_NOTOOL].base_rotate); 00386 setServoPulse(4, Arm_Table[DRIVE_POSITION_NOTOOL].claw_rotate); 00387 setServoPulse(5, Arm_Table[DRIVE_POSITION_NOTOOL].claw_open); 00388 wait(3); //wait for servos to settle 00389 */ 00390 slightMove(FORWARD,3100); 00391 current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2; 00392 wait(2); 00393 00394 state = IDENTIFY_TOOLS; 00395 break; 00396 00397 00398 00399 /************************************************** 00400 * STAGE 3 00401 * 00402 * - Determine order of tools 00403 * - Aquire appropriate tool 00404 * 00405 **************************************************/ 00406 case IDENTIFY_TOOLS: 00407 00408 state = NAVIGATE_WAVES_ROW1; 00409 break; 00410 00411 00412 wait(5); 00413 servoPosition(TOOL_2); //arm/camera looks over tool 00414 wait(5); //wait for servos to settle 00415 00416 //shape_detected = shapeDetection(); //determines the shape 00417 //clearBounds(); 00418 //printImageToFile(BINARY); 00419 shape_alignX_done = 0; 00420 shape_alignY_done = 0; 00421 /* 00422 while( shape_alignY_done == 0) { 00423 wait(1); 00424 shape_detected = shapeDetection(); 00425 00426 //pc.printf("Y - Adjust to center tool\n\r"); 00427 00428 if(get_com_y() < 50) { 00429 wait(1); 00430 slightMove(FORWARD,25); 00431 current-=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2; 00432 00433 } else if(get_com_y() > 70) { 00434 wait(1); 00435 slightMove(BACKWARD,25); 00436 current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2; 00437 00438 } else { 00439 shape_alignY_done = 1; 00440 } 00441 }*/ 00442 00443 00444 for(int i = 0; i < 4; i++) { 00445 shape_detected = shapeDetection(); 00446 wait(2); 00447 if(get_com_x() > X_CENTER ) { 00448 deltaX = get_com_x()-X_CENTER; 00449 setServoPulse2(0,Arm_Table[TOOL_2].base_rotate += (deltaX/60.0) ); 00450 Arm_Table[PU_TOOL_2].base_rotate = Arm_Table[TOOL_2].base_rotate; 00451 } 00452 if(get_com_x() < X_CENTER) { 00453 deltaX = get_com_x()-X_CENTER; 00454 setServoPulse2(0,Arm_Table[TOOL_2].base_rotate += (deltaX/60.0) ); 00455 Arm_Table[PU_TOOL_2].base_rotate = Arm_Table[TOOL_2].base_rotate; 00456 } 00457 } 00458 00459 00460 /* 00461 00462 while( shape_alignX_done == 0) { 00463 wait(1); 00464 shape_detected = shapeDetection(); 00465 00466 pc.printf("X - Adjust to center tool\n\r"); 00467 if(get_com_x() > 95) { 00468 setServoPulse(0,Arm_Table[TOOL_2].base_rotate+=1); 00469 00470 } else if(get_com_x() < 65) { 00471 setServoPulse(0,Arm_Table[TOOL_2].base_rotate-=1); 00472 00473 } else { 00474 shape_alignX_done = 1; 00475 } 00476 }*/ 00477 00478 //printImageToFile(BINARY); 00479 //either goes to aquire the tool or look at the next shape 00480 if(shape_detected == tool_needed) { 00481 state = AQUIRE_TOOL2; 00482 break; 00483 } else { 00484 slightMove(FORWARD,40); 00485 current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2; 00486 00487 00488 servoPosition(TOOL_1); 00489 wait(5); //wait for servos to settle 00490 shape_alignX_done = 0; 00491 shape_alignY_done = 0; 00492 /* 00493 while( shape_alignY_done == 0) { 00494 wait(1); 00495 shape_detected = shapeDetection(); 00496 00497 pc.printf("Y - Adjust to center tool\n\r"); 00498 00499 if(get_com_y() < 50) { 00500 wait(1); 00501 slightMove(FORWARD,25); 00502 current-=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2; 00503 00504 } else if(get_com_y() > 70) { 00505 wait(1); 00506 slightMove(BACKWARD,25); 00507 current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2; 00508 00509 } else { 00510 shape_alignY_done = 1; 00511 } 00512 }*/ 00513 00514 00515 for(int i = 0; i < 4; i++) { 00516 shape_detected = shapeDetection(); 00517 wait(2); 00518 if (get_com_x() > X_CENTER) { 00519 deltaX = get_com_x()-X_CENTER; 00520 setServoPulse2(0,Arm_Table[TOOL_1].base_rotate += (deltaX/60.0) ); 00521 Arm_Table[PU_TOOL_1].base_rotate = Arm_Table[TOOL_1].base_rotate; 00522 } 00523 if (get_com_x() < X_CENTER) { 00524 deltaX = get_com_x()-X_CENTER; 00525 setServoPulse2(0,Arm_Table[TOOL_1].base_rotate += (deltaX/60.0) ); 00526 Arm_Table[PU_TOOL_1].base_rotate = Arm_Table[TOOL_1].base_rotate; 00527 } 00528 } 00529 00530 00531 /* 00532 while( shape_alignX_done == 0) { 00533 wait(1); 00534 shape_detected = shapeDetection(); 00535 00536 pc.printf("X - Adjust to center tool\n\r"); 00537 if(get_com_x() > 100) { 00538 setServoPulse(0,Arm_Table[TOOL_1].base_rotate+=1); 00539 00540 } else if(get_com_x() < 60) { 00541 setServoPulse(0,Arm_Table[TOOL_1].base_rotate-=1); 00542 00543 } else { 00544 shape_alignX_done = 1; 00545 } 00546 }*/ 00547 00548 if (shape_detected == tool_needed) { 00549 state = AQUIRE_TOOL1; 00550 break; 00551 } else { 00552 servoPosition(TOOL_3); 00553 wait(3); //wait for servos to settle 00554 state = AQUIRE_TOOL3; 00555 } 00556 } 00557 00558 break; 00559 00560 case AQUIRE_TOOL1: 00561 00562 servoPosition(PU_TOOL_1); 00563 setServoPulse(4, 175); 00564 wait(5); 00565 setServoPulse(0, Arm_Table[PU_TOOL_1].base_rotate ); 00566 wait(1); 00567 setServoPulse(1, Arm_Table[PU_TOOL_1].base_arm - 5); 00568 wait(1); 00569 setServoPulse(5, 105); 00570 wait(.5); 00571 setServoPulse(5, 00); 00572 setServoPulse(1, Arm_Table[PU_TOOL_1].base_arm - 6); 00573 wait(.5); 00574 setServoPulse(5, 105); 00575 wait(.5); 00576 setServoPulse(5, 00); 00577 setServoPulse(1, Arm_Table[PU_TOOL_1].base_arm - 10); 00578 wait(1); 00579 setServoPulse(5, 105); 00580 wait(1); 00581 setServoPulse(1, Arm_Table[PU_TOOL_1].base_arm); 00582 wait(2); 00583 00584 setServoPulse(2, Arm_Table[DRIVE_POSITION_TOOL].big_arm); 00585 setServoPulse(3, Arm_Table[DRIVE_POSITION_TOOL].claw_arm); 00586 setServoPulse(1, Arm_Table[DRIVE_POSITION_TOOL].base_arm); 00587 setServoPulse(0, Arm_Table[DRIVE_POSITION_TOOL].base_rotate); 00588 setServoPulse(4, Arm_Table[DRIVE_POSITION_TOOL].claw_rotate); 00589 setServoPulse(5, Arm_Table[DRIVE_POSITION_TOOL].claw_open); 00590 00591 00592 state = NAVIGATE_WAVES_ROW1; 00593 break; 00594 00595 case AQUIRE_TOOL2: 00596 servoPosition(PU_TOOL_2); 00597 setServoPulse(4, 175); 00598 wait(5); 00599 setServoPulse(0, Arm_Table[PU_TOOL_2].base_rotate ); 00600 wait(1); 00601 setServoPulse(1, Arm_Table[PU_TOOL_2].base_arm - 2); 00602 wait(1); 00603 setServoPulse(3, Arm_Table[PU_TOOL_2].claw_arm - 2); 00604 wait(1); 00605 setServoPulse(5, 105); 00606 wait(2); 00607 setServoPulse(5, 00); 00608 setServoPulse(1, Arm_Table[PU_TOOL_2].base_arm - 6); 00609 wait(2); 00610 setServoPulse(5, 105); 00611 wait(2); 00612 setServoPulse(5, 00); 00613 setServoPulse(1, Arm_Table[PU_TOOL_2].base_arm - 10); 00614 wait(2); 00615 setServoPulse(5, 105); 00616 wait(2); 00617 setServoPulse(1, Arm_Table[PU_TOOL_2].base_arm); 00618 00619 wait(2); 00620 00621 setServoPulse(2, Arm_Table[DRIVE_POSITION_TOOL].big_arm); 00622 setServoPulse(3, Arm_Table[DRIVE_POSITION_TOOL].claw_arm); 00623 setServoPulse(1, Arm_Table[DRIVE_POSITION_TOOL].base_arm); 00624 setServoPulse(0, Arm_Table[DRIVE_POSITION_TOOL].base_rotate); 00625 setServoPulse(4, Arm_Table[DRIVE_POSITION_TOOL].claw_rotate); 00626 setServoPulse(5, Arm_Table[DRIVE_POSITION_TOOL].claw_open); 00627 00628 00629 state = NAVIGATE_WAVES_ROW1; 00630 break; 00631 00632 case AQUIRE_TOOL3: 00633 /* 00634 while( shape_alignY_done == 0) { 00635 wait(1); 00636 00637 servoPosition(PU_TOOL_3); 00638 shape_detected = shapeDetection(); 00639 wait(2); 00640 00641 pc.printf("Y - Adjust to center tool\n\r"); 00642 00643 if(get_com_y() < 50) { 00644 wait(1); 00645 slightMove(FORWARD,25); 00646 current-=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2; 00647 00648 } else if(get_com_y() > 70) { 00649 wait(1); 00650 slightMove(BACKWARD,25); 00651 current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2; 00652 00653 } else { 00654 shape_alignY_done = 1; 00655 } 00656 } 00657 00658 */ 00659 for(int i = 0; i < 6; i++) { 00660 shape_detected = shapeDetection(); 00661 wait(2); 00662 if (get_com_x() > X_CENTER) { 00663 deltaX = get_com_x()-X_CENTER; 00664 setServoPulse2(0,Arm_Table[PU_TOOL_3].base_rotate += (deltaX/60.0) ); 00665 } 00666 if (get_com_x() < X_CENTER) { 00667 deltaX = get_com_x()-X_CENTER; 00668 setServoPulse2(0,Arm_Table[PU_TOOL_3].base_rotate += (deltaX/60.0) ); 00669 } 00670 } 00671 00672 setServoPulse(4, 175); 00673 wait(5); 00674 setServoPulse(0, Arm_Table[PU_TOOL_3].base_rotate ); 00675 wait(1); 00676 setServoPulse(1, Arm_Table[PU_TOOL_2].base_arm - 2); 00677 wait(1); 00678 setServoPulse(3, Arm_Table[PU_TOOL_2].claw_arm - 3); 00679 wait(1); 00680 setServoPulse(5, 105); 00681 wait(.5); 00682 setServoPulse(5, 00); 00683 setServoPulse(1, Arm_Table[PU_TOOL_3].base_arm - 6); 00684 wait(.5); 00685 setServoPulse(5, 105); 00686 wait(.5); 00687 setServoPulse(5, 00); 00688 setServoPulse(1, Arm_Table[PU_TOOL_3].base_arm - 10); 00689 wait(1); 00690 setServoPulse(5, 105); 00691 wait(1); 00692 setServoPulse(1, Arm_Table[PU_TOOL_3].base_arm); 00693 00694 wait(2); 00695 00696 setServoPulse(2, Arm_Table[DRIVE_POSITION_TOOL].big_arm); 00697 setServoPulse(3, Arm_Table[DRIVE_POSITION_TOOL].claw_arm); 00698 setServoPulse(1, Arm_Table[DRIVE_POSITION_TOOL].base_arm); 00699 setServoPulse(0, Arm_Table[DRIVE_POSITION_TOOL].base_rotate); 00700 setServoPulse(4, Arm_Table[DRIVE_POSITION_TOOL].claw_rotate); 00701 setServoPulse(5, Arm_Table[DRIVE_POSITION_TOOL].claw_open); 00702 00703 state = NAVIGATE_WAVES_ROW1; 00704 break; 00705 00706 00707 /************************************************** 00708 * STAGE 4 00709 * 00710 * - Navigate through the ocean 00711 * 00712 **************************************************/ 00713 00714 case NAVIGATE_WAVES_ROW1: 00715 from_tools_section(location,current); 00716 00717 mid_section(location, current, direction); 00718 00719 state = NAVIGATE_WAVES_ROW2; 00720 break; 00721 00722 case NAVIGATE_WAVES_ROW2: 00723 mid_section2(location, current, direction); 00724 state = NAVIGATE_WAVES_ROW3; 00725 break; 00726 00727 case NAVIGATE_WAVES_ROW3: 00728 shape_detected=1; 00729 00730 00731 if(shape_detected == 1) { 00732 rig_section(location, current, direction, 1); 00733 while(1); 00734 state = NAVIGATE_TO_SQUARE_RIG; 00735 } else if(shape_detected == 2) { 00736 rig_section(location, current, direction, 2); 00737 while(1); 00738 state = NAVIGATE_TO_TRIANGLE_RIG; 00739 } else { 00740 rig_section(location, current, direction, 3); 00741 while(1); 00742 state = NAVIGATE_TO_CIRCLE_RIG; 00743 } 00744 break; 00745 00746 /************************************************** 00747 * STAGE 5 00748 * 00749 * - Travel to appropriate rig 00750 * 00751 **************************************************/ 00752 case NAVIGATE_TO_SQUARE_RIG: 00753 //NAVIGATION CODE HERE 00754 state = RIG_ALIGN; 00755 break; 00756 case NAVIGATE_TO_TRIANGLE_RIG: 00757 //NAVIGATION CODE HERE 00758 state = RIG_ALIGN; 00759 break; 00760 case NAVIGATE_TO_CIRCLE_RIG: 00761 //NAVIGATION CODE HERE 00762 state = RIG_ALIGN; 00763 break; 00764 00765 /************************************************** 00766 * STAGE 6 00767 * 00768 * - Align with appropriate rig 00769 * 00770 **************************************************/ 00771 case RIG_ALIGN: 00772 00773 //*********************// 00774 //******* TODO ********// 00775 //*********************// 00776 // CODE TO ALIGN ROBOT WITH RIG 00777 00778 // servoPosition(ORIENT_TOOL); 00779 wait(1); //wait for servos to settle 00780 state = INSERT_TOOL; 00781 break; 00782 00783 /************************************************** 00784 * STAGE 7 00785 * 00786 * - Insert Tool 00787 * - Extenguish fire 00788 * - win contest 00789 * 00790 **************************************************/ 00791 00792 case INSERT_TOOL: 00793 //*********************// 00794 //******* TODO ********// 00795 //*********************// 00796 // CODE TO INSERT TOOL 00797 break; 00798 00799 /************************************************** 00800 * STAGE 8 00801 * 00802 * - END COMPETITION 00803 * 00804 **************************************************/ 00805 case END: 00806 servoPosition(STORE_POSITION); 00807 myled1 = 1; 00808 wait(.2); 00809 myled2 = 1; 00810 wait(.2); 00811 myled3 = 1; 00812 wait(.2); 00813 myled4 = 1; 00814 wait(.2); 00815 break; 00816 default: 00817 00818 break; 00819 } 00820 } // End while loop 00821 00822 } // End if for start button 00823 00824 00825 } // main loop 00826 00827 00828 00829 /************ 00830 00831 Servo Functions 00832 00833 **************/ 00834 00835 void setServoPulse(uint8_t n, int angle) 00836 { 00837 int pulse; 00838 pulse = MIN_SERVO_PULSE + (( angle * (MAX_SERVO_PULSE - MIN_SERVO_PULSE)) / SERVO_MAX_ANGLE); 00839 float pulselength = 20000; // 20,000 us per second 00840 int i = currentPosition[n]; 00841 //pc.printf("ServoNumber: %d Begining Pulse: %d\n\r",n,currentPosition[n]); 00842 int pulse2; 00843 if(currentPosition[n] < pulse) { 00844 for(i; i < pulse; i++) { 00845 pulse2 = 4094 * i / pulselength; 00846 pwm.setPWM(n, 0, pulse2); 00847 wait_ms(3); 00848 } 00849 } else if (currentPosition[n] > pulse) { 00850 for(i; i > pulse; i--) { 00851 pulse2 = 4094 * i / pulselength; 00852 pwm.setPWM(n, 0, pulse2); 00853 wait_ms(3); 00854 } 00855 } 00856 currentPosition[n] = i; 00857 //pc.printf("END: pulse: %d, angle: %d\n\r", i, angle); 00858 } 00859 00860 void initServoDriver(void) 00861 { 00862 pwm.begin(); 00863 //pwm.setPWMFreq(100); //This dosen't work well because of uncertain clock speed. Use setPrescale(). 00864 pwm.setPrescale(140); //This value is decided for 20ms interval. 00865 pwm.setI2Cfreq(400000); //400kHz 00866 00867 } 00868 00869 void servoBegin(void) 00870 { 00871 //pc.printf("Setting Initial Servo Position\n\r"); 00872 setServoPulseNo_delay(3, Arm_Table[STORE_POSITION].claw_arm); 00873 setServoPulseNo_delay(2, Arm_Table[STORE_POSITION].big_arm); 00874 wait(2); 00875 setServoPulseNo_delay(1, Arm_Table[STORE_POSITION].base_arm); 00876 setServoPulseNo_delay(0, Arm_Table[STORE_POSITION].base_rotate); 00877 setServoPulseNo_delay(4, Arm_Table[STORE_POSITION].claw_rotate); 00878 setServoPulseNo_delay(5, Arm_Table[STORE_POSITION].claw_open); 00879 } 00880 00881 void setServoPulseNo_delay(uint8_t n, int angle) 00882 { 00883 int pulse = MIN_SERVO_PULSE + (( angle * (MAX_SERVO_PULSE - MIN_SERVO_PULSE)) / SERVO_MAX_ANGLE); 00884 float pulselength = 20000; // 20,000 us per second 00885 currentPosition[n] = pulse; 00886 //pc.printf("ServoNumber: %d Pulsewidth: %d Angle: %d \n\r",n,pulse, angle); 00887 pulse = 4094 * pulse / pulselength; 00888 pwm.setPWM(n, 0, pulse); 00889 00890 } 00891 void setServoPulse2(uint8_t n, float angle) 00892 { 00893 float pulse = MIN_SERVO_PULSE + (( angle * (MAX_SERVO_PULSE - MIN_SERVO_PULSE)) / SERVO_MAX_ANGLE); 00894 float pulselength = 20000; // 10,000 us per second 00895 pulse = 4094 * pulse / pulselength; 00896 pwm.setPWM(n, 0, pulse); 00897 } 00898 00899 00900 00901 void servoPosition(int set) 00902 { 00903 //moves to current position 00904 setServoPulse(3, Arm_Table[set].claw_arm); 00905 setServoPulse(2, Arm_Table[set].big_arm); 00906 setServoPulse(1, Arm_Table[set].base_arm); 00907 setServoPulse(0, Arm_Table[set].base_rotate); 00908 setServoPulse(4, Arm_Table[set].claw_rotate); 00909 setServoPulse(5, Arm_Table[set].claw_open); 00910 } 00911 00912 00913 int fire_checker(int rig) 00914 { 00915 switch (rig) { 00916 00917 case 1: 00918 for (int i = 0; i<10; i++) { 00919 distLaser = getDistance(); 00920 //pc.printf("L DISTANCE: %d \n\r", distLaser); 00921 if ((distLaser < OILRIG1_MAX) 00922 && (distLaser > OILRIG1_MIN)) { 00923 fire_detected++; 00924 } else { 00925 fire_not_detected++; 00926 } 00927 } 00928 break; 00929 case 2: 00930 for (int i = 0; i<10; i++) { 00931 distLaser = getDistance(); 00932 //pc.printf("L DISTANCE: %d \n\r", distLaser); 00933 if ((distLaser < OILRIG2_MAX) 00934 && (distLaser > OILRIG2_MIN)) { 00935 fire_detected++; 00936 } else { 00937 fire_not_detected++; 00938 } 00939 } 00940 break; 00941 00942 } 00943 00944 if (fire_detected > 0) { 00945 return 1; 00946 } else { 00947 return 0; 00948 } 00949 } 00950 00951 void errFunction(void) 00952 { 00953 //Nothing 00954 } 00955 void wall_follow(int side, int direction, int section) 00956 { 00957 float location, set=4; 00958 int dir=1; 00959 00960 pid1.reset(); 00961 00962 if(direction == BACKWARD) dir=-1; 00963 if(section == TOOLS)set= 10; 00964 00965 leftEncoder.reset(); 00966 rightEncoder.reset(); 00967 00968 location=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2; 00969 00970 while(location< 66.5) { 00971 location=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2; 00972 00973 pid1.setInputLimits(0, set); 00974 pid1.setOutputLimits( -MAX_SPEED, MAX_SPEED); 00975 pid1.setSetPoint(set); 00976 if(side) { 00977 rangeFinderLeft.startMeas(); 00978 wait_ms(20); 00979 rangeFinderLeft.getMeas(range); 00980 } else { 00981 rangeFinderRight.startMeas(); 00982 wait_ms(20); 00983 rangeFinderRight.getMeas(range); 00984 //pc.printf("%d\r\n",range); 00985 } 00986 00987 if(range > 15) { 00988 //pc.printf("wavegap %f\r\n",wavegap); 00989 // AT WAVE OPENING!!!! 00990 motors.setMotor1Speed(dir*0.25*127);//left 00991 motors.setMotor0Speed(dir*0.25*127);//right 00992 } else { 00993 00994 pid1.setProcessValue(range); 00995 pid_return = pid1.compute(); 00996 00997 if(pid_return > 0) { 00998 if(side) { 00999 motors.setMotor0Speed(dir*MAX_SPEED - dir*pid_return);//right 01000 motors.setMotor1Speed(dir*MAX_SPEED);//left 01001 } else { 01002 motors.setMotor1Speed(dir*MAX_SPEED - dir*pid_return);//left 01003 motors.setMotor0Speed(dir*MAX_SPEED);//right 01004 } 01005 } else if(pid_return < 0) { 01006 if(side) { 01007 motors.setMotor0Speed(dir*MAX_SPEED);//right 01008 motors.setMotor1Speed(dir*MAX_SPEED + dir*pid_return);//left 01009 } else { 01010 motors.setMotor1Speed(dir*MAX_SPEED);//left 01011 motors.setMotor0Speed(dir*MAX_SPEED + dir*pid_return);//right 01012 } 01013 } else { 01014 motors.setMotor0Speed(dir*MAX_SPEED);//right 01015 motors.setMotor1Speed(dir*MAX_SPEED);//left 01016 } 01017 } 01018 } 01019 01020 //STOP 01021 motors.setMotor0Speed(dir*-0.3*127); //right 01022 motors.setMotor1Speed(dir*-0.3*127); //left 01023 wait_ms(10); 01024 motors.stopBothMotors(0); 01025 } 01026 01027 /* MODIFIED WALL_FOLLOW FOR NAVIGATION */ 01028 01029 void wall_follow2(int side, int direction, int section, float location, int rig) 01030 { 01031 int dir=1, limit=80, lowlim=4; 01032 float set=9, loc=0, Rigloc=0; 01033 bool SeeWaveGap = false; 01034 01035 if(rig == 1) Rigloc= 16; 01036 else if(rig == 2) Rigloc= 45; 01037 else if(rig== 3) Rigloc = 70; 01038 01039 pid1.reset(); 01040 01041 if(direction == BACKWARD) { 01042 dir=-1; 01043 limit = 100; 01044 } 01045 else if(direction == FORWARD) lowlim=-20; 01046 if(section == TOOLS) { 01047 set= 9; 01048 limit = 86; 01049 } 01050 else if(section == RIGS) set = 9; 01051 else if(section == RETURN) lowlim=4; 01052 else if(section == MID2) limit =85; 01053 01054 if(location <4) limit=80; 01055 01056 leftEncoder.reset(); 01057 rightEncoder.reset(); 01058 01059 pc.printf("before %f\r\n", location); 01060 01061 //pc.printf("dir*loc+location %f\r\n",dir*loc + location ); 01062 //pc.printf("limit %d \r\n", limit); 01063 01064 while((dir*loc + location <= limit) && (dir*loc + location >= lowlim)) { 01065 01066 loc=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2; 01067 //pc.printf("loc %f \r\n", loc); 01068 01069 pid1.setInputLimits(0.0, set); 01070 pid1.setOutputLimits( -MAX_SPEED1, MAX_SPEED1); 01071 pid1.setSetPoint(set); 01072 01073 if(side) { 01074 rangeFinderLeft.startMeas(); 01075 wait_ms(20); 01076 rangeFinderLeft.getMeas(range); 01077 } else { 01078 rangeFinderRight.startMeas(); 01079 wait_ms(20); 01080 rangeFinderRight.getMeas(range); 01081 } 01082 01083 if(section == RIGS) { 01084 rangeFinderLeft.startMeas(); 01085 wait_ms(20); 01086 rangeFinderLeft.getMeas(range2); 01087 01088 if(range2< 15) { 01089 if( abs(dir*loc + location - Rigloc) < 10) { 01090 //STOP 01091 motors.stopBothMotors(127); 01092 break; 01093 } 01094 } 01095 } 01096 01097 01098 //pc.printf("wall follow 2 range %f\r\n",range); 01099 //pc.printf("loc+location = %f\r\n", loc+location); 01100 if(range > 15 ) { 01101 if(section == RIGS || section == RETURN) { 01102 motors.setMotor0Speed(dir*0.25*127); //right 01103 motors.setMotor1Speed(dir*0.25*127); //left 01104 } else { 01105 if(!SeeWaveGap) { 01106 wait_ms(40); 01107 SeeWaveGap=true; 01108 } else { 01109 //STOP 01110 motors.stopBothMotors(127); 01111 01112 //pc.printf("wavegap\r\n"); 01113 // AT WAVE OPENING!!!! 01114 break; 01115 } 01116 } 01117 } else { 01118 SeeWaveGap = false; 01119 pid1.setProcessValue(range); 01120 pid_return = pid1.compute(); 01121 pc.printf("Range: %f\n PID: %f\r\n", range, pid_return); 01122 01123 if(pid_return > 0) { 01124 if(side) { 01125 motors.setMotor0Speed(dir*MAX_SPEED - dir*pid_return);//right 01126 motors.setMotor1Speed(dir*MAX_SPEED);//left 01127 } else { 01128 motors.setMotor1Speed(dir*MAX_SPEED - dir*pid_return);//left 01129 motors.setMotor0Speed(dir*MAX_SPEED);//right 01130 } 01131 } else if(pid_return < 0) { 01132 if(side) { 01133 motors.setMotor0Speed(dir*MAX_SPEED);//right 01134 motors.setMotor1Speed(dir*MAX_SPEED + dir*pid_return);//left 01135 } else { 01136 motors.setMotor1Speed(dir*MAX_SPEED);//left 01137 motors.setMotor0Speed(dir*MAX_SPEED + dir*pid_return);//right 01138 } 01139 } else { 01140 motors.setMotor0Speed(dir*MAX_SPEED); 01141 motors.setMotor1Speed(dir*MAX_SPEED); 01142 } 01143 } 01144 } 01145 01146 //STOP 01147 motors.stopBothMotors(127); 01148 } 01149 01150 01151 void alignWithWall(int section) 01152 { 01153 float usValue = 0; 01154 01155 if(section == TOOLS) { 01156 //pc.printf("tools section align\r\n"); 01157 // turn at an angle 01158 leftEncoder.reset(); 01159 rightEncoder.reset(); 01160 motors.setMotor0Speed(-1.2*MAX_SPEED); //right 01161 motors.setMotor1Speed(0.4*MAX_SPEED); //left 01162 while(rightEncoder.getPulses()>-1000); 01163 motors.stopBothMotors(0); 01164 01165 //go backwards toward wall 01166 leftEncoder.reset(); 01167 rightEncoder.reset(); 01168 motors.setMotor0Speed(-0.25*127); //right 01169 motors.setMotor1Speed(-0.25*127); //left 01170 while(abs(leftEncoder.getPulses()) < 300 || abs(rightEncoder.getPulses()) < 300); 01171 motors.stopBothMotors(0); 01172 01173 // turn left towards wall 01174 leftEncoder.reset(); 01175 rightEncoder.reset(); 01176 motors.setMotor0Speed(MAX_SPEED); //right 01177 motors.setMotor1Speed(-MAX_SPEED); //left 01178 while(rightEncoder.getPulses() < 20 || abs(leftEncoder.getPulses()) < 20); 01179 01180 motors.stopBothMotors(127); 01181 wait_ms(300); 01182 return; 01183 /* 01184 rangeFinderRight.startMeas(); 01185 wait_ms(20); 01186 rangeFinderRight.getMeas(range); 01187 01188 if(range>15){ 01189 // turning left 01190 leftEncoder.reset(); 01191 rightEncoder.reset(); 01192 motors.setMotor0Speed(-MAX_SPEED); //right 01193 motors.setMotor1Speed(MAX_SPEED); //left 01194 while(abs(rightEncoder.getPulses()) < 50 || abs(leftEncoder.getPulses()) < 50); 01195 motors.stopBothMotors(127); 01196 return; 01197 } 01198 */ 01199 // turning left 01200 //motors.setMotor0Speed(0.9*MAX_SPEED); //right 01201 //motors.setMotor1Speed(-0.9*MAX_SPEED); //left 01202 01203 } else if(section == RIGS) { 01204 // check distance to wall 01205 rangeFinderRight.startMeas(); 01206 wait_ms(20); 01207 rangeFinderRight.getMeas(range); 01208 01209 if(range < 3) return; 01210 01211 // turn at an angle 01212 leftEncoder.reset(); 01213 rightEncoder.reset(); 01214 motors.setMotor1Speed(-1.2*MAX_SPEED); //left 01215 motors.setMotor0Speed(0.4*MAX_SPEED); //right 01216 while(abs(leftEncoder.getPulses())<500); 01217 motors.stopBothMotors(0); 01218 wait(2); 01219 01220 //go backwards toward wall 01221 leftEncoder.reset(); 01222 rightEncoder.reset(); 01223 motors.setMotor0Speed(-0.25*127); //right 01224 motors.setMotor1Speed(-0.25*127); //left 01225 while(abs(leftEncoder.getPulses()) < 200 || abs(rightEncoder.getPulses()) < 200); 01226 motors.stopBothMotors(0); 01227 wait(2); 01228 01229 // turn right towards wall 01230 leftEncoder.reset(); 01231 rightEncoder.reset(); 01232 motors.setMotor0Speed(-MAX_SPEED); //right 01233 motors.setMotor1Speed(MAX_SPEED); //left 01234 while(abs(rightEncoder.getPulses()) < 50 || abs(leftEncoder.getPulses()) < 50); 01235 01236 motors.stopBothMotors(127); 01237 /* wait(2); 01238 01239 // turning left 01240 motors.setMotor0Speed(-0.9*MAX_SPEED); //right 01241 motors.setMotor1Speed(0.9*MAX_SPEED); //left 01242 */ 01243 }else if(section == MID2){ 01244 leftEncoder.reset(); 01245 rightEncoder.reset(); 01246 motors.setMotor0Speed(-1.2*MAX_SPEED); //right 01247 motors.setMotor1Speed(0.4*MAX_SPEED); //left 01248 while(rightEncoder.getPulses()>-1000); 01249 motors.stopBothMotors(0); 01250 01251 //go backwards toward wall 01252 leftEncoder.reset(); 01253 rightEncoder.reset(); 01254 motors.setMotor0Speed(-0.25*127); //right 01255 motors.setMotor1Speed(-0.25*127); //left 01256 while(abs(leftEncoder.getPulses()) < 150 || abs(rightEncoder.getPulses()) < 150); 01257 01258 // turn left towards wall 01259 leftEncoder.reset(); 01260 rightEncoder.reset(); 01261 motors.setMotor0Speed(0.4*127); //right 01262 motors.setMotor1Speed(-0.4*127); //left 01263 while(abs(rightEncoder.getPulses()) < 65 || abs(leftEncoder.getPulses()) < 65); 01264 motors.stopBothMotors(127); 01265 01266 slightMove(FORWARD,100); 01267 return; 01268 01269 } 01270 else {// MID 01271 //pc.printf("in mid section align\r\n"); 01272 // turn right towards wall 01273 rightTurn(); 01274 // turning left towards wall 01275 motors.setMotor0Speed(0.9*MAX_SPEED); //right 01276 motors.setMotor1Speed(-0.9*MAX_SPEED); //left 01277 01278 } 01279 01280 usValue = 0; 01281 /* while(1) { 01282 if(section == 10) { // CURENTLY NOT USED (WAS RIGS) 01283 rangeFinderRight.startMeas(); 01284 wait_ms(20); 01285 rangeFinderRight.getMeas(range); 01286 } else { 01287 rangeFinderLeft.startMeas(); 01288 wait_ms(20); 01289 rangeFinderLeft.getMeas(range); 01290 } 01291 //pc.printf("Range %f \t OldValue %f\n\r",range, usValue); 01292 if(range > usValue && usValue != 0 && range < 25) { 01293 break; 01294 } else { 01295 usValue = range; 01296 } 01297 } 01298 motors.stopBothMotors(0);*/ 01299 } 01300 01301 void rightTurn(void) 01302 { 01303 motors.begin(); 01304 leftEncoder.reset(); 01305 rightEncoder.reset(); 01306 motors.setMotor0Speed(-0.5*127);//right 01307 motors.setMotor1Speed(0.5*127);//left 01308 while(abs(leftEncoder.getPulses())<850 || abs(rightEncoder.getPulses())<850); 01309 motors.stopBothMotors(127); 01310 } 01311 01312 void leftTurn(void) 01313 { 01314 motors.begin(); 01315 leftEncoder.reset(); 01316 rightEncoder.reset(); 01317 motors.setMotor0Speed(0.5*127);// right 01318 motors.setMotor1Speed(-0.5*127);// left 01319 while(abs(leftEncoder.getPulses())<1075 || rightEncoder.getPulses()<1075); 01320 motors.stopBothMotors(127); 01321 } 01322 01323 void slightleft(void) 01324 { 01325 01326 leftEncoder.reset(); 01327 rightEncoder.reset(); 01328 motors.setMotor0Speed(0.5*127);// right 01329 motors.setMotor1Speed(-0.5*127);// left 01330 while(abs(leftEncoder.getPulses())<70 || rightEncoder.getPulses()<70); 01331 motors.stopBothMotors(127); 01332 } 01333 01334 void slightright(void) 01335 { 01336 01337 leftEncoder.reset(); 01338 rightEncoder.reset(); 01339 motors.setMotor0Speed(-0.4*127);// right 01340 motors.setMotor1Speed(0.4*127);// left 01341 while(abs(leftEncoder.getPulses())<200 || abs(rightEncoder.getPulses())<200); 01342 motors.stopBothMotors(127); 01343 } 01344 01345 void slightMove(int direction, float pulses) 01346 { 01347 int dir=1; 01348 01349 if(direction == BACKWARD) dir= -1; 01350 01351 leftEncoder.reset(); 01352 rightEncoder.reset(); 01353 motors.setMotor0Speed(dir*0.25*127); //right 01354 motors.setMotor1Speed(dir*0.25*127); //left 01355 while(abs(leftEncoder.getPulses()) < pulses || abs(rightEncoder.getPulses()) < pulses); 01356 01357 motors.stopBothMotors(127); 01358 } 01359 01360 void UntilWall(int dir) 01361 { 01362 01363 if(dir == BACKWARD) dir=-1; 01364 01365 leftEncoder.reset(); 01366 rightEncoder.reset(); 01367 motors.setMotor0Speed(dir*0.2*127); //right 01368 motors.setMotor1Speed(dir*0.2*127); //left 01369 01370 range = 30; 01371 01372 while(range > 20) { 01373 rangeFinderRight.startMeas(); 01374 wait_ms(20); 01375 rangeFinderRight.getMeas(range); 01376 } 01377 01378 motors.stopBothMotors(127); 01379 } 01380 01381 void overBump(int section) 01382 { 01383 int preLeft=5000, preRight=5000, out=0; 01384 01385 motors.begin(); 01386 // slight backwards 01387 leftEncoder.reset(); 01388 rightEncoder.reset(); 01389 motors.setMotor0Speed(-0.25*127); //right 01390 motors.setMotor1Speed(-0.25*127); //left 01391 while(abs(leftEncoder.getPulses()) < 50 || abs(rightEncoder.getPulses()) < 50); 01392 motors.stopBothMotors(127); 01393 01394 //pc.printf("slight backwards\r\n"); 01395 wait_ms(200); 01396 01397 // Over bump 01398 leftEncoder.reset(); 01399 rightEncoder.reset(); 01400 motors.setMotor0Speed(0.3*127); //right 01401 motors.setMotor1Speed(0.3*127); //left 01402 while((abs(leftEncoder.getPulses()) < 800 || abs(rightEncoder.getPulses()) < 800) /*&& preLeft!=0*/ && IR.getIRDistance() >15 ); 01403 01404 01405 01406 //pc.printf("forward \r\n"); 01407 wait_ms(200); 01408 01409 motors.stopBothMotors(0); 01410 motors.begin(); 01411 01412 preLeft=preRight=5000 ; 01413 leftEncoder.reset(); 01414 rightEncoder.reset(); 01415 motors.setMotor0Speed(.25*127); //right 01416 motors.setMotor1Speed(.25*127); //left 01417 01418 if(section == TOOLS) { 01419 while(IR.getIRDistance() > 10 && (abs(leftEncoder.getPulses()) < 200 || abs(rightEncoder.getPulses()) < 200) && (leftEncoder.getPulses() != preLeft || rightEncoder.getPulses() != preRight)) { 01420 01421 if(IR.getIRDistance() > 38) break; 01422 01423 preLeft=leftEncoder.getPulses(); 01424 preRight=rightEncoder.getPulses(); 01425 wait_ms(200); 01426 } 01427 } else if(section == MID || section == MID2) { 01428 if(section == MID2) while(IR.getIRDistance() > 15 && (abs(leftEncoder.getPulses()) < 100 || abs(rightEncoder.getPulses()) < 100)); 01429 01430 while(IR.getIRDistance() > 15 && (abs(leftEncoder.getPulses()) < 100 || abs(rightEncoder.getPulses()) < 100) && (leftEncoder.getPulses() != preLeft || rightEncoder.getPulses() != preRight)) { 01431 01432 if(IR.getIRDistance() > 38) break; 01433 01434 preLeft=leftEncoder.getPulses(); 01435 preRight=rightEncoder.getPulses(); 01436 wait_ms(200); 01437 } 01438 01439 } else {// RIGS 01440 while(abs(leftEncoder.getPulses()) < 220 || abs(rightEncoder.getPulses()) < 220); 01441 01442 // go backwards to line up with bump 01443 leftEncoder.reset(); 01444 rightEncoder.reset(); 01445 01446 motors.setMotor0Speed(-.15*127); //right 01447 motors.setMotor1Speed(-.15*127); //left 01448 while((abs(leftEncoder.getPulses()) < 100 || abs(rightEncoder.getPulses()) < 100) && preLeft!=0 ) { 01449 preLeft = leftEncoder.getPulses(); 01450 preRight = rightEncoder.getPulses(); 01451 wait_ms(200); 01452 if(leftEncoder.getPulses() == preLeft || rightEncoder.getPulses()== preRight) preLeft=preRight=0; 01453 } 01454 motors.stopBothMotors(127); 01455 motors.begin(); 01456 01457 return; 01458 } 01459 01460 motors.stopBothMotors(127); 01461 wait_ms(20); 01462 motors.begin(); 01463 01464 } 01465 void to_tools_section1(float* location, float ¤t) 01466 { 01467 slightMove(FORWARD,6700); 01468 current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2; 01469 01470 } 01471 01472 void to_tools_section2(float* location, float ¤t) 01473 { 01474 slightMove(FORWARD,3200); 01475 current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2; 01476 01477 } 01478 01479 void from_tools_section(float* location, float ¤t) 01480 { 01481 01482 //alignWithWall(TOOLS); 01483 //current-=4; 01484 01485 //slightMove(FORWARD,150); 01486 //current+=1; 01487 //pc.printf("align\r\n"); 01488 //wait_ms(200); 01489 01490 //wall_follow2(LEFT,FORWARD,MID, current); 01491 01492 01493 rangeFinderLeft.startMeas(); 01494 wait_ms(20); 01495 rangeFinderLeft.getMeas(range); 01496 01497 if(range < 15) { 01498 wall_follow2(LEFT,BACKWARD,MID, current,0); 01499 //pc.printf("wall follow\r\n"); 01500 location[0]= current - ((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); 01501 current= location[0]; 01502 //pc.printf("current %f \r\n",current); 01503 // go backwards 01504 //slightMove(BACKWARD,200); 01505 01506 wait_ms(100); 01507 leftTurn(); 01508 slightleft(); 01509 overBump(TOOLS); 01510 } else { 01511 //pc.printf("else greater than 20\r\n"); 01512 location[0]= current; 01513 leftTurn(); 01514 overBump(TOOLS); 01515 } 01516 01517 //pc.printf("First Wavegap = %f\r\n",location[0]); 01518 01519 } 01520 void tools_section(float* location, float ¤t) 01521 { 01522 wall_follow(LEFT,FORWARD, TOOLS); 01523 // current position in reference to the starting position 01524 current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2; 01525 01526 //////////////////////////////// determine tool 01527 wait(2); 01528 /////////////////////////////////////////////////////////////////////////////////////// 01529 // Move Forward 01530 slightMove(FORWARD, 100); 01531 01532 //////////////////////////////////////////Tool aquiring 01533 wait(2); 01534 //////////////////////////////////////////////////////////////////// After tool is aquired 01535 01536 //alignWithWall(TOOLS); 01537 //pc.printf("align\r\n"); 01538 //wait_ms(100); 01539 01540 //wall_follow2(LEFT,FORWARD,MID, current); 01541 //current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); 01542 01543 rangeFinderLeft.startMeas(); 01544 wait_ms(20); 01545 rangeFinderLeft.getMeas(range); 01546 01547 if(range < 20) { 01548 wall_follow2(LEFT,BACKWARD,TOOLS, current,0); 01549 //pc.printf("wall follow\r\n"); 01550 location[0]= current - ((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); 01551 current= location[0]; 01552 //pc.printf("current %f \r\n",current); 01553 // go backwards 01554 leftEncoder.reset(); 01555 rightEncoder.reset(); 01556 motors.setMotor0Speed(-MAX_SPEED); //right 01557 motors.setMotor1Speed(-MAX_SPEED); //left 01558 while(abs(leftEncoder.getPulses()) < 120 || abs(rightEncoder.getPulses())< 120); 01559 // hard stop 01560 01561 motors.stopBothMotors(127); 01562 01563 wait_ms(100); 01564 leftTurn(); 01565 overBump(TOOLS); 01566 } else { 01567 //pc.printf("else greater than 20\r\n"); 01568 location[0]= current; 01569 leftTurn(); 01570 overBump(TOOLS); 01571 } 01572 01573 //pc.printf("First Wavegap = %f\r\n",location[0]); 01574 } 01575 01576 void mid_section(float* location, float ¤t, int* direction) 01577 { 01578 if(IR.getIRDistance() > 38) { 01579 direction[0]= STRAIGHT; 01580 overBump(MID); 01581 return; 01582 } 01583 //pc.printf("before align with wall \r\n"); 01584 //alignWithWall(MID); 01585 //current-=4; 01586 //wait_ms(200); 01587 01588 //if(current > 20){ 01589 //alignWithWall(MID2); 01590 //current-=4; 01591 //} 01592 rightTurn(); 01593 //pc.printf("mid section current = %f\r\n",current); 01594 wall_follow2(LEFT,FORWARD,MID, current,0); 01595 current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); 01596 //pc.printf("after wf2 current = %f\r\n",current); 01597 01598 wait_ms(500); 01599 rangeFinderLeft.startMeas(); 01600 wait_ms(20); 01601 rangeFinderLeft.getMeas(range); 01602 01603 if(range > 20 ) { 01604 direction[0]= RIGHT; 01605 location[1]= current; 01606 wait_ms(300); 01607 slightMove(FORWARD,100); 01608 //current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); 01609 } else { 01610 direction[0]= LEFT; 01611 wall_follow2(LEFT,BACKWARD,MID,current,0); 01612 location[1]= current- ((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); 01613 current= location[1]; 01614 01615 if(location[1] < 18) { 01616 slightMove(FORWARD, 75); 01617 } 01618 //slightMove(BACKWARD,100); 01619 01620 } 01621 01622 wait_ms(200); 01623 //pc.printf("wavegap2 = %f\r\n",location[1]); 01624 //left turn 01625 motors.begin(); 01626 leftEncoder.reset(); 01627 rightEncoder.reset(); 01628 motors.setMotor0Speed(0.5*127);// right 01629 motors.setMotor1Speed(-0.5*127);// left 01630 while(abs(leftEncoder.getPulses())<1045 || rightEncoder.getPulses()<1045); 01631 motors.stopBothMotors(127); 01632 01633 wait_ms(100); 01634 01635 overBump(MID); 01636 01637 } 01638 01639 void mid_section2(float* location, float ¤t, int* direction) 01640 { 01641 //pc.printf("mid section 2\r\n"); 01642 01643 if(IR.getIRDistance() > 38) { 01644 direction[1]= STRAIGHT; 01645 overBump(RIGS); 01646 return; 01647 } 01648 01649 //alignWithWall(MID); 01650 wait_ms(100); 01651 01652 rightTurn(); 01653 slightright(); 01654 wait_ms(100); 01655 01656 01657 wall_follow2(LEFT,FORWARD,MID, current,0); 01658 current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); 01659 01660 wait_ms(500); 01661 01662 //pc.printf("midseection 2 after wf2 %f",current); 01663 rangeFinderLeft.startMeas(); 01664 wait_ms(20); 01665 rangeFinderLeft.getMeas(range); 01666 01667 if(range > 20 ) { 01668 direction[1]= RIGHT; 01669 location[2]= current; 01670 slightMove(FORWARD,50); 01671 current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); 01672 } else { 01673 direction[1]= LEFT; 01674 wall_follow2(LEFT,BACKWARD,MID,current,0); 01675 location[2]= current- ((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); 01676 current=location[2]; 01677 //slightMove(BACKWARD,100); 01678 } 01679 01680 //LEFT turn 01681 motors.begin(); 01682 leftEncoder.reset(); 01683 rightEncoder.reset(); 01684 motors.setMotor0Speed(0.5*127);// right 01685 motors.setMotor1Speed(-0.5*127);// left 01686 while(abs(leftEncoder.getPulses())<950 || rightEncoder.getPulses()<950); 01687 motors.stopBothMotors(127); 01688 01689 overBump(RIGS); 01690 //pc.printf("overbump rigs\r\n"); 01691 } 01692 01693 void rig_section(float* location, float ¤t, int* direction, int rig) 01694 { 01695 float loc; 01696 01697 if(rig == 1) loc= 15; 01698 else if(rig == 2) loc= 45; 01699 else loc = 75; 01700 01701 // Slight forward for turn 01702 slightMove(FORWARD,150); 01703 wait_ms(100); 01704 rightTurn(); 01705 //slightright(); 01706 01707 01708 if(current > loc) { 01709 //pc.printf("RIG section %f\r\n",current); 01710 wall_follow2(RIGHT, BACKWARD, RIGS, current, rig); 01711 current-=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); 01712 } else { 01713 //pc.printf("RIG section %f\r\n",current); 01714 wall_follow2(RIGHT, FORWARD, RIGS, current, rig); 01715 current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); 01716 } 01717 01718 alignWithWall(MID2); 01719 current-=4; 01720 wall_follow2(RIGHT, FORWARD, RIGS, current, rig); 01721 current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); 01722 slightMove(FORWARD, 75); 01723 01724 01725 01726 01727 01728 } 01729 01730 void tools_section_return(float* location, float ¤t) 01731 { 01732 if(location[0] > 16) { 01733 leftTurn(); 01734 wall_follow2(LEFT, BACKWARD, RETURN, location[0], 0); 01735 } 01736 motors.stopBothMotors(0); 01737 01738 } 01739 01740 void mid_section_return(float* location, float ¤t, int* direction) 01741 { 01742 if(direction[0] == RIGHT) { 01743 leftTurn(); 01744 alignWithWall(MID); 01745 wall_follow2(LEFT, BACKWARD, MID, current,0); 01746 current-=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); 01747 rightTurn(); 01748 } else if(direction[0] == LEFT) { 01749 leftTurn(); 01750 wall_follow2(RIGHT, FORWARD, MID, current,0); 01751 current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); 01752 rightTurn(); 01753 } 01754 //ELSE and GO FORWARD 01755 overBump(RIGS); 01756 } 01757 01758 void mid_section2_return(float* location, float ¤t, int* direction) 01759 { 01760 if(direction[1] == RIGHT) { 01761 leftTurn(); 01762 wall_follow2(LEFT, BACKWARD, MID, current,0); 01763 current-=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); 01764 rightTurn(); 01765 } else if(direction[1] == LEFT) { 01766 leftTurn(); 01767 wall_follow2(RIGHT, FORWARD, MID, current,0); 01768 current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); 01769 rightTurn(); 01770 } 01771 //ELSE and GO FORWARD 01772 overBump(MID); 01773 } 01774 01775 void rig_section_return(float* location, float ¤t, int* direction) 01776 { 01777 if(location[2] > current) { 01778 wall_follow2(RIGHT, FORWARD, MID, current,0); 01779 current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); 01780 } else { 01781 wall_follow2(RIGHT, BACKWARD, MID, current,0); 01782 current-=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); 01783 } 01784 rightTurn(); 01785 overBump(MID2); 01786 }
Generated on Wed Jul 20 2022 17:29:43 by
1.7.2