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 startup_servos(void) { enable_servos(); set_servo_position(kServoPortScoopLeft, 2047 - 1706); set_servo_position(kServoPortScoopRight, 1706); set_servo_position(kServoPortScoopTiltLeft, 800); set_servo_position(kServoPortScoopTiltLeft, 2047 - 800); }
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(); }
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(); }
// prepare: void prepare(){ camera_open(LOW_RES); camera_load_config(CONFIG); camera_update(); camera_update(); camera_update(); // prepare camera enable_servos(); set_servo_position(BSV, BSV_LEVEL); set_servo_position(ASV, ASV_BACK); set_servo_position(RSV, RSV_DOWN); set_servo_position(SSV, SSV_BACK); // prepare servos set_analog_pullup(FSS, 0); set_analog_pullup(BSS, 0); // prepare sensors display_clear(); printf("prepare succeeds"); while(digital(TSS) == 0); msleep(1500); //wait_for_light(SSS); //shut_down_in(TIME_LIMIT); // show controller that coke is ready }
void cube_is_near(){ camera_update(); msleep(500); camera_update(); enable_servos(); claw_open(); printf("cube is near"); freeze(Motor_Left); freeze(Motor_Right); if(get_object_count(0)!=0){ printf("gleich gefunden\n"); if(get_object_bbox(0,0).width > 20){ printf("groß genug"); funden=1; found_something(); } } double s=seconds(); motor(Motor_Right,30); while(funden==0){//nichts gefunden camera_update(); printf("nach rechts\n"); if(get_object_count!=0){ printf("1 gefunden \n"); if(get_object_bbox(0,0).width > 20){ printf("groß genug"); found_something(); funden=1; break; } } if(seconds()>s+2){ printf("zeit rechts\n"); freeze(Motor_Right); break; } } s=seconds(); motor(Motor_Left,30); while(funden==0){ camera_update(); printf("nach links drehen \n"); if(get_object_count!=0){ printf("1 gefunden \n"); if(get_object_bbox(0,0).width > 20){ printf("groß genug"); found_something(); funden=1; break; } } if(seconds()>s+4){ printf("zeit links\n"); freeze(Motor_Left); break; } } }
void Luggagecartcafe() { enable_servos(); set_servo_position(1,200); track_update(); int X = 69; if (track_x(0,0) < track_x(2,0)) //Pink on left { track_update(); while (track_x (0,0) > X+5 || track_x (0,0) < X-5) { if (track_x(0, 0) > X+5) { while(track_x(0,0) > X) { track_update(); left(250); } ao(); } else // (track_x(0,0) < X) { while (track_x(0, 0) < X) { track_update(); right(250); } ao(); } } } else if (track_x(2,0) < track_x(0,0)) //Pink on left { track_update(); while (track_x (2,0) > X+5 || track_x (2,0) < X-5) { if (track_x(2, 0) > X+5) { while(track_x(2,0) > X+5) { track_update(); left(250); } ao(); } else // (track_x(0,0) < X) { while (track_x(2, 0) < X-5) { track_update(); right(250); } } } } }
void ServoTest() { int pos; enable_servos(); ServoRange(); disable_servos(); }
void LuggageTrack() { enable_servos(); set_servo_position(1,0); track_update(); int X = 73; //x coord of center of left cart int y = 5; //how far right/left of center luggage carts can be if (track_x(0,0) < track_x(2,0)) //Pink on left { track_update(); while (track_x (0,0) < X-y || track_x (0,0) > X+y) { if (track_x(0, 0) > X+y) { while(track_x(0,0) > X+y) { track_update(); spinCW(50); } ao(); } else if (track_x(0,0) < X-y) { while (track_x(0, 0) < X-y) { track_update(); spinCCW(50); } } } } else { track_update(); while (track_x (2,0) > X+y || track_x (2,0) < X-y) { if (track_x(2, 0) > X+y) { while(track_x(2,0) > X+y) { track_update(); spinCCW(50); } ao(); } else if (track_x(0,0) < X-y) { while (track_x(2, 0) < X-y) { track_update(); spinCW(50); } ao(); } } } }
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() { set_each_analog_state(0,1,0,0,0,0,0,0); create_connect(); enable_servos(); armPosSB(); clawSB(); //seedReturn(); seed(); }
void SetArm() { printf ("Positioning claw, please wait.\n"); enable_servos(); set_servo_position(MiniServo, LowerMini); Release(); ArmDown(); printf ("positioning complete\n"); }
int main() { _MODE = PRACTICE_MODE; _ROBOT = LEGO; initialize_camera(LOW_RES); //point2 xy = get_camera_reading(); //DESIRED_X = xy.x; //DESIRED_Y = xy.y; set_servo_position(LIFT_SERVO, LIFT_SERVO_UP_POSITION); set_servo_position(DUMP_SERVO, DUMP_UP_POSITION); enable_servos(); //wait_for_light(3); //shut_down_in(119); pd_follow(STOPPING_TOPHAT, 0); lego_spin_degrees(90, 50, RIGHT); lego_drive_distance(16, 50, FORWARDS); //suck_up_pom(); lego_spin_degrees(50, 50, RIGHT); //suck_up_pom(); lego_spin(70, LEFT); while (analog10(LREFLECTANCE) < 400); while (analog10(RREFLECTANCE) < 400); lego_stop(); line_follow(STOPPING_TIME, 3); lego_drive_distance(2, 100, FORWARDS); lego_drive_distance(2, 100, BACKWARDS); lego_spin_degrees(40, 90, LEFT); suck_up_pom(); lego_spin_degrees(80, 90, RIGHT); suck_up_pom(); lego_spin(40, RIGHT); sleep(2); while (analog10(RREFLECTANCE) < 100); while (analog10(LREFLECTANCE) < 100); lego_stop(); line_follow(STOPPING_TOPHAT, 0); lego_spin_degrees(90, 50, LEFT); lego_spin(70, RIGHT); while (analog10(RREFLECTANCE) < 400); while (analog10(LREFLECTANCE) < 400); lego_stop(); sleep(20); line_follow(STOPPING_TIME, 3); lego_drive_distance(7, 70,FORWARDS); lego_spin_degrees(100, 50, RIGHT); lego_drive_distance(10, 100, BACKWARDS); set_servo_position(DUMP_SERVO, DUMP_DOWN_POSITION); sleep(1); set_servo_position(DUMP_SERVO, DUMP_UP_POSITION); set_servo_position(DUMP_SERVO, DUMP_DOWN_POSITION); return 0; }
int main() { enable_servos(); collect_poms(); msleep(500); disable_servos(); return 0; }
// FIXME: Make the following function robot-neutral and put it in a servo_motor file. void initialize_servos() { // FIXME: THESE SHOULD SAY "START POSITIONS", with those defined as DATA elsewhere. // Really, there should be a way to say what the four servo ports are used for, // with unused a possibility, and this code initializes and enables if there is at least one used. set_servo_position(CLAW_SERVO, CLAW_START_POSITION); set_servo_position(SORTER_SERVO, SORTER_START_POSITION); set_servo_position(CAMERA_SERVO, CAMERA_START_POSITION); set_servo_position(DUMPER_SERVO, DUMPER_START_POSITION); enable_servos(); }
int main() { msleep(1000); set_servo_position(1, 442); set_servo_position(3, 233); enable_servos(); msleep(5000); servo_booster(1, 0, 1500 , 250); msleep(5000); return 0; }
int main() { create_connect(); enable_servos(); set_create_total_angle(0); set_create_distance(0); WhichSide(); CenterCamera(); Lift(); }
void CompeteInit(int fPre) { enable_servos(); Gate(0); if (fPre) { sleep(0.25); disable_servos(); } fBall = 0; ipMotor = start_process(MotorDriver()); }
/*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 main() { create_connect(); enable_servos(); set_each_analog_state(1,1,0,0,0,0,0,0); initialize(); armPosBlockStack2(); clawStack2(); clawOpen(); create_drive_straight(300); run_for(3, clawGrab); create_stop(); clawGrab(); }
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; }
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); }
int main() { //startup(); set_servo_position(0,1230); set_servo_position(3,1330); mav(3,500); mav(0,500); sleep(2.8); ao(); enable_servos(); set_servo_position(0,60);//90 degrees set_servo_position(3,160); sleep(sleep_time); return 0; }
void prepare() { camera_open(LOW_RES); camera_load_config(CONFIG); camera_update(); enable_servos(); set_servo_position(BSV, BSV_LEVEL); set_servo_position(ASV, ASV_BACK); set_servo_position(RSV, RSV_DOWN); set_servo_position(SSV, SSV_BACK); display_clear(); printf("prepare succeeds"); while(!(a_button_clicked())); }
void main() { calibrate(); enable_servos(); set_digital_pullup(BUTTON,1); //set_servo_position(SERVO,800); int on=1; int intervall = 5; while(!b_button()){ if(digital(BUTTON)){ stop(); msleep(400); printf("button pressed"); on=(on+1)%2; } if(on){ drive_speed=analog10(SPEED_SENSOR)/10; if(drive_speed>100){ drive_speed=100; } forward(); while(analog10(LEFT_SENSOR)>=left_blk-150){ turn_left() ; set_servo_position(SERVO,servo_pos); if(digital(BUTTON)){ on=(on+1)%2;msleep(100);} if(servo_pos>=400||servo_pos<=0){intervall*=-1; } servo_pos=servo_pos+intervall; } while(analog10(RIGHT_SENSOR)>=right_blk-150){ turn_right(); set_servo_position(SERVO,servo_pos); if(digital(BUTTON)){ on=(on+1)%2;msleep(100);} if(servo_pos>=400||servo_pos<=0){intervall*=-1; } servo_pos=servo_pos+intervall; } set_servo_position(SERVO,servo_pos); if(servo_pos>=400||servo_pos<=0){intervall*=-1; } servo_pos=servo_pos+intervall; } else{ stop(); } } stop(); }
/******** 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; }
int main() { enable_servos(); set_servo_position(0,0); msleep(500) set_servo_position(0,2046); msleep(255) set_servo_position(0,812); msleep(7777777777777777777777777777777777777777777777777777777777777777777775 set_servo_position(0,223); sleep(.5); set_servo_position(0,680); sleep(.8); set_servo_position(0,400); sleep(.4); sleep(86400); return 0; }
int main() { create_connect(); go(457.2,250); resetd(); while (get_create_total_angle(10) < 90) {turnl(250);} reseta(); go(457.2,250); resetd(); while (get_create_total_angle(10) > -90) {turnr(250);} reseta(); go(250,250); while (get_create_total_angle(10) > -90)//Riddhi..your job is to write a //function like mine, go, that makes an easy command to turn the robot. {turnr(250);} reseta(); go(400,250); resetd(); motor(3,50); sleep(1.5);//this is to lower the cage thing for the green //and pink containers. might have to go forward or backward. go(-400,250);//to go backwards resetd(); while (get_create_total_angle(10) < 90) {turnl(250);} reseta(); go(-457.2,250); resetd(); //need help with steps 13-21 on the pseudocode. while (analog(2) < 100) {create_drive_straight(250);} //to raise the cage, and leave the filled containers of luggage. motor(3,50); sleep(1.5); enable_servos(); get_servo_position(2); if (get_servo_position(2) > 1023) {set_servo_position(2,get_servo_position(2)-1023);} if (get_servo_position(2) < 1023) {set_servo_position(2,get_servo_position(2)+1023);} }
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; }