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;
}
