void Start() //Misc. set up functions { create_connect(); set_create_total_angle(0); //wait_for_light(LightPort); shut_down_in(RunTime); //TEST THIS }
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 Start (void) { wait_for_light(LightPort); shut_down_in (117); //TEST THIS create_connect(); set_create_total_angle(0); }
void StartUp() { enable_servos(); set_servo_position(ARM_PORT, ARM_DOWN); set_servo_position(CARDBOARD_PORT, CARDBOARD_START); set_servo_position(2, 800); msleep(300); wait_for_light(5); shut_down_in(119); }
int main() { printf("Mini Bot!\n"); enable_servos(); set_servo_position(ARM_SERVO, ARM_UP); printf("Waiting for light!\n"); while (analog(0) > 700 && a_button() == 0) { msleep(10); } shut_down_in(118.0); msleep (25000); go(90, 90); msleep(6700); stop(); msleep(100); printf("stopped\n"); set_servo_position(ARM_SERVO, ARM_DOWN); printf("arm down\n"); go(-80,-80); msleep(500); stop(); go(-90, 90); msleep(1150); stop(); go(90, 90); msleep(2500); stop(); set_servo_position(ARM_SERVO, ARM_UP); go(90, 90); msleep(2500); stop(); msleep(1000000); return 0; }
int main() { wait_for_light(LIGHT_SENS); //shake(); //startingT(); start(); shut_down_in(119); Abyssal_Voyage(); Chomp(); now(); Acquired_Taste(); Devour(); Tougue_Lash(); Acquired_Taste(); //dance(); now(); }
void start(int servo_position) { int k; camera_open(); for (k = 0; k < 10; k = k+1) { camera_update(); msleep(200); } set_servo_position(1, servo_position); enable_servos(); msleep(1000); wait_for_light(7); shut_down_in(115); }
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; }
// Created on Sat April 13 2013 // This is a transport move test. int main() { wait_for_light(3); // start the robot after the light is turned on shut_down_in(120); // shutdown the robot in 2 Minutes enable_servo(); printf("Hello, World!\n"); set_servo_position(1, 0); motor(1,100); // move the right wheel motor(3,100); // move the left wheel msleep(3500); // move for 3.5 seconds ao(); set_servo_position(1, 2048); // kick bot guy out motor(1,-100); motor(3,-100); disable_servo(); return 0; }
//start 48500 //down 58000 void main() { set_servo_position(Servo_Back,Servo_Back_Up); set_servo_position(Servo_Left,Servo_Left_Closed); set_servo_position(Servo_Right,Servo_Right_Closed); enable_servos(); calibrate(); //wait for light //printf("wait for light oida"); //set_b_button_text("I am the Twilight"); //while(!b_button()){} wait_for_light(Sensor_Light); //shutdown stuff shut_down_in(115); start=seconds(); //start position for create start_position(); //we see everything camera_open(); //drive in front of the cubes take_position(); //watch for cubes cube_is_near(); ao(); //bring the first cube to the pipes bringback(); //take the 2nd cube and bring them to the pipes bringback2cube(); }
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; }
void main (int argc, char ** argv) { int loop_done = 0; int task_A_done = 0; int task_B_done = 0; int angle = 0; int dangle = 0; int angle0 = 0; int distance0 = 0; int distance = 0 ; int ddistance = 0; // float lf_args[20]; // read inputs // printf("Args: %d\n", argc); // for (i = 2; i < argc; i++) // { // scanf("%lf%, argv[i] // } //get_create_total_ angle (.1); //create_drive_direct (left speed, right speed)\ //move_to_position (motor #, motor power(-for backwards), end position) //get_motor_done (motor#) //clear_motor_position_counter (motor#) //msleep (seconds) //i'll probably use this often //to start by light, do: //while(analog(1) > 200) if (0) { //we have seen the light msleep(100); if(analog(1) < 800){ shut_down_in(119); } } create_connect (); clear_motor_position_counter (0); enable_servo (0); set_servo_position (0 ,1300); //set_servo_position (1800); // ------------------------------------------------------------------------ //step:1 aim the arm to knock over the box //raise arm printf("-----Step 1-----\n"); move_motor (0, -900, -1200); // swing arm to hit the box, then get back to position spin (50, -50, 20); spin (-50, 50, 20); create_stop (); // ------------------------------------------------------------------------ //step:2 knock over box and turn //lift arm //turn 90 printf("-----Step 2-----\n"); move_to_position (0, -900, -2600); msleep (900); //turn create in a circular position and raise arm so it doesn't hit anything angle0 = get_create_total_angle (.05); angle = angle0; dangle = abs (angle0 - angle); printf("a0: %d, a: %d, da: %d, d: %d\n", angle0, get_create_total_angle (.05), dangle , task_A_done); create_drive_direct (-178,-648); while(! loop_done) { if (dangle >= 155) { task_A_done = 1; create_stop (); } else if (dangle >= 125) { create_drive_direct (-120, -181); } if (get_motor_done (0)) { task_B_done = 1; } if (task_A_done && task_B_done) { loop_done = 1; } msleep(100); angle = get_create_total_angle (.05); dangle = abs (angle0 - angle); printf("a0: %d, a: %d, da: %d, d: %d\n", angle0, get_create_total_angle (.05), dangle , task_A_done); } create_stop (); // ------------------------------------------------------------------------ //step:4 aim claw to point of botguy //turn 20 //lower arm printf("-----Step 4-----\n"); //--tweaking position so it gets in just the right position move_to_position (0, 400, -900); move (20, 20, 50); set_servo_position (0 ,400); create_drive_straight (-30); move_motor (0, 600, -300); create_drive_straight (-38);// at this point, botguy is in his palm //can't remember function to program msleep (3000); loop_done = 0; if (! loop_done) { create_stop (); if (! digital (10)) { move_motor (0, 400, -900); spin (20, -20, 5); move_motor (0, 600, -300); create_stop (); loop_done = 1; } else { loop_done = 1; } } // ------------------------------------------------------------------------ //step:5 grab botguy and lift him up (not complete) printf("-----Step 5-----\n"); move (-10, -10, 10); msleep (1300); //spin (60, -60, 5); //close arm and grab botguy set_servo_position (0, 1510); msleep (400); move_to_position (0, -1000, -3000); //wait for him to finish msleep (5000); // ------------------------------------------------------------------------ //step:6 move backwards till it's in position printf("-----Step 6-----\n"); //back up in an arc move (130, 280, 780); //move towards the center and lower drop height move_to_position (0, -900, -500); create_drive_straight (-20); msleep (3000); //drop botguy in transport set_servo_position (0, 200); create_stop (); //start moving towards the pipe move_motor (0, 500, -1200); spin (50, -50, 70); move (50, 50, 100); spin (-50, 50, 70); /* if (! loop_done) { if (get_object_count(2) == 1) { create_stop (); spin(-10, 10, 5); create_stop (); move(20, 20, 20); create_stop (); set_servo_position (0, 1400); loop_done = 1; } } loop_done = 0; spin (10, 10, 5); set_servo_position (0, 200); */ /* spin (50, -50, 85); move_motor (0, 400, -400); move (-100, -100, 50); loop_done = 0; //close arm on pipe set_servo_position (0, 1510); //turn arm to face the pole spin (-50, 50, 50); //up, forward, then drop pipe move_motor (0, 400, -2300); move (-100, -100, 100); set_servo_position (0, 200); ao (); */ create_disconnect (); }
int main() { double start = 0.0; // initialize servo, camera, etc here?. // if you are using a Create you should uncomment the next line //create_connect(); enable_servos (); set_servo_position(3, ARM_UP); //goto GO_DOWN_CAVE; wait_for_light(2); // change the port number to match where your robot shut_down_in(119); // shut off the motors and stop the Create after 119 seconds // note that shut_down_in should immediately follow wait_for_light!! printf("My program should score more points than this template!\n"); // replace printf above with your code //create_disconnect(); // Created on Mon March 2 2015 int a1; int a2; //turn on robot motor(0,-700); motor(2,-700); msleep(700); #if 0 //go forward motor(0,400); motor(2,400); msleep(1000); //turn to aline motor(0, -100); motor(2, 100); msleep(500); //aline robot motor(0,-100); motor(2,-100); msleep(2000); go(0, 0); msleep(1500); #else //go forward motor(0,400); motor(2,400); msleep(400); //turn to aline motor(0,0); motor(2,200); msleep(1300); //aline robot motor(0,-100); motor(2,-100); msleep(2000); #endif // ao(); return 0; //go forwad motor(0,85); motor(2,85); //tweek if wheel goes sideways //msleep(6000); printf("before 3000 delay\n"); msleep (1500); set_servo_position(3, ARM_UP_PEG); printf("after 3000 delay\n"); msleep(400); printf("%d\n",analog(1)); start=seconds (); while ((a1 = analog(1)) <500) { // printf("%d\n",a1); //msleep(5); if(seconds()-start>3.5){ printf("time_out\n"); break; } } printf("Found black %d\n", a1); //turn to go into middle turn_right(); //go forward motor(0,60); motor(2,60); printf("go forward\n"); //msleep(2000); while ((a1 = analog(1)) <600 || (a2 = analog(3)) <600) { // printf("%4d %4d\n",a1, a2); //msleep(5); if (seconds()-start >10.5) { break; } } printf("%4d %4d\n",a1, a2); //turn in cave motor(0,0); motor(2,0); turn_right(); set_servo_position(3, ARM_UP); //go to pick up cubes //motor(0,75); //motor(2,0); printf("turn right\n"); //msleep(1500); //line follow start=seconds (); while(1){ if(seconds()-start>4.9){ break; } if (analog(1)>500) { motor(0,80); motor(2,100); }else if (analog(3)>500) { motor(0,100); motor(2,80); }else { motor(0,100); motor(2,100); } } //motor(0,85); //motor(2,85); //msleep(5500); //put arm downto grab GO_DOWN_CAVE: go(0,0); set_servo_position(3, ARM_DOWN); msleep(500); //begining of picking up poms go(75,75); msleep(2500); go_for(-50,-90, 1000); // back up/turn/pull pom-poms go_for(90, -90, 500); go_for(-100, -90, 600); set_servo_position(3, ARM_UP); go_for(100, 100, 2000); go_for(-100, -100,2000); set_servo_position(3, ARM_DOWN); msleep(700); go_for(100,100,1000); go(-10, -10); set_servo_position(3, ARM_DOWN); msleep(100); set_servo_position(3, ARM_DOWN + 50); msleep(100); set_servo_position(3, ARM_DOWN + 100); msleep(100); set_servo_position(3, ARM_DOWN + 150); msleep(100); set_servo_position(3, ARM_DOWN + 200); msleep(100); //set_servo_position(3, ARM_DOWN + 250); //msleep(100); //set_servo_position(3, ARM_DOWN + 400); //msleep(100); //push the poms over the top of the PVC go_for(20, 20, 1000); //StOp go(0,0); //wiggle the arm up and down so the poms fall off set_servo_position(3, ARM_DOWN+350); msleep(250); set_servo_position(3, ARM_DOWN+300); msleep(250); set_servo_position(3, ARM_DOWN+350); msleep(250); // Raise the arm set_servo_position(3, ARM_UP); msleep(300); go_for(0, -100, 1000); //line follow start=seconds (); while(1){ if(seconds()-start>1.5){ break; } if (analog(1)>500) { motor(0,50); motor(2,80); }else if (analog(3)>500) { motor(0,80); motor(2,50); }else { motor(0,80); //why is this 60 and 80 motor(2,60); } } //line follow start=seconds (); while(1){ if(seconds()-start>9.1){ break; } if (analog(1)>500) { motor(0,80); motor(2,100); }else if (analog(3)>500) { motor(0,100); motor(2,80); }else { motor(0,100); motor(2,100); } } set_servo_position(3,ARM_DOWN); go_for(100, 100, 3500); // Ram into PVC at the end of the cave #if 1 go_for(-50, -90, 1000); // Back up with poms go_for(90, -90, 1000); set_servo_position(3, ARM_DOWNISH); // TODO: Run back across the cave to the good end start=seconds (); while(1){ if(seconds()-start>1.5){ break; } if (analog(1)>500) { motor(0,50); motor(2,80); }else if (analog(3)>500) { motor(0,80); motor(2,50); }else { motor(0,80); //why is this 60 and 80 motor(2,60); } } start=seconds (); while(1){ if(seconds()-start>8.5){ // change from 8 sec break; } if (analog(1)>500) { motor(0,80); motor(2,100); }else if (analog(3)>500) { motor(0,100); motor(2,80); }else { motor(0,100); motor(2,100); } } set_servo_position(3, ARM_DOWN); go(75,75); msleep(2500); go_for(-50,-90, 1000); // back up/turn/pull pom-poms go_for(90, -90, 500); go_for(-100, -90, 600); set_servo_position(3, ARM_UP); go_for(100, 100, 2000); go_for(-100, -100,2000); set_servo_position(3, ARM_DOWN); msleep(700); go_for(100,100,1000); go(-10, -10); set_servo_position(3, ARM_DOWN); msleep(100); set_servo_position(3, ARM_DOWN + 50); msleep(100); set_servo_position(3, ARM_DOWN + 100); msleep(100); set_servo_position(3, ARM_DOWN + 150); msleep(100); set_servo_position(3, ARM_DOWN + 200); msleep(100); //set_servo_position(3, ARM_DOWN + 250); //msleep(100); //set_servo_position(3, ARM_DOWN + 400); //msleep(100); //push the poms over the top of the PVC go_for(20, 20, 1000); //StOp go(0,0); //wiggle the arm up and down so the poms fall off set_servo_position(3, ARM_DOWN+350); msleep(250); set_servo_position(3, ARM_DOWN+300); msleep(250); set_servo_position(3, ARM_DOWN+350); msleep(250); //backs up to push poms second timeb set_servo_position(3, ARM_UP); go_for(-100, -100,2000); //push poms second time go_for(100, 100, 2000); go_for(-100, -100,2000); set_servo_position(3, ARM_DOWN); msleep(700); go_for(100,100,1000); go(-10, -10); set_servo_position(3, ARM_DOWN); msleep(100); set_servo_position(3, ARM_DOWN + 50); msleep(100); set_servo_position(3, ARM_DOWN + 100); msleep(100); set_servo_position(3, ARM_DOWN + 150); msleep(100); set_servo_position(3, ARM_DOWN + 200); msleep(100); //set_servo_position(3, ARM_DOWN + 250); //msleep(100); //set_servo_position(3, ARM_DOWN + 400); //msleep(100); //push the poms over the top of the PVC go_for(20, 20, 1000); //StOp go(0,0); //wiggle the arm up and down so the poms fall off set_servo_position(3, ARM_DOWN+350); msleep(250); set_servo_position(3, ARM_DOWN+300); msleep(250); set_servo_position(3, ARM_DOWN+350); msleep(250); //motor(0,-75); //motor(2,-75); //msleep(8500); //turn_left(); //go_for(100, 100, 1800); //turn_left(); //go_for(100, 100, 5000); #endif ao(); return 0; }
int main() { double start = 0.0; // initialize servo, camera, etc here?. // if you are using a Create you should uncomment the next line //create_connect(); enable_servos (); set_servo_position(3, ARM_UP); int a1; int a2; //goto START_DOWN_CAVE; //lights #if 0 wait_for_light(2); // change the port number to match where your robot shut_down_in(119); // shut off the motors and stop the Create after 119 seconds // note that shut_down_in should immediately follow wait_for_light!! printf("My program should score more points than this template!\n"); // replace printf above with your code #endif //create_disconnect(); //go forward to line follow motor(0,700); motor(2,700); msleep(1000); while ((a1 = analog(1)) <500 && (a2 = analog(3)) <500) { printf("%5d %5d\n",a1,a2); //msleep(5); if(seconds()-start>6.5){ printf("time_out\n"); break; } } printf("Found black %d\n", a1); // //line follow start=seconds (); while(1){ a1 =analog (1); a2 =analog (3); printf("cave%5d %5d\n",a1,a2); if(seconds()-start>4.5){ break; } if ((a1<500 && a2<500) || (a1>500 && a2>500)) { motor(0,90 * factor); motor(2,56 * factor); } else if (a1<500) { motor(0,90 * 1.00 * factor); motor(2,75 * 0.80 * factor); }else { motor(0,90 * 0.80 * factor); motor(2,75 * 1.00 * factor); } } return 0; }
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!"); }
int main() { //light_start(0); shut_down_in(119.5); arm_up(); claw_open(); bin_down(); enable_servos(); collect_poms(); printf("Collected Initial Poms in base\n"); /* forward(70); msleep(500); left(35, 0); msleep(300); //face 1st pile forward(45); pomPileOne(); */ forward(47); msleep(500); left(30, 0); //face 1st pile forward(60); //needs to be exact or else claw will hit table divider pomPileOne(); /* //4/9/16 Test Code forward(20); msleep(500); left(15, 0); msleep(300); //face 1st pile forward(80); pomPileOne(); */ //go to pile 2 multforward(45, 0.50); msleep(100); forward(15); msleep(300); left(85, ks/2); msleep(300); forward(10); pomPileTwo(); if(green == 1 && red == 1) { printf("Collected all\n"); //go to bin right(60, ks/2); forward(100); right(100, ks/2); msleep(500); bin_mid(); msleep(500); bin_dump(); } else { //do pom pile three only if robot does not have both one red and one green pomPileThree(); //go to bin left(60, ks/2); forward(60); } msleep(2000); disable_servos(); ao(); return 0; }
int main() { printf("version 1.9.5\n"); shut_down_in(0.010*1000); int start=seconds();//starting time for two minutes enable_servo(0); set_servo_position(0,2047); straight(1.8, 170);//gets in front of transport rightC(0.28, 100); leftC(0.28, -100); straight(1.65, 170); leftC(0.44, 100); printf("completed first dead reckoning\n"); set_analog_pullup(ETport, 0); //until distance is 5 centimeter /* int claw_threshold = 13; while(analog10(ETport)>claw_threshold){ back(0.1, -10); msleep(1000); printf("move once\n"); printf("%i\n", analog10(ETport)); }*/ back(0.25, -20);//pushes against transport msleep(2000); set_servo_position(0, 500);//puts claw on transport msleep(1000); printf("done with attaching to transport\n"); while(digital(8)==0)//goes forward with transport until it hits the wall { straight(0.1, 100); msleep(10); } printf("at wall\n"); back(0.25, -50);//backs up from wall to deposit transport set_servo_position(0, 2047);//raises claw msleep(1500); while(digital(8)==0){//goes forward until it hits wall for second time straight(0.1, 100); msleep(10); } printf("at wall 2\n"); back(0.35, -100);//goes back and turns left in order to be oriented with left inner wall leftC(0.6, 142); int bump_counter=0; while(digital(8)==0){//hits side wall straight(0.1, 100); msleep(10); } bump_counter++;//has one bump int i=0; while(i<10){//if hits side wall, pause stop(0.2); i++; } back(0.25, -100); leftC(0.46, 100); camera_open(LOW_RES); armUp(); clawOpen(); double start_wait=seconds(); while((seconds()-start_wait)<=25){//wait for poms or for 25 seconds int i=0; while(i<10){//picks latest image from the buffer camera_update(); i++; } if(get_object_count(chan)>0){//if camera sees objects, skip the 25 seconds and go onto picking up stuff break; }else{//if camera doesn't see any objects, keep waiting until 25 seconds is up stop(0.1); } } int x=get_object_center(chan, 0).x;//declares global unchanging variable for the x location of the largest object while((seconds()-start)<=120){//while there is still time left int area=get_object_area(chan, 0);//creates local changing variable. this is th area of the largest object camera sees if(area<=600){//if the object is small enough(far enough), navigate towards object. 600 is threshold int i=0; while(i<10){//buffer updating camera_update(); i++; } navigate_poms(x);//similar to line followig function; gets to poms. } armDown(); clawClose(); armUp(); /* leftF(0.5, 100, 80);//turn and go forward until transport is contacted while(digital(8)==0){ straight(0.1, 100);*/ } /* back(1.5, 100); leftC(0.44, 100); while(digital(8)==0 && digital(9)==1){ straight(0.1, 100); msleep(10); if(digital(9)==0){ rightF(0.1, 100, 40); left(0.1, 100); msleep(10); } } straight(2, 200); rightC(0.44, 100); straight(1, 80); rightC(0.92, 100); while(analog10(ETport<=600)){ msleep(10); } msleep(10000); straight(1, 80); leftC(0.44, 100); straight(2, 200); */ return 0; }
int main() { enable_servos(); printf("Houston we have ignition\n"); set_servo_position(0, 950); set_servo_position(1, 60); msleep (1000); while (analog(0) > 700 && a_button() == 0) { msleep(10); } shut_down_in(118.0); msleep(2000); go(40, 0); msleep(3000); stop(); set_servo_position(0, 1350); msleep(1000); set_servo_position(1, 1400); msleep(1000); set_servo_position(0,600); stop(); msleep(1000); go (50,50); msleep (4900); stop (); set_servo_position(0, 498); msleep(2500); go (70, 40); msleep(2600); stop (); go (35, -30); msleep(1000); go (15, 15); msleep(1500); stop (); set_servo_position(1, 600); go (-40, -40); msleep(4000); stop(); go (-35, 30); msleep(1650); stop(); go (40, 40); msleep (1950); set_servo_position(1, 600); return 0; }
int main(int argc, char** argv) { enable_servo(kServoPortSorter); set_servo_position(kServoPortSorter, kServoPositionSorterCenter); clear_motor_position_counter(kMotorPortBayLeft); clear_motor_position_counter(kMotorPortBayRight); create_connect(); camera_open(LOW_RES); scanf("%s", NULL); printf("waiting for light\n"); while (analog(0) > 200) {} shut_down_in(700000); motor(kMotorPortBayLeft, 10); motor(kMotorPortBayRight, 10); while (get_motor_position_counter(kMotorPortBayLeft) < 230 || get_motor_position_counter(kMotorPortBayRight) < 230) { printf("pos: %i/%i\n", get_motor_position_counter(kMotorPortBayLeft), get_motor_position_counter(kMotorPortBayRight)); } motor(kMotorPortBayLeft, 0); motor(kMotorPortBayLeft, 0); // wait_for_side_button(); msleep(6000); create_drive_straight(200); msleep(1850); create_spin_CCW(200); msleep(750); create_drive_straight(-200); msleep(1500); create_spin_CW(200); msleep(1750); create_drive_straight(200); while (!get_create_lbump() && !get_create_rbump()) {} create_drive_straight(-150); msleep(900); create_spin_CCW(200); msleep(950); create_drive_straight(150); while (!get_create_lbump() && !get_create_rbump()) {} create_stop(); msleep(10000); thread all_off = thread_create(wait_for_kill); thread_start(all_off); raise_bay(); create_spin_CW(100); msleep(500); create_drive_straight(-100); msleep(1000); create_stop(); thread jiggle_c = thread_create(jiggle_create); thread_start(jiggle_c); thread sort_b = thread_create(sort_balls); thread_start(sort_b); msleep(33000); while (true) {} /*thread_destroy(jiggle_c); create_stop(); lower_bay(); create_drive_straight(100); while (!get_create_lbump() && !get_create_rbump()) {} create_stop(); msleep(10000); raise_bay(); create_spin_CW(100); msleep(500); create_drive_straight(-100); msleep(1000); create_stop(); thread_create(jiggle_create); thread_start(jiggle_c); thread_create(sort_balls); while (true) {} camera_close();*/ return 0; }
int main() { shut_down_in(30); countdown(); }
int main() { //waiting for light to start the program. wait_for_light(0); shut_down_in(120.0); //making sure the claw and arm are in position. set_servo_position(1, 0);//ensures that the arm is in the right position sleep(0.6);//0.6 is how many seconds the motors are turning on their input values set_servo_position(0, 624);//ensures that the claw is in the right position sleep(0.6);//0.6 is how many seconds the motors are turning on their input values //this section allows the robot to come out of the starting box, turn, drive forward and put itself in position to grab the polyps. mav(0, -1000);//-600 is velocity mav(2, -975);//-600 is velocity sleep(5.0);//7.9 is how many seconds the motors are turning on their input values mav(0, -500);//-500 is velocity mav(2, 500);//500 is velocity sleep(1.6);//7.9 is how many seconds the motors are turning on their input values mav(0, 900); mav(2, 900); sleep(1.8); mav(2, -800);//-800 is the velocity mav(0, -800);//-800 is the velocity sleep(2.0);//1.8 is how many seconds the motors are turning on their input values mav(0, -300);//-300 is the velocity mav(2, -100);//-100 is the velocity sleep(1.9);//1.7 is how many seconds the motors are turning on their input values ao();//turns motors off //in this ectionthe arm is lowered the claw closes capturing the polyps and the arm is raised to expose the touch sensor. set_servo_position(0, 800);//this lowers the claw sleep(0.6);//0.6 is how many seconds it give the servo to spin to the input position set_servo_position(0, 900); sleep(1.8); set_servo_position(0, 1000); sleep(1.6); set_servo_position(0, 1300); sleep(2.6); set_servo_position(0, 1700);//this lowers the claw all the way to the board sleep(1.4);//1.4 is how many seconds it give the servo to spin to the input position set_servo_position(1, 2048);//this closes the claws with the polyps inside sleep(0.6);//0.6 is how many seconds it give the servo to spin to the input position set_servo_position(0, 1324);//this raises the arm to expose the touch sensors sleep(0.6);//0.6 is how many seconds it give the servo to spin to the input position //in this section the robot turns toward MPA and drives straight until the touch sensor is triggered by hitting one of the walls of the MPA. mav(0, -100); mav(2, -300); sleep(2.0); mav(0, 1000); mav(2, 1000); sleep(2.8); mav(0, -200); mav(2, -200); sleep(2.6); mav(0, 500); mav(2, -500); sleep(1.0); set_servo_position(0, 1324); sleep(0.6); while(digital(8)==0) { mav(2, -900); mav(0, -700); sleep(0.2); if(digital(9)==1) { mav(2, -700); mav(0, -975); sleep(0.7); } } mav(0, 300); mav(2, 300); sleep(1.2); ao(); set_servo_position(0, 1224); sleep(0.8); set_servo_position(1, 0); sleep(1.4); set_servo_position(0, 1024); sleep(0.2); set_servo_position(0, 1124); sleep(0.2); set_servo_position(1, 2048); sleep(1.4); set_servo_position(1, 0); sleep(1.4); set_servo_position(1, 2048); sleep(1.4); set_servo_position(0, 524); sleep(0.6); set_servo_position(1, 0); sleep(1.4); set_servo_position(0, 1424); sleep(0.6); set_servo_position(1, 0); sleep(1.4); set_servo_position(0, 624); sleep(0.6); set_servo_position(0, 1424); sleep(0.8); set_servo_position(0, 624); sleep(0.8); mav(0, 300); mav(2, 0); sleep(0.6); mav(0, 500); mav(2, 500); sleep(0.8); mav(0, 500); mav(2, 500); sleep(0.1); mav(0, -500); mav(2, 500); sleep(1.8); mav(0, 500); mav(2, -500); sleep(1.8); mav(0, -500); mav(2, -500); sleep(0.1); mav(0, -500); mav(2, 500); sleep(1.6); mav(0, 1000); mav(2, 1000); sleep(1.0); set_servo_position(0, 1324); sleep(1.0); mav(0, 1000); mav(2, 1000); sleep(1.8); mav(0, -500); mav(2, -500); sleep(4.6); mav(0, -500); mav(2, 500); sleep(1.7); mav(0, -500); mav(2, -500); sleep(2.0); mav(0, -500); mav(2, 500); sleep(1.6); //mav(0, 500); //mav(3, 500); //sleep(0.8); mav(0, 300); mav(2, 300); sleep(0.2); ao(); set_servo_position(0, 1024); sleep(1.4); set_servo_position(1, 700); sleep(16.0); set_servo_position(1, 0); set_servo_position(0, 1324); sleep(1.4); sleep(1.0); mav(0, 500); mav(2, 500); sleep(1.2); mav(0, 500); mav(2, -500); sleep(1.6); mav(0, -1000); mav(2, -1000); sleep(2.3); mav(0, 500); mav(2, -500); sleep(1.4); while(digital(8)==0) { mav(2, -1975); mav(0, -2000); sleep(0.5); } mav(0, 500); mav(2, -500); sleep(1.6); mav(2, -1975); mav(0, -2000); sleep(2.3); }
void startup() { shut_down_in(115);//We get 120 seconds, but its nice to be on the safe side //if using this, make sure to use ao() }
int main(int argc, char** argv) { startup_servos(); //wait_for_light(0); scanf("%s", NULL); printf("waiting for light\n"); while (analog(0) > 200) {} shut_down_in(700000); raise_scoop_level(); drive_straight(900); drive_straight(-50);//old value 320 /*//to be replaced with tick thing motor(kMotorPortDriveLeft, 100); motor(kMotorPortDriveRight, 5); msleep(2430); motor(kMotorPortDriveLeft, 0); motor(kMotorPortDriveRight, 0);*/ //dump(); //close_scoop_doors(); //open_scoop_doors(); //dump(); //msleep(800); turn_ticks_in_place(50, 890); // pile 1 open_scoop_doors(); lower_scoop_level(); drive_straight(250); close_scoop_doors(); raise_scoop_level(); //Reverse to pile 2 drive_straight(-290); turn_ticks(50, -700); drive_straight(-250); //set_servo_position(kServoPortScoopTiltLeft, kServoPositionScoopTiltHighBack); //set_servo_position(kServoPortScoopTiltRight, 2047 - kServoPositionScoopTiltHighBack); turn_ticks(50, 770);//this value lower_scoop_level(); open_scoop_doors(); //pick up pile 2 drive_straight(270); close_scoop_doors(); raise_scoop_level(); drive_straight(80); raise_scoop_very_high(); drive_straight(170); turn_ticks(50, 500); drive_straight(-90); turn_ticks_in_place(50, 650); drive_straight(100); //set_servo_position(kServoPortScoopTiltLeft, kServoPositionScoopTiltHigh - 300); //set_servo_position(kServoPortScoopTiltLeft, (2047 - kServoPositionScoopTiltHigh) + 300); // turn_ticks(50, 555); // drive_straight(50); //set_servo_position(kServoPortScoopTiltLeft, 1000); //set_servo_position(kServoPortScoopTiltRight, 2047 - 1000); msleep(200); open_doors_wide(); dump(); msleep(500); dump_shake(); dump(); dump_shake(); dump(); //raise_scoop(); drive_straight(-400); return 0; raise_scoop_level(); close_doors_wide(); raise_scoop_very_high(); turn_ticks_in_place(50, -1350); drive_straight(100); //drive to pile 3 raise_scoop_level(); turn_ticks_in_place(50, 700); turn_ticks_in_place(50, -400); lower_scoop_level(); turn_ticks_in_place(50, -300); drive_straight(100); //pick up pile 3 open_scoop_doors(); drive_straight(400); close_scoop_doors(); turn_ticks_in_place(50, -100); drive_straight(-400); raise_scoop_level(); turn_ticks_in_place(50, -100); drive_straight(-400); turn_ticks_in_place(50, -200); drive_straight(-290); lower_scoop_level(); turn_ticks_in_place(50, 1150); turn_ticks_in_place(50, -100); raise_scoop_level(); // drive to/pick up pile 4 drive_straight(500); turn_ticks_in_place(50, 400); mid_scoop_level(); turn_ticks_in_place(50, -900); raise_scoop_level(); turn_ticks_in_place(50, 400); lower_scoop_level(); turn_ticks_in_place(50, 500); turn_ticks_in_place(50, -50); open_scoop_doors(); drive_straight(230); close_scoop_doors(); raise_scoop_level(); // dump other load turn_ticks_inverse(50, 500); drive_straight(-100); turn_ticks_in_place(50, -100); drive_straight(-150); raise_scoop_very_high(); turn_ticks_in_place(50, 700); drive_straight(200); set_servo_position(kServoPortScoopTiltLeft, 1000); set_servo_position(kServoPortScoopTiltRight, 2047 - 1000); msleep(200); open_doors_wide(); dump(); msleep(500); dump_shake(); dump(); dump_shake(); dump(); return 0; turn_ticks_in_place(50, -475); drive_straight(-200); turn_ticks_in_place(50, 475); drive_straight(400); //pick up pile 4 lower_scoop_level(); drive_straight(500); open_scoop_doors(); }
int main() { //wait_for_light(1);//initialization printf("version 1.9.5\n"); shut_down_in(115); int servo_counter=0; while(servo_counter<=3){ enable_servo(servo_counter); servo_counter++; } // initialize routine set_servo_position(0,2047); armUp(); clawClose(); //get in front of transport straight(1.8, 170); rightC(0.28, 100); leftC(0.28, -100); straight(1.65, 170); leftC(0.44, 100); printf("completed first dead reckoning\n"); //until distance is 5 centimeter /* int claw_threshold = 13; while(analog10(ETport)>claw_threshold){ back(0.1, -10); msleep(1000); printf("move once\n"); printf("%i\n", analog10(ETport)); }*/ back(0.25, -20);//pushes against transport msleep(2000); set_servo_position(0, 500);//puts claw on transport msleep(1000); printf("done with attaching to transport\n"); while(digital(8)==0)//goes forward with transport until it hits the wall { straight(0.1, 100); msleep(10); } printf("at wall\n"); back(0.15, -50);//backs up from wall to deposit transport set_servo_position(0, 2047);//raises claw msleep(1500); while(digital(8)==0){//goes forward until it hits wall for second time straight(0.1, 100); msleep(10); } printf("at wall 2\n"); back(0.35, -100);//goes back and turns left in order to be oriented with left inner wall leftC(0.6, 142); int bump_counter=0; while(digital(8)==0){//hits side wall straight(0.1, 100); msleep(10); } bump_counter++;//has one bump int i=0; while(i<10){//if hits side wall, pause stop(0.2); i++; } /* ========================== ==========End of routine 1====== KIPR at the wall ========================== */ // Pause here to wait for Create to drop POMS msleep(1*1000); // int start=seconds();// start time printf("will get out of corner\n"); back(0.5, -100); while(seconds()-start<=3){//separates back from right angle stop(0.1); } right_angle(left); while(seconds()-start<=5){//separates right angle from straight stop(0.1); } double start_drive=seconds(); straight(0.7, 100);//go to poms stop(0.5); camera_open(LOW_RES); /*while(seconds()-start<=20){ int update_counter=0; while(update_counter<10){ camera_update(); update_counter++; } if(get_object_count(chan)==0){ stop(0.1); } int x=get_object_center(chan, 0).x; int area=get_object_area(chan, 0); if(area<=400){ navigate(x); } if(area>=800){ //if objects are clumped are clumped, area will be automatically larger. //this means that KIPR will start earlier than it should. We counter this here //straight(0.18, 100); straight(0.1, 100); break; } }*/ stop(0.1); printf("object seen\n"); double end_drive=seconds();//only for simulation. real end and start times would have to be taken from whole routine //pick up POMS that are right there pickup(); int offset=(end_drive-start_drive)*(3.0/4.0);;//half of distance from back of launch area to border of launch area back((end_drive-start_drive)+1.5, -100);//go to the back of wall plus a little more to straighten out printf("at back wall\n"); straight(2.0, 100);//goes halway between border and back of launch area right_angle(left);//faces transport int starpof=seconds();//time after right angle while(seconds()-starpof<=2){ motor(motorL, 0); motor(motorR, 0); } while(digital(8)==0){ straight(0.1, 100); } straight(0.1, 100); printf("touching transport\n"); motor(motorL, 0); motor(motorR, 0); dropoff();//drops poms in launch area printf("poms in transport\n"); back(0.25, -100); int starfop=seconds(); while(seconds()-starfop<=2){ motor(motorL, 0); motor(motorR, 0); } int turns=1; while(turns<=2){ right_angle(right); stop(0.3); turns++; } /* ====================== ======Part 2 of routine 2==== ====================== */ while(1){ stop(0.5);// while(digital(8)==0){ straight(0.1, 100); } back(0.3, -100); right_angle(left); pickup(); back((end_drive-start_drive)+1.5, -100); straight(2.2, 100); right_angle(left); while(digital(8)==0){ straight(0.1, 100); } dropoff(); back(0.5, -100); right_angle(left); right_angle(left); } /*while(1){ start=0; while(digital(8)==0){ straight(0.1, 80); } back(0.5, -100); right_angle(right); straight(0.5, 100); right_angle(right); right_angle(right); while(seconds()-start<=20){ int update_counter=0; while(update_counter<10){ camera_update(); update_counter++; } if(get_object_count(chan)==0){ stop(0.1); } int x=get_object_center(chan, 0).x; int area=get_object_area(chan, 0); if(area<=400){ navigate(x); } if(area>=800){ //if objects are clumped are clumped, area will be automatically larger. //this means that KIPR will start earlier than it should. We counter this here //straight(0.18, 100); straight(0.1, 100); break; } } int servo_counter=1; while(servo_counter<=3){ enable_servo(servo_counter); printf("servo %d enabled\n", servo_counter); servo_counter++; } pickup(); int offset=(end_drive-start_drive)*(3.0/4.0);;//half of distance from back of launch area to border of launch area back((end_drive-start_drive)+1.5, -100);//go to the back of wall plus a little more to straighten out printf("at back wall\n"); straight(1.8, 100);//goes halway between border and back of launch area right_angle(left);//faces transport int starpof=seconds();//time after right angle while(seconds()-starpof<=2){ motor(motorL, 0); motor(motorR, 0); } while(digital(8)==0){ straight(0.1, 100); } straight(0.1, 100); printf("touching transport\n"); motor(motorL, 0); motor(motorR, 0); dropoff();//drops poms in launch area printf("poms in transport\n"); back(0.25, -100); int starfop=seconds(); while(seconds()-starfop<=2){ motor(motorL, 0); motor(motorR, 0); } int turns=1; while(turns<=2){ right_angle(right); turns++; } repeat_runs++; }*/ while(1){ int servo_counter=0;//enables servos while(servo_counter<=3){ enable_servo(servo_counter); servo_counter++; } armUp();//raises arm at start stop(0.5); while(digital(TOUCH_SENSOR)==0){//go forward until left wall is contacted straight(0.1, 100); } stop(0.1); back(0.3, -100);//back up stop(0.1); right_angle2(left);//face poms stop(0.1); back(3+1.5, -100);//back up to rocket wall to straighten out stop(0.1); straight(3+1.5, 100);//go to poms stop(0.1); pickup();//pick poms up stop(0.1); back(3+1.5, -100);// back up until rocket wall stop(0.1); straight(2.2, 100);//go forward stop(0.1); right_angle2(left);//turn to face transport stop(0.1); while(digital(TOUCH_SENSOR)==0){//go forward until transport contacted straight(0.1, 100); } stop(0.1); dropoff();//drop poms off stop(0.1); back(0.5, -100);//back up from transpor stop(0.1); right_angle2(right);//orient for next iteration stop(0.1); right_angle2(right); } return 0; }
int main(int argc, char *argv[]) { shut_down_in(10.0); for(;;); return 0; }
int main() { light_start(0); shut_down_in(119.5); arm_up(); claw_open(); bin_down(); enable_servos(); collect_poms(); printf("Collected Initial Poms in base\n"); /* forward(70); msleep(500); left(35, 0); msleep(300); //face 1st pile forward(45); pomPileOne(); */ forward(50); msleep(500); left(28, 0); //face 1st pile forward(59); //needs to be exact or else claw will hit table divider msleep(500); pomPileOne(); /* //4/9/16 Test Code forward(20); msleep(500); left(15, 0); msleep(300); //face 1st pile forward(80); pomPileOne(); */ //go to pile 2 multforward(45, 0.50); msleep(100); forward(15); msleep(300); left(85, ks/2); msleep(300); backward(2); //forward(2); pomPileTwo(); /* if(green == 1 && red == 1) { printf("Collected all\n"); //go to bin right(60, ks/2); forward(100); right(100, ks/2); msleep(500); bin_mid(); msleep(500); bin_dump(); } else { //do pom pile three only if robot does not have both one red and one green pomPileThree(); //go to bin left(60, ks/2); forward(60); } */ backward(5); right(80, ks/2); msleep(300); forward(130); msleep(300); right(50, ks/2); //turn against the wall msleep(300); backward(5); msleep(500); /* arm_mid(); msleep(300); bin_mid(); msleep(500); bin_dump(); msleep(1000); */ right(40, ks/2); msleep(500); backward(15); servo(MAIN_ARM, 1200); //move away arm from box msleep(300); bin_mid(); msleep(500); bin_dump(); msleep(1000); bin_mid(); msleep(500); bin_dump(); msleep(1000); /* int i = 0; for(i = 0; i < 3; i++) { forward(2); msleep(300); backward(2); msleep(300); } */ msleep(1000); right(30, ks/2); forward(90); msleep(2000); disable_servos(); ao(); return 0; }
int main() { printf("checklist"); printf("hit black button when ready"); while(black_button()); printf("MAKE SURE CREATE IS ON!!!!!!!!"); while(!black_button()); printf("MAKE SURE ARM IS IN DOWN POSITION!!!!!!!!"); while(!black_button()); printf("MAKE SURE ARM AND CLAW WORK"); while(!black_button()); printf("CHECK POSITION OF CLAW AND ARM"); while(!black_button()); printf("MAKE SURE LIGHT SENSORT IS CALIBRATED"); while(!black_button()); // this starts the robot SECTion 1 wait_for_light(0); shut_down_in(120.0); //this shuts the robot down after 120.0 seconds while(create_connect()); //connects create to cbc set_servo_position(3, 0); set_servo_position(0, 1800); enable_servos(); msleep(500); create_full(); //section 2 create_drive_direct(SUPER_FAST, SUPER_FAST); msleep(1646); //leave beach create_stop(); msleep(300); create_drive_direct(SUPER_FAST, -SUPER_FAST); msleep(435); //turn towards center create_stop(); msleep(300); create_drive_direct(SUPER_FAST, SUPER_FAST); msleep(1600); //move towards botguy create_stop(); msleep(500); //section 3 set_servo_position(0, 1424); msleep(1800); set_servo_position(0, 1100); msleep(1800); set_servo_position(0, 924); msleep(1800); set_servo_position(0, 724); msleep(1800); set_servo_position(3, 1024); msleep(1800); set_servo_position(0, 924); msleep(1800); set_servo_position(0, 1100); msleep(1800); set_servo_position(0, 1200); msleep(2000); //retrieve botguy //section 4 create_drive_direct(-FAST, FAST); msleep(445); create_stop(); create_drive_direct(-FAST, -FAST); msleep(2000); create_stop(); create_drive_direct(FAST, -FAST); msleep(445); create_stop(); create_drive_direct(-FAST, -FAST); msleep(900); create_stop(); msleep(1600); create_stop(); set_servo_position(3, 0); set_servo_position(0, 1800); msleep(1200); set_servo_position(0, 1200); msleep(1000); set_servo_position(0, 1800); msleep(1000); create_drive_direct(1000, 1000); msleep(800); create_drive_direct(-1000, -1000); msleep(800); DRIVE(SUPER_FAST); MSLEEP(800); TURN(-SUPER_FAST, SUPER_FAST); MSLEEP(460); DRIVE(SUPER_FAST); MSLEEP(1380); TURN(SUPER_FAST, -SUPER_FAST); MSLEEP(425); DRIVE(SUPER_FAST); SLEEP(1.5); while(get_create_lbump(0.01) == 0) { DRIVE(SUPER_FAST); } TURN(-SUPER_FAST, SUPER_FAST); MSLEEP(435); TURN(SUPER_FAST, 975); MSLEEP(500); while(get_create_rbump(0.01) == 0) { TURN(SUPER_FAST, 990); } create_stop(); create_disconnect(); }
int main() { struct cbcRobot robot = {0,3,150,55,1100}; //Define the robot. 0 and 3 are the motor ports, //150 mm is the wheel distance //55 is wheel diameter, 1100 is ticks per rotation light_it_up(4); //Wait for the light to turn on set_servo_position(arm_servo, top_pos); //Set the arm servo to the top position shut_down_in(118.0); //Shut down in 118 seconds //msleep(5000); //Wait 5 seconds float initial_time=seconds(); //Create a variable that records the initial time cbc_align(400); //Align with both of the top hats on the black line ao(); cbc_align_white(300); lower_arm(600); //Lower claw and open arm open_claw(600); cbc_straight(140, 800); //Straight out to get away from pvc wait_for_cbc(); cbc_arc_left(160, 135, 100); //Get past pvc to lower arm and open claw wait_for_cbc(); cbc_straight(180, 800); //Go straight into tribbles wait_for_cbc(); close_claw(1000); //Close claw on tribbles cbc_straight(280, 800); wait_for_cbc(); raise_arm(1200); //Raise the arm msleep(800); cbc_arc_right(180, 34, 80); //Arc right to get into position to approach the lattice wall wait_for_cbc(); cbc_touch(700, 5000); //Bump into lattice wall msleep(300); cbc_straight(-100, 600); //Back up 10 cm from wall wait_for_cbc(); open_claw(800); //Open claw to release tribbles and turn 94 degrees right cbc_spin(-94, 500); wait_for_cbc(); cbc_straight(120, 700); //Go forward into the turnstile and lower arm to mid position arm_mid(2000); msleep(1200); cbc_spin(20, 500); //Turn 20 degrees left to hit the turnstile out of the way wait_for_cbc(); msleep(200); cbc_spin(-16, 700); //Turn back 16 degrees to the right and close the claw close_claw(800); wait_for_cbc(); cbc_straight(-280, 800); //Go backwards to get into position to grab the tribbles wait_for_cbc(); lower_arm(1000); //Lower the arm and open the claw msleep(800); open_claw(800); msleep(1000); cbc_spin(8,700); //Turn 8 degrees to get ready to grab the tribbles wait_for_cbc(); cbc_arc_left(1500, 6, 100); //Go nearly straight into the tribbles wait_for_cbc(); close_claw(200); //Go forward and grab the tribbles cbc_straight(200, 600); wait_for_cbc(); close_claw(600); msleep(800); cbc_straight(-60, 600); //Back up to clear the turnstile raise_arm(1000); wait_for_cbc(); cbc_spin(4,700); //Turn slightly left wait_for_cbc(); follow_tape_left(600); //Run into the turnstile for horizonal alignment cbc_straight(-100, 400); //Back up before crossing over to the other side wait_for_cbc(); cbc_spin(90, 500); //Turn 90 degrees to the left wait_for_cbc(); cbc_straight(-80, 600); //Back up and align with the black tape wait_for_cbc(); cbc_align(400); ao(); cbc_straight(-50, 300); //Back up and align with the black tape wait_for_cbc(); cbc_align(400); ao(); cbc_straight(540,800); //Go forward to the other side wait_for_cbc(); open_claw(1000); //Open the claw to release tribbles and arc right approximately 100 degrees mrp(robot.leftWheel,780,2340); mrp(robot.rightWheel,400,1200); bmd(robot.leftWheel); bmd(robot.rightWheel); msleep(200L); lower_arm(1400);//Go forward and lower the arm msleep(500L); cbc_straight(370, 800); wait_for_cbc(); close_claw(1000); //Go forward and close the claw cbc_straight(300,800); wait_for_cbc(); raise_arm(1500); //Raise the arm msleep(800); cbc_spin(-73,500); //Turn 73 degrees right wait_for_cbc(); open_claw(1000); //Run into the lattice wall and open the claw to release the tribbles cbc_touch(500, 5000); cbc_straight(-80, 600); //Back up 8 cm from wall wait_for_cbc(); cbc_spin(-92, 500); //Turn 92 degrees right and close the claw close_claw(1000); wait_for_cbc(); cbc_straight(-80, 600); //Go backwards to get into position to grab the tribbles wait_for_cbc(); lower_arm(1500); //Lower the arm and open the claw msleep(800); open_claw(800); msleep(1000); cbc_spin(4,700); //Turn a bit in order to put the robot in an optimal tribble gathering position wait_for_cbc(); cbc_arc_left(1500, 6, 100); //Go nearly straight into the tribbles wait_for_cbc(); close_claw(200); //Go forward and grab tribbles cbc_straight(180, 600); wait_for_cbc(); close_claw(600); msleep(800); cbc_straight(-80, 400); //Back up to clear the turnstile wait_for_cbc(); raise_arm(1200); //Raise the arm msleep(500); cbc_straight(220, 700); //Go forward into the turnstile wait_for_cbc(); cbc_spin(80, 500); //Turn 80 degrees to the left wait_for_cbc(); cbc_straight(-80, 600); //Back up and align with the black tape wait_for_cbc(); cbc_align(400); ao(); cbc_straight(-50, 300); //Back up and align with the black tape wait_for_cbc(); cbc_align(400); ao(); //cbc_straight(1080, 800); //Old go forward function //wait_for_cbc(); mrp(robot.rightWheel,825,7700); //Go forward and correct the non-straightness of the motors mrp(robot.leftWheel,800,7700); wait_for_cbc(); cbc_touch(500, 5000); //Run into the wall msleep(300); cbc_straight(-125, 600); //Back up 12.5 cm from wall wait_for_cbc(); cbc_spin(90, 500); //Turn 90 degrees to the left wait_for_cbc(); while(analog10(et_right)<850) //While the tape is not seen by the right top hat { mav(robot.rightWheel,615); //Go forward mav(robot.leftWheel,600); msleep(25L); } off(robot.rightWheel); //Turn the motors off off(robot.leftWheel); cbc_straight(240, 700); //Go forward to get under the injection chute wait_for_cbc(); msleep(8000); //Wait for the injection chute tribbles to be loaded up cbc_straight(250, 700); //Go forward into the wall wait_for_cbc(); cbc_straight(-40, 600); //Back up 4 cm from pvc wait_for_cbc(); arm_mid(1000); //Lower the arm to the mid position and open the claw msleep(800); open_claw(800); msleep(800); cbc_straight(-40, 600); //Back up 4 cm from pvc wait_for_cbc(); close_claw(1000); //Turn around 180 degrees and close the claw cbc_spin(180,600); wait_for_cbc(); cbc_straight(-80, 480); //Back up to wall wait_for_cbc(); lower_arm(1400); //Lower the arm msleep(800L); mrp(dump_mot, 200, 500); //Dump the tribble into the MPA bmd(dump_mot); msleep(1000); mrp(dump_mot, 200, -500); //Dump the tribble into the MPA bmd(dump_mot); mrp(dump_mot, 200, 500); //Dump the tribble into the MPA bmd(dump_mot); mrp(dump_mot, 200, -500); //Dump the tribble into the MPA bmd(dump_mot); float time = seconds() - initial_time; //Create a variable that represents the time taken for the robot to complete its program cbc_display_clear(); //Clear the CBC display and then print the time the robot took to complete its program printf("Time: %f",time); return 0; }