void Acquired_Taste(){//dump motor_off(); shake(); ao(); enable_servos(); servo_set(0, 450,4);//half way up motor(MOT_LEFT, -40); motor(MOT_RIGHT, -30);//back up msleep(2000); servo_set (0, 20, 2);//full up ao(); motor(MOT_LEFT, 40); motor(MOT_RIGHT, 30);//unjam if jammed msleep(1000); motor(MOT_LEFT, -40); motor(MOT_RIGHT, -30);//redump msleep(1000); ao(); disable_servos(); msleep(2000); enable_servos(); motor(MOT_LEFT, 60); motor(MOT_RIGHT, 30); servo_set (0, 700, 2);//forward and drop motor(MOT_LEFT, -40); motor(MOT_RIGHT, -30); servo_set (0, 800, 2); disable_servos(); }
void bringback2cube(){ //claw up motor(Motor_Up,Motor_up_speed); //zurück 1s drive(1000,-Drivespeed,-Drivespeed); //turn right 90 drive(970,Turnspeed,-Turnspeed); //vor to calibrate drive(1000,Drivespeed,Drivespeed); //back drive(500,-Drivespeed,-Drivespeed); //turn so little bit b4 cubes drive(1130,Turnspeed,-Turnspeed); //vor drive(900,Drivespeed,Drivespeed); //wait for claw up claw_up(); //camera fix camera_update(); //hide your cubes cube_is_near(); //back drive(1000,-Drivespeed,-Drivespeed); //turn left more than 90 idk drive(1330,-Turnspeed,Turnspeed); //start to down motor motor(Motor_Up,Motor_down_speed); //vor to calibrate drive(1000,Drivespeed,Drivespeed); //langsamer weil wackeldackel drive(2000,Drivespeed_middle+20,Drivespeed_middle+20); //back drive(400,-Drivespeed,-Drivespeed); //turn more than 90 lulz drive(980,-Turnspeed,Turnspeed); //light left and shit drive(5000,Drivespeed_middle*2,(Drivespeed_middle*2)-7); //light back and shit drive(1000,-Drivespeed_middle*2,(-Drivespeed_middle*2)+7); //wait for claw down while(!digital(Sensor_Down)){ if(seconds()>start+113){ claw_open(); disable_servos(); } } freeze(Motor_Up); msleep(2000); claw_open(); disable_servos(); }
void CalibrateGate() { int i; PromptFor("Disengage Gate"); enable_servos(); Gate(0); PromptFor("Re-engage at top"); printf("Testing..."); for (i = 3; i >= 0; i--) { printf("%d", i); sleep(1.0); } printf("\n"); for (i = 0; i < 5; i++) { Gate(1); sleep(5.0); Gate(0); sleep(1.0); } disable_servos(); }
void find_and_grab(int color){ int k; RIGHT(100); LEFT(100); msleep(2950); LFREEZE(); RFREEZE(); RIGHT(-50); LEFT(50); msleep(700); LFREEZE(); RFREEZE(); center_on_blob(90, RED, 0); for(k = 0; k <= 10; k + 1){ RIGHT(100); LEFT(100); msleep(200); center_on_blob(90, RED, 0); } /* while(1){ RIGHT(100); LEFT(100); msleep(200); center_on_blob(90, RED, 0); if(depth_y > Camera_y){ RIGHT(50); LEFT(-50); msleep(710); LFREEZE(); RFREEZE(); RIGHT(100); LEFT(100); msleep(800); LFREEZE(); RFREEZE(); } if(depth_y == OBJECT){ RIGHT(50); LEFT(-50); msleep(710); LFREEZE(); RFREEZE(); RIGHT(100); LEFT(100); msleep(1600); LFREEZE(); RFREEZE(); } if(depth_y != OBEJECT){ RIGHT(50); LEFT(-50); msleep(710); } }*/ disable_servos(); }
int main() { camera_open(); ssp(SORT_SERVO,START); ssp(RED_SERVO,RED_START); ssp(GREEN_SERVO,GREEN_START); wait_for_light(0); start(); shut_down_in(119); enable_servos(); msleep(5000); reset(); right(47,0); forward(35); right(10,0); backward(46); full_sort(); other_side(); ssp(GREEN_SERVO,GREEN_DUMP); msleep(1000); ssp(GREEN_SERVO,GREEN_START); msleep(250); now(); ao(); disable_servos(); }
void push_botguy() { set_servo_position(1, PUSH_BOTGUY_SERVO_DOWN); msleep(2000); // Garrett: Put the robot against the PVC. // It may have to BACK up a little bit // or maybe do a BIT of a turn before centering on Botguy. //drives to edge of starting box RIGHT(100); LEFT(100); msleep(400); LFREEZE(); RFREEZE(); // Garrett: Try replacing the next step with: // center_on_blob(80, RED, 1); // First get Aaron's help to make center_on_blob USE // its time parameter to be a FASTER center. //turns to knock cube out of the way RIGHT(-50); LEFT(50); msleep(2500); // Garrett: Add code to: // Drive to Botguy, knocking him down. // KEEP GOING, so that you are out of the way of the Create // and IN the way of their team's robots. // Talk to Aaron about moving, then recentering, // as the robot goes to Botguy and then through the gate. disable_servos(); }
void ServoTest() { int pos; enable_servos(); ServoRange(); disable_servos(); }
int main() { check_poms(); start(); //start timer //pom_collection(); disable_servos(); return 0; }
int main() { enable_servos(); collect_poms(); msleep(500); disable_servos(); return 0; }
void CompeteInit(int fPre) { enable_servos(); Gate(0); if (fPre) { sleep(0.25); disable_servos(); } fBall = 0; ipMotor = start_process(MotorDriver()); }
void kill() { printf("killing in %i.", wait * 1000); msleep(wait * 1000); printf("killing program.\n"); disable_servos(); create_drive_straight(0); motor(0, 0); motor(1, 0); motor(2, 0); motor(3, 0); exit(1); }
/*This program moves two servos. Servos need to be plugged into ports 0 and 3. The program presets servo 0 to position 150. Then the program waits for the black button to be pushed. Then it enables servos, servo 0 goes to position 150 and servo 3 goes to 1900. Then servo 3 goes to 1900. Finally servo 0 goes to 1900 and servo 3 goes to 150 and the program ends. See appendix for finding your servo?s range. */ int main() { set_servo_position(1,150); //preset port 0 to 150 printf("servo 0 at position 0\n"); printf("press black button to continue\n"); while(!black_button()) {} //wait for black button enable_servos(); //enable the servos sleep(1); //wait for servo to move set_servo_position(1,1900); //move port 3 to 1900 sleep(1); //wait for servo to move set_servo_position(1,1900); //move port 0 to 1900 set_servo_position(1,150); //move port 3 to 150 sleep(3); //wait for servos to move disable_servos(); //power down servos }
void wait_for_kill(void) { while (!side_button()) {} alloff(); motor(0, 0); motor(1, 0); motor(2, 0); motor(3, 0); alloff(); disable_servos(); create_drive_direct(0, 0); create_stop(); create_disconnect(); exit(0); }
void shake() { off(REVOLVE_MOTOR); ao(); enable_servos(); int i; for(i=0; i<20; i++){ ssp(0,1333); msleep(170); ssp(0,1260); msleep(170); } disable_servos(); motor(REVOLVE_MOTOR, 100); }
/******** move the servo from 700 to 2048 *****************************************************************/ int main() { // preset servo 1 position set_servo_position(1,700); printf("Press B to stop"); enable_servos(); // turn on servos msleep(2000); // pause while it moves and user reads screen for 2 seconds while((b_button()==0)) // while the button is not pressed { // move servo 1 in steps of 1348 set_servo_position(1,get_servo_position(1)+ 1348); printf("servo at %d\n", get_servo_position(1)); msleep(200); // pause before next move while((!a_button()) && (!b_button())) {} } disable_servos(); printf("Robot is done\n"); return 0; }
void push_botguy(){ set_servo_position(1,PUSH_BOTGUY_SERVO_DOWN); msleep(2000); //drives to edge of starting box RIGHT(100); LEFT(100); msleep(400); LFREEZE(); RFREEZE(); //turns to knock cube out of the way RIGHT(-50); LEFT(50); msleep(2500); disable_servos(); }
int main() { printf("In Drivepath test\n"); //light_start(0); shut_down_in(119.5); arm_drive(); claw_open(); bin_down(); enable_servos(); //forward(100); forward(10); int green = 0; //whether green has been collected int red = 0; //whether red has been collected start(); //start timer int pile1Result = pom_collection(); backward(pom_collection_turn * 5); pom_collection_turn = 0; //reset turn if(pile1Result == 0) { green = 1; printf("Collecting first pile of green poms\n"); } else if(pile1Result == 1) { red = 1; printf("Collecting first pile of red poms\n"); } //backward(5); left(15, ks/2); off(MOT_RIGHT); off(MOT_LEFT); collect_poms(); collect_poms(); msleep(1000); disable_servos(); ao(); return 0; }
int main() { printf("starting connor\n"); enable_servo(1); set_servo_position(1,100); //disable_servo(1) create_connect(); while (! digital(11)) { printf("%d\n", digital(11)); create_drive_direct(-150,-150); } printf("starting connor\n"); set_servo_position(1,1400); while (get_create_lbump() == 0 && get_create_rbump() == 0) { create_drive_direct(150, 150); // drive straight at 150mm/s } disable_servos(); }
void dance(){ int i=0; enable_servos(); while(1){ if(i%6<3){ motor(MOT_LEFT, 40); motor(MOT_RIGHT, 30); }else{ motor(MOT_LEFT, -40); motor(MOT_RIGHT, -30); } ssp(0,1133); msleep(200); ssp(0,1060); msleep(200); now(); i++; } disable_servos(); }
void _shut_down_task() { int i,j; msleep(_shut_down_delay); create_stop(); ao(); //disable_servos(); // delete this line if you want your servos to freeze but remain powered at the end Bb_Game_Over= 1; printf("Game over"); i= 0; for (j= 256; j <= 1024; j+=256) { // hog_processor(); while (i < j) { kill_process(_shut_down_pid+(++i)); kill_process(_shut_down_pid-i); } ao(); disable_servos(); // delete this line if you want your servos to freeze but remain powered at the end } printf(".\n"); }
int main() { enable_servos(); //enable servos set_servo_position(2,1000); //close claw,grab blocks sleep(1.0); //wait motor(1,100); //lift blocks sleep(2.0); //wait set_servo_position(2,2047); //open claw, drop blocks disable_servos(); //disable servos //variable declarations go here. // uncomment the line below anf put 1s in the appropriate places if using floating sensors (ET and sonar) //set_each_analog_state(0,0,0,0,0,0,0,0);// This line sets analog ports to be pullup (0) or floating (1) // uncomment the line below to have the rest of the program wait for the starting light to turn on //wait_for_light(port_number);//replace "port_number" with the analog port number into which the light sensor is connected // uncomment the line below to have a function run for a specified amount of seconds //run_for(num_secs, function_name);//replace num_secs with the amount of time to run and function_name with your function's name }
int main(){ /*===Start===*/ servo_setup(); printf("Press any button to start"); WAIT(a_button()||b_button()||c_button()); //printf("waiting 2 seconds"); sleep(2); /*===Gold One===*/ servo_drive_pos(); squareup(-80,1); forward(35,100); squareup(80,1); get_gold(); /*===Score Gold===*/ back(17,80); score_gold(); /*===Reposition===*/ left(90,0,100); squareup(80,3); /*===Botguy===*/ thread going=thread_create(arms_down_slow); thread_start(going); back(20,100); back(30,100); /*set_servo_position(ARM,ARM_UP); msleep(500); forward(34+5,100); back(5); set_servo_position(ARM,ARM_DOWN); msleep(500);*/ /*===Tail===*/ disable_servos(); ao(); }
/* Measure ticks per second */ void CalibrateSpeed() { long msStart; long msEnd; int cForward; int cBack; int cTicks = 500; int i; int fServo; for (fServo = 0; fServo < 2; fServo++) { if (fServo) { enable_servos(); Gate(0); printf("Servo: "); } else printf("No Servo: "); for (i = 0; i < 2; i++) { msStart = mseconds(); Move(cTicks,cTicks); msEnd = mseconds(); rgSpeed[i] = MultDiv(100, 1000, (int) (msEnd - msStart)); printf("%d ", rgSpeed[i]); cTicks = -cTicks; sleep(1.0); } if (fServo) disable_servos(); printf("\n"); StartPress(); } }
void main() { calibrate(); set_servo_position(CAM_SERVO,CAM_SERVO_POSITION_RIGHT); camera_open(LOW_RES); enable_servos(); set_analog_pullup(3,0); while(!b_button()){ claw_at_linefollowing(); do { camera_update(); linefolowing(); } while(get_object_count(green) ==0); //linefolowing bis er was sieht if(get_object_bbox(0,0).width>BLOB_SIZE_TAKE&&get_object_bbox(0,0).height>BLOB_SIZE_TAKE){ takepom(); } printf("kompletdurch \n"); } disable_servos(); stop(); }
void stop(){ ao(); camera_close(); disable_servos(); // stop all parts }
void seeding(){ msleep(WAIT_FOR_CREATE); //starts driving to the black line LEFT(100); RIGHT(100); while (1) { if (analog10(LEFTSENSOR) > LEFTDARK) { break; } } ao(); //turns on the black line msleep(1500); RIGHT(50); LEFT(-50); msleep(880); LFREEZE(); RFREEZE(); msleep(1500); //starts driving to end of black line LEFT(100); RIGHT(100); while(1) { if (analog10(RIGHTSENSOR) < RIGHTLIGHT && analog10(CENTERSENSOR) < CENTERLIGHT) { break; } } LFREEZE(); RFREEZE(); RIGHT(100); LEFT(100); msleep(200); LFREEZE(); RFREEZE(); //opens servo to score position set_servo_position(1, 2047); msleep(500); // turns to botguy RIGHT(-50); LEFT(50); msleep(760); LFREEZE(); RFREEZE(); center_on_blob(90, RED, 1); //drives to botguy RIGHT(100); LEFT(100); msleep(800); LFREEZE(); RFREEZE(); /*while(1){ RIGHT(100); LEFT(100); msleep(200); center_on_blob(90, RED, 0); }*/ //close the claw around botguy set_servo_position(1, 853); msleep(500); //backs up with botguy RIGHT(-100); LEFT(-100); msleep(1200); LFREEZE(); RFREEZE(); //turns to FMT RIGHT(50); LEFT(-50); msleep(1750); LFREEZE(); RFREEZE(); //drives to FMT RIGHT(100); LEFT(100); msleep(3900); LFREEZE(); RFREEZE(); disable_servos(); }
int main() { // INITIALIZE int x, y, color=0, pink=1; // set up for color channel 0 (green) int xvalue, yvalue; int greencentered = 0; int loop = 0; int centerx=77; // calibrate these number and enter manually int centery=63; int margin=8; int deltax = 0; int deltay = 0; int stepsizex; int stepsizey; int currpos; int s_time; light_start(L_SENSOR); // light start shut_down_in(119); camera_open(LOW_RES); enable_servos(); // enable servos set_servo_position(S_CATCHER,S_DOWN); msleep(500); set_servo_position(S_GATE,S_OPEN); msleep(500); // TRIBBLE PILE 1 //set_servo_position(S_GATE,S_OPEN); // open gate msleep(5000); set_servo_position(S_CATCHER,CATCHER_UP); right(28,ks/2); // right f_until_black(TOPHAT_RIGHT/*,TOPHAT_LEFT*/); // forward until right sensor sees black] forward(2); //printf("see black! time to close the gate\n"); set_servo_position(S_GATE,S_GAP); // close gate //f_until_white(TOPHAT_RIGHT); set_servo_position(CATCHERARM, CATCHER_UP); // camera sort s_time = curr_time(); while((curr_time()-s_time) < 14) { while (greencentered == 0) { camera_update(); // process the most recent image camera_update(); // process the most recent image camera_update(); // process the most recent image camera_update(); // process the most recent image camera_update(); // process the most recent image loop = loop + 1; printf("Update camera, loop %d\n", loop); //msleep(10); if (get_object_count(color) > 0) { xvalue = get_object_center(color,0).x; yvalue = get_object_center(color,0).y; deltax = abs(xvalue-centerx); deltay = abs(yvalue-centery); stepsizex = (deltax>10) ? 50: 20; stepsizey = (deltay>10) ? 50: 20; printf("x is %d, y is %d\n", xvalue, yvalue); greencentered = ((xvalue >= centerx-margin) & (xvalue <= centerx+margin)) && ((yvalue >= centery-margin) & (yvalue <= centery+margin)); if(greencentered == 1)//get x, y for the biggest blob the channel sees { printf("Biggest blob at (%d,%d)\n",xvalue,yvalue); msleep(300); set_servo_position(CATCHERARM, CATCHER_MIDWAY); msleep(300); set_servo_position(CATCHERARM, CATCHER_DOWN); //slow_servo(CATCHERARM,20,40,CATCHER_UP,CATCHER_DOWN); msleep(300); currpos=get_servo_position(CATCHERARM); printf("%d\n",currpos); /*if(CATCHER_MIDWAY>=currpos>CATCHER_DOWN) { currpos=get_servo_position(CATCHERARM); set_servo_position(CATCHERARM, currpos-INCREMENT); msleep(SLEEP_INCREMENT); currpos=get_servo_position(CATCHERARM); } if(currpos<CATCHER_MIDWAY) { currpos=get_servo_position(CATCHERARM); set_servo_position(CATCHERARM, currpos+INCREMENT); msleep(SLEEP_INCREMENT); currpos=get_servo_position(CATCHERARM); }*/ set_servo_position(CATCHERARM,CATCHER_UP); msleep(500); } else { if(xvalue <= centerx+margin) //moves right if senses value greater than 80 { motor(MOT_RIGHT,70); motor(MOT_LEFT,-70); msleep(stepsizex); motor(MOT_RIGHT,0); motor(MOT_LEFT,0); //msleep(10); printf("blob is too right\n"); } if(xvalue >= centerx-margin) // moves left if senses value less than 70 { motor(MOT_LEFT,70); motor(MOT_RIGHT,-70); msleep(stepsizex); motor(MOT_LEFT,0); motor(MOT_RIGHT,0); //msleep(10); printf("blob is too left\n"); } if ((xvalue > 67 /*centerx-margin*/) && (xvalue < 87 /*centerx+margin*/)) { if(yvalue > centery+margin) //moves forward if senses value greater than 43 { motor(MOT_LEFT,-50); motor(MOT_RIGHT,-50); msleep(stepsizey); motor(MOT_LEFT,0); motor(MOT_RIGHT,0); //msleep(10); printf("blob is too close\n"); } if(yvalue < centery-margin) // moves backwar if senses value less than 38 { motor(MOT_LEFT,50); motor(MOT_RIGHT,50); msleep(stepsizey); motor(MOT_LEFT,0); motor(MOT_RIGHT,0); //msleep(10); printf("blob is too far\n"); } } } } } } set_servo_position(S_GATE,S_CLOSE); while (analog10(2)<BLACK_SEN_THRESH) { motor(MOT_LEFT,-100); } // SCORE PILE 1 left(5,0); //backward(50); touch_back(TOUCH_SEN); forward(10); backward(10); forward(20); right(55,0); forward(35); backward(4); right(45,0); while (analog10(3)<BLACK_SEN_THRESH) { motor(MOT_LEFT, 100); motor(MOT_RIGHT, 100); } msleep(10); backward(24); left(42,0); backward(10); left(50,0); //backward(35); touch_back(TOUCH_SEN); forward(120.00); // TRIBBLE PILE 2 set_servo_position(S_GATE,S_OPEN); forward(40.00); set_servo_position(S_GATE,S_GAP); // camera sort s_time = curr_time(); while((curr_time()-s_time) < 14) //timer { while (greencentered == 0) { camera_update(); // process the most recent image camera_update(); // process the most recent image camera_update(); // process the most recent image camera_update(); // process the most recent image camera_update(); // process the most recent image loop = loop + 1; printf("Update camera, loop %d\n", loop); //msleep(10); if (get_object_count(color) > 0) { xvalue = get_object_center(color,0).x; yvalue = get_object_center(color,0).y; deltax = abs(xvalue-centerx); deltay = abs(yvalue-centery); stepsizex = (deltax>10) ? 50: 20; stepsizey = (deltay>10) ? 50: 20; printf("x is %d, y is %d\n", xvalue, yvalue); greencentered = ((xvalue >= centerx-margin) & (xvalue <= centerx+margin)) && ((yvalue >= centery-margin) & (yvalue <= centery+margin)); if(greencentered == 1)//get x, y for the biggest blob the channel sees { printf("Biggest blob at (%d,%d)\n",xvalue,yvalue); msleep(300); set_servo_position(CATCHERARM, CATCHER_MIDWAY); msleep(300); set_servo_position(CATCHERARM, CATCHER_DOWN); //slow_servo(CATCHERARM,20,40,CATCHER_UP,CATCHER_DOWN); msleep(300); currpos=get_servo_position(CATCHERARM); printf("%d\n",currpos); /*if(CATCHER_MIDWAY>=currpos>CATCHER_DOWN) { currpos=get_servo_position(CATCHERARM); set_servo_position(CATCHERARM, currpos-INCREMENT); msleep(SLEEP_INCREMENT); currpos=get_servo_position(CATCHERARM); } if(currpos<CATCHER_MIDWAY) { currpos=get_servo_position(CATCHERARM); set_servo_position(CATCHERARM, currpos+INCREMENT); msleep(SLEEP_INCREMENT); currpos=get_servo_position(CATCHERARM); }*/ set_servo_position(CATCHERARM,CATCHER_UP); msleep(500); } else { if(xvalue <= centerx+margin) //moves right if senses value greater than 80 { motor(MOT_RIGHT,70); motor(MOT_LEFT,-70); msleep(stepsizex); motor(MOT_RIGHT,0); motor(MOT_LEFT,0); //msleep(10); printf("blob is too right\n"); } if(xvalue >= centerx-margin) // moves left if senses value less than 70 { motor(MOT_LEFT,70); motor(MOT_RIGHT,-70); msleep(stepsizex); motor(MOT_LEFT,0); motor(MOT_RIGHT,0); //msleep(10); printf("blob is too left\n"); } if ((xvalue > 67 /*centerx-margin*/) && (xvalue < 87 /*centerx+margin*/)) { if(yvalue > centery+margin) //moves forward if senses value greater than 43 { motor(MOT_LEFT,-50); motor(MOT_RIGHT,-50); msleep(stepsizey); motor(MOT_LEFT,0); motor(MOT_RIGHT,0); //msleep(10); printf("blob is too close\n"); } if(yvalue < centery-margin) // moves backwar if senses value less than 38 { motor(MOT_LEFT,50); motor(MOT_RIGHT,50); msleep(stepsizey); motor(MOT_LEFT,0); motor(MOT_RIGHT,0); //msleep(10); printf("blob is too far\n"); } } } } } } set_servo_position(S_GATE,S_CLOSE); // SCORE PILE 2 right(55,0); forward(28); backward(12); right(55,0); touch_back(TOUCH_SEN); forward(200); right(50,0); touch_back(TOUCH_SEN); forward(15); left(50,0); while (analog10(3)<BLACK_SEN_THRESH) { motor(MOT_LEFT, 100); motor(MOT_RIGHT, 100); } backward(4); right(50,0); msleep(10); forward(24); right(42,0); touch_back(TOUCH_SEN); backward(4); // END ao(); disable_servos(); printf("done!"); }
void halt() { freeze_halt(); disable_servos(); }
void stop(){ ao(); camera_close(); disable_servos(); }
int main() { /* FIRST CALIBRATION: based on poms position adjust starting position */ printf("test 3.11, calibration value: \nclaw down : %d, \nright ang forward right: %d, \nright ang forward left : %d, \nback right angle %d ...\n", DOWN_SERVO, RIGHT_ANGLE_CLICKS_MRP_RIGHT, RIGHT_ANGLE_CLICKS_MRP_LEFT, RIGHT_ANGLE_CLICKS_BACK_MRP); double start_time = seconds(); enable_servos(); clear_motor_position_counter(RIGHT_MOTOR); clear_motor_position_counter(LEFT_MOTOR); wait_for_light(1); shut_down_in(118); clawUp(); //************************************** //* PART 1 : get first set of poms to lower storage area //* //************************************** moveBackward(1, NO_DEBUG); moveForwardHighSpeed(23, DEBUG); clawDown(); //moveForward(8,NO_DEBUG); //we have the Poms rightAngleBwd(RIGHT, NO_DEBUG); //bump against the upper storage area moveBackwardHighSpeed(15, NO_DEBUG); moveForwardHighSpeed(17, NO_DEBUG); rightAngleBwd(RIGHT, NO_DEBUG); //bump against upper PVC side moveBackwardHighSpeed(18, NO_DEBUG); //move towards dropping poms moveForward(1.5, NO_DEBUG); msleep(300); rightAngleFwd(LEFT, NO_DEBUG); msleep(300); clawUp(); //uses IR sensor to stop the forward movement moveForwardTilBlackLine(15, NO_DEBUG); printf("====> elapsed time end of part 1: %f\n", (seconds() - start_time)); //************************************** //* END OF PART 1 : get first set of poms to lower storage area //* //************************************** //************************************** //* PART 2 : get the blue cube to the corner //* //************************************** moveBackwardHighSpeed(25, NO_DEBUG); //recalibrate against top PVC next to botguy rightAngleBwd(LEFT, NO_DEBUG); moveBackwardHighSpeed(11, NO_DEBUG); // SECOND CALIBRATION: based on blackline/blue position moveForward(7.5, NO_DEBUG); rightAngleFwd(RIGHT, NO_DEBUG); //get the cube now //====>>>>> may have to be adjust the day of competition based on position of 2nd //====>>>>> set of poms moveForwardHighSpeed(15, NO_DEBUG); //====>>>>> clawAlmostDownCube(); twentyTwoAngleFwd(LEFT, NO_DEBUG); clawUp(); //====>>>>> calibration to get to the corner 22.5 fwd, 22.5 more forward, aiming at bottom left hand corner moveForwardHighSpeed(2.5, NO_DEBUG); twentyTwoAngleFwd(LEFT, NO_DEBUG); //clawAlmostDownCube(); moveForwardHighSpeed(30, NO_DEBUG); //====>>>>> //moveForwardHighSpeed(2, NO_DEBUG); //====>>>>> moveBackwardHighSpeed(9, NO_DEBUG); fortyFiveAngleFwd(LEFT, NO_DEBUG); printf("====> elapsed time end of part 2: %f\n", (seconds() - start_time)); //************************************** //* END OF PART 2 : get the blue cube in the corner //* //************************************** //************************************** //* PART 3 : prepare to get second set of poms //* //************************************** //recalibrate against side PVC rightAngleBwd(RIGHT, NO_DEBUG); moveBackwardHighSpeed(25, NO_DEBUG); moveForward(5, NO_DEBUG); //calibrating with top PVC rightAngleBwd(LEFT,NO_DEBUG); moveBackwardHighSpeed(23,NO_DEBUG); // THIRD CALIBRATION: based on second set of Poms //below is the number of inches from the side PVC // moveForwardHighSpeed(10, NO_DEBUG); rightAngleBwd(RIGHT, NO_DEBUG); //recalibrate with side pvc moveBackwardHighSpeed(5,NO_DEBUG); printf("====> elapsed time end of part 3: %f\n", (seconds() - start_time)); //************************************** //* END OF PART 3 : prepare to get second set of poms //* //************************************** //************************************** //* PART 4 : bring second set of poms home //* //************************************** //grabs the poms, bring down claw and go home moveForwardHighSpeed(12.5,NO_DEBUG); clawDown(); moveForwardHighSpeed(59.5, NO_DEBUG); //here is the code to recalibrate one more time... but do we have enough time? /*//recalibrate against top PVC rightAngleBwd(LEFT,NO_DEBUG); moveBackwardHighSpeed(10, NO_DEBUG); //move towards dropping poms moveForwardHighSpeed(10, NO_DEBUG); //msleep(500); printf("==> moving righ angle bwd\n"); rightAngleFwd(LEFT, NO_DEBUG); */ //recalibrate against top PVC //moveForwardHighSpeed(41,NO_DEBUG); //last calibration before dropping second set of poms rightAngleBwd(LEFT,NO_DEBUG); moveBackwardHighSpeed(14, NO_DEBUG); //move towards dropping poms moveForwardSlow(1.5, NO_DEBUG); msleep(300); rightAngleFwd(LEFT, NO_DEBUG); msleep(300); clawUp(); moveForwardTilBlackLine(15, DEBUG); //************************************** //* END OF PART 4 : bring second set of poms home //* //************************************** printf("====> elapsed time: %f\n", (seconds() - start_time)); ao(); disable_servos(); return 0; }