示例#1
0
void testDrive(){
	float time1, time2;
	set_create_distance(0);
	printf("//Press Black Button to start/stop drive\n");
	while(!black_button());	time1 = seconds();
	create_drive_straight(-50);
	printf("drive(50);\t");
	sleep(2.0);
	while(!black_button());	time2 = seconds();
	stop();
	printf("sleep(%f);\t//move %dmm",time2-time1,get_create_distance(0));
	done = 1;
}
int main()
{
    X=0;
    create_connect();
    set_create_total_angle(0);
    printf("X\tAngle");
    while(black_button()==0)
    {
        if (a_button()==1)
        {
            turnHalfCCW();
            printf("%d\t%d\n",X,get_create_total_angle(0.1));
        }
        if (b_button()==1)
        {
            turnHalfCW();
            printf("%d\t%d\n",X,get_create_total_angle(0.1));
        }
        if (left_button()==1)
        {
            TurnCCW();
            printf("%d\t%d\n",X,get_create_total_angle(0.1));
        }
        if (right_button() == 1)
        {
            TurnCW();
            printf("%d\t%d\n",X,get_create_total_angle(0.1));
        }
    }
}
void strategySelect()
{
	printf("Press A for luggage carts\n"); //Strategy 1
	printf("Press B for tower tip\n"); //Strategy 2
	printf("Press <- for biofuel block\n"); //Strategy 3
	printf("Press -> for center defense\n"); //Strategy 4
	while (black_button() == 0)
	{
		if (a_button() == 1)
		{
			printf("You picked luggage carts\n");
			printf("Press black button to confirm\n");
			strategy = 1;
		}
		if (b_button() == 1)
		{
			printf("You picked tower tip\n");
			printf("Press black button to confirm\n");
			strategy = 2;
		}
		if (left_button() == 1)
		{
			printf("You picked biofuel block\n");
			printf("Press black button to confirm\n");
			strategy = 3;
		}
		if (right_button() == 1)
		{
			printf("You picked center defense\n");
			printf("Press black button to confirm\n");
			strategy = 4;
		}
	}
}
示例#4
0
int main()
{
	create_connect();
	float x=.45;
	if(black_button() == 1)
	{
		create_drive_straight(-100);
		sleep(10);
		create_stop();
	}
	while(left_button() == 0) 
	{
		while(black_button() == 0)
		{
			if (a_button() == 1)
			{
				x=x+.001;
				printf("%f\n", x);
				sleep(.5);
			}
			if (b_button() == 1)
			{
				x=x-.001;
				printf("%f\n", x);
				sleep(.5);
			}
			if (up_button() == 1)
			{
				x=x+.0001;
				printf("%f\n", x);
				sleep(.5);
			}
			if (down_button() == 1)
			{
				x=x-.0001;
				printf("%f\n", x);
				sleep(.5);
			}
		}
		create_spin_CW(450);
		sleep(x);
		create_stop();
	}
}
示例#5
0
void testArc(){
	float time1, time2;
	cbc_display_clear();
	set_create_total_angle(0);
	printf("//Press Black Button to start/stop arc\n");
	while(black_button()) while(!black_button()){}
		time1 = seconds();
	create_drive(-100,-580);
	while(!black_button());	time2 = seconds();	stop();
	printf("%fsecs\t %dmm\n",time2-time1,get_create_distance(0));
	while(!black_button());
	clawSB();
	armPosSB();
	printf("\npress A for too far, B for not far enough, Black for just right\n");
	while(!done){
		if(a_button()){
			arc_value = arc_value-5;
			done = 1;
		}
		if(b_button()){
			arc_value = arc_value+5;
			done = 1;
		}
		if(black_button()) done = 1;
	}
	printf("arc = %d",arc_value);
    printf("press black button to exit");
	while(!black_button());
}
示例#6
0
void instructions(){
	cbc_display_clear();
	msleep(500);
	printf("A to drive.\n");
	printf("  Black Button to stop\n");
	printf("B to turn.\n");
	printf("  Left to turn CCW 90 deg.\n");
	printf("  Right to turn CW  90 deg.\n");
	printf("\nBlack Button to clear this screen");
	while(!black_button())	     msleep(500);
	cbc_display_clear();
	msleep(500);
}
示例#7
0
/*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
}
int main()
{
	init_captain_planet_with_our_powers_combined();
	
	int phase = get_phase(CURRENT_TIME);
	
	while(!black_button())
	{
		int phase = get_phase(CURRENT_TIME);
		
		printf("%d\n",phase);
	}
	
	serializer_disconnect();
}
int main() 
{
	init_captain_planet_with_our_powers_combined();
	
	while(!black_button())
	{
		WATCHDOG_ACTIVE = 1;
		printf("Time: %f\ntimer_interrupt: %d\n\n",CURRENT_TIME,TIMER_INTERRUPT);
		if(TIMER_INTERRUPT)
			WATCHDOG_ACTIVE = 0;
		sleep(1);
	}
	
	serializer_disconnect();
}
示例#10
0
int main ()
{
	float CENTER = 79.5;
	float OFFSET = 0.05;
	
	drone_connect();
	
	enable_drone_vision();	
	drone_front_camera();
	
	sleep(2);
	
	drone_takeoff();
	
	printf("Black button to exit test loop and land the drone");
	
	float x, y;
	
	track_update();
	while(!black_button())
	{
		x = OFFSET * ((track_x(0,0) - CENTER) / CENTER);
		y = OFFSET * ((track_y(0,0) - CENTER) / CENTER);
		
		if(track_count(0) > 0)
		{
			printf("Items tracking: %d\n", track_count(0));
			drone_move(0,0, x, y);
		}
		els	
		{
			printf("No items: Hovering \n");
			drone_hover();
		}
		sleep(4);
		track_update();
	}
		
	printf("Landing");
	drone_move(0,0,0,-.25);
	sleep(0.5);
	
	drone_land();
	
	drone_disconnect();
	
	return 0;
}
/* Global variables go below (if you absolutely need them).*/
int look_for_color_green() { 
  int x, y, color=2;  // set up for color channel 2 (green)
  while (black_button() == 0) //run till button is pressed
    { 
      track_update(); // process the most recent image
      if (track_count(color) > 0)
        { // get  x, y for the biggest blob the channel sees
           x = track_x(color,2); y = track_y(color,2); 
	        printf("Biggest blob at (%d,%d)\n",x,y); 
        }
	   else
        { printf("No color match in Frame\n"); }
      sleep(0.2);  // give print time to register
    }
    printf("Program is done.\n");
}
int main() 
{
	//step 1
	//print a haiku describing the sensor used in step 2
	printf("This here example\n");//5 syllables
	printf("One more clever solution\n");//7
	printf("Written by Haiku\n");//5
	
	//step 2 / 3
	//print the value of a sensor every .1 seconds
	//quit when the black button is pressed
	while(!black_button()){//same as black_button()==0
		printf("The ____ sensor reads:\t%d\n",analog(1));
		sleep(.1);
	}
}
示例#13
0
int main()
{
    int port_1 = 6;
    int port_2 = 2;
    set_each_analog_state(1,1,1,1,1,1,0,0);
    while(!black_button())
    {
        //cbc_display_clear();
        //printf("    see_bar(%d): %d\n",14,see_bar(14));
        //printf("    see_bar(%d): %d\n",15,see_bar(15));
        //printf("bar sensor (%d): %d\n",port_1, bar_sensor(14, 15));
        //printf("analog to inches (%d): %f\n",port_1,analog(port_1));
        printf("analog to inches (%d): %f\n",port_2, analog_to_inches_avg(port_2,10));
        sleep(.3);
    }
}
示例#14
0
int main()
{
	X=0;
	create_connect();
	set_create_total_angle(0);
	printf("X\tAngle");
	cbc_display_clear();
		printf("A to turn CCW 90deg.\tB to turn CW 90deg.\n");
		printf("Left to turn CCW 45deg.\tRight to turn CW 45deg.\n");
		printf("Down to force angle readjustment.\n");
	while(black_button()==0)
	{
		if (a_button()==1)
		{
			turn(tspeed,45);
			//turnHalfCCW();
			printf("%d\t%d\n",X,get_create_total_angle(0.1));
		}
		if (b_button()==1)
		{
			turn(tspeed,-45);
			//turnHalfCW();
			printf("%d\t%d\n",X,get_create_total_angle(0.1));
		}
		if (left_button()==1)
		{
			turn(tspeed,90);
			//turnCCW();
			printf("%d\t%d\n",X,get_create_total_angle(0.1));
		}
		if (right_button() == 1)
		{
			turn(tspeed,-90);
			//turnCW();
			printf("%d\t%d\n",X,get_create_total_angle(0.1));
		}
		if (down_button() == 1)
		{
			turnAdjust();
			printf("%d\t%d\n",X,get_create_total_angle(0.1));
		}
	}
}
void main()
{
	init_captain_planet_with_our_powers_combined();
	start_movement();
	CURRENT = MID;
	
	float cycle_start_time;
	while(!black_button())
	{
		//cbc_display_clear();
		cycle_start_time = CURRENT_TIME;
		DESTINATION=calculate_destination();
		printf("Setting Dest to: %d from: %d\n",DESTINATION,CURRENT);
		move_to(CURRENT,DESTINATION);
		CURRENT = DESTINATION;
		CHARGED = dock(CURRENT);
		printf("dock returned: %d\n", CHARGED);
		update_source_order(cycle_start_time);
		printf("SOURCE_ORDER: {%d,%d,%d} @ %f\n\n",SOURCE_ORDER[0],SOURCE_ORDER[1],SOURCE_ORDER[2],		cycle_start_time);
		sleep(.1);
	}
	serializer_disconnect();
}
示例#16
0
int main()
{
    printf("Press A for penis, B for v****a, or black button for titties!\n");
    while(a_button() == 0 || b_button() == 0 || black_button() == 0)
    {
        if(a_button() == 1)
        {
            strategy = 1;
        }
        if(b_button() == 1)
        {
            strategy = 2;
        }
        if(black_button() == 1)
        {
            strategy = 3;
        }
    }

    sleep(0.5);

    if (strategy == 1)
    {
        printf("You picked penis as your first strategy, press B for v****a or black button for titties next\n");
        while(b_button() == 0 || black_button() == 0)
        {
            if (b_button() == 1)
            {
                strategy = strategy * 10 + 2;
            }
            if (black_button() == 1)
            {
                strategy = strategy * 10 + 3;
            }
        }
    }
    if (strategy == 2)
    {
        printf("You picked v****a as your first strategy, press A for penis or black button for titties next\n");
        while(a_button() == 0 || black_button() == 0)
        {
            if (a_button() == 1)
            {
                strategy = strategy * 10 + 1;
            }
            if (black_button() == 1)
            {
                strategy = strategy * 10 + 3;
            }
        }
    }
    if (strategy == 3)
    {
        printf("You picked titties as your first strategy, press A for penis or B for v****a next\n");
        while(a_button() == 0 || b_button() == 0)
        {
            if (a_button() == 1)
            {
                strategy = strategy * 10 + 1;
            }
            if (b_button() == 1)
            {
                strategy = strategy * 10 + 2;
            }
        }
    }

    if (strategy - 1 / 10 == 2)
    {
        strategy = 213;
    }
//Someone else can finish the rest :l
}
示例#17
0
文件: simCBC.c 项目: jaekim708/kiss
void kissSimComputerDraw()
{
	int oriX=ksWindow.simWidth, oriY=0, i;
	static char digitals[]="0 0 0 0 0 0 0 0";
	static char buttons[]="0 0 0 0 0 0 0";
	static char analogs[11][10]={"         ","         ","         ","         ","         ","         ","         ","         ","         ","         ","         "};
	static char actuators[4][25]={"                        ","                        ","                        ","                        "};
	static char servos[4][25]={"                        ","                        ","                        ","                        "};
	if(!glfwGetWindowParam(GLFW_OPENED)) return;
	graphics_rectangle_fill(oriX,oriY,oriX+1,ksWindow.height,BLACK); // draw left border
	draw_bg(oriX+5,oriY+9,0.36); // Botguy
	NOBOLD g_printString(" CBC SIMULATOR",oriX+25, oriY+10,TEAL,1.0);
	NOBOLD g_printString("    DIGITALS  ",oriX+25, oriY+40,TEAL,1.0);
	NOBOLD g_printString("0 1 2 3 4 5 6 7",oriX+25, oriY+55,TEAL,1.0);
	NOBOLD g_printString("    BUTTONS  ",oriX+25, oriY+90,TEAL,1.0);
	//NOBOLD g_printString("< ^ V > A B .",oriX+30, oriY+105,TEAL,1.0);
	NOBOLD g_printString("\xb \xd \xe \xc A B .",oriX+30, oriY+105,TEAL,1.0);
    // hex b, c, d, e are left, right, up, and down arrows
	NOBOLD g_printString(digitals,oriX+25, oriY+70,WHITE,1.0); // erase old digital values
	NOBOLD g_printString("    ANALOGS  ",oriX+25, oriY+140,TEAL,1.0);
	for(i=0;i<8;i++){
		digitals[2*i]=48+digital(i);
	}
	NOBOLD g_printString(digitals,oriX+25, oriY+70,BLACK,1.0); // write new digital values
	NOBOLD g_printString(buttons,oriX+30, oriY+120,WHITE,1.0); // erase old button values
	buttons[0]=48+!!left_button();
	buttons[2]=48+!!up_button();
	buttons[4]=48+!!down_button();
	buttons[6]=48+!!right_button();
	buttons[8]=48+!!a_button();
	buttons[10]=48+!!b_button();
	_bob.button=!!kiss_get_key('.');
	buttons[12]=48+black_button();
	NOBOLD g_printString(buttons,oriX+30, oriY+120,BLACK,1.0); // erase old button values
	for(i=8;i<16;i++){//print out analogs
		analogs[i-8][1]=(i<10)?48+i:48+i-10;
		analogs[i-8][0]=(i<10)?32:49;
		analogs[i-8][2]=':';
		analogs[i-8][3]=' ';
		NOBOLD g_printString(analogs[i-8],oriX+5, oriY+155+(15*(i-8)),WHITE,1.0);
		convertNumToString(analog10(i),4,*analogs,10,i-8,3);
		graphics_rectangle_fill(oriX+30,oriY+155+(15*(i-8)),oriX+157, oriY+165+(15*(i-8)),GRAY);
		graphics_line(oriX+30+analog(i)/2, oriY+155+(15*(i-8)),oriX+30+analog(i)/2,oriY+165+(15*(i-8)),RED);
		NOBOLD g_printString(analogs[i-8],oriX+5, oriY+155+(15*(i-8)),BLACK,1.0);

	}
	analogs[8][1]='X';
	analogs[8][0]='A';
	analogs[8][2]=':';
	analogs[8][3]=' ';
	NOBOLD g_printString(analogs[8],oriX+5, oriY+155+(15*(8)),WHITE,1.0);
	convertNumToString(accel_x(),5,*analogs,10,8,3);
	graphics_rectangle_fill(oriX+30,oriY+155+(15*(8)),oriX+157, oriY+165+(15*(8)),GRAY);
	graphics_line(oriX+30+(accel_x()+2048)/32, oriY+155+(15*(8)),oriX+30+(accel_x()+2048)/32,oriY+165+(15*(8)),RED);
	NOBOLD g_printString(analogs[8],oriX+5, oriY+155+(15*(8)),BLACK,1.0);
	analogs[9][1]='Y';
	analogs[9][0]='A';
	analogs[9][2]=':';
	analogs[9][3]=' ';
	NOBOLD g_printString(analogs[9],oriX+5, oriY+155+(15*(9)),WHITE,1.0);
	convertNumToString(accel_y(),5,*analogs,10,9,3);
	graphics_rectangle_fill(oriX+30,oriY+155+(15*(9)),oriX+157, oriY+165+(15*(9)),GRAY);
	graphics_line(oriX+30+(accel_y()+2048)/32, oriY+155+(15*(9)),oriX+30+(accel_y()+2048)/32,oriY+165+(15*(9)),RED);
	NOBOLD g_printString(analogs[9],oriX+5, oriY+155+(15*(9)),BLACK,1.0);
	analogs[10][1]='Z';
	analogs[10][0]='A';
	analogs[10][2]=':';
	analogs[10][3]=' ';
	NOBOLD g_printString(analogs[10],oriX+5, oriY+155+(15*(10)),WHITE,1.0);
	convertNumToString(accel_z(),5,*analogs,10,10,3);
	graphics_rectangle_fill(oriX+30,oriY+155+(15*(10)),oriX+157, oriY+165+(15*(10)),GRAY);
	graphics_line(oriX+30+(accel_z()+2048)/32, oriY+155+(15*(10)),oriX+30+(accel_z()+2048)/32,oriY+165+(15*(10)),RED);
	NOBOLD g_printString(analogs[10],oriX+5, oriY+155+(15*(10)),BLACK,1.0);

	NOBOLD g_printString("MOT PWM TPS  ENCODER VAL",oriX+5, oriY+330,TEAL,1.0);
	for(i=0;i<4;i++){
		actuators[i][0]=48+i;
		actuators[i][1]=':';
		actuators[i][2]=' ';
		NOBOLD g_printString(actuators[i],oriX+5, oriY+345+(15*(i)),WHITE,1.0);
		convertNumToString(_bob.motor_pwm[i],4,*actuators,25,i,2);
		convertNumToString(_bob.motor_tps[i],4,*actuators,25,i,7);
		convertNumToString(_bob.motor_counter[i],10,*actuators,25,i,12);
		NOBOLD g_printString(actuators[i],oriX+5, oriY+345+(15*(i)),BLACK,1.0);
	}
	if(_bob.enable_servos){
		NOBOLD g_printString("SERVO TARGET DISABLED",oriX+5, oriY+410,WHITE,1.0);
		NOBOLD g_printString("SERVO TARGET ENABLED",oriX+5, oriY+410,TEAL,1.0);
	}
	else {
		NOBOLD g_printString("SERVO TARGET ENABLED",oriX+5, oriY+410,WHITE,1.0);
		NOBOLD g_printString("SERVO TARGET DISABLED",oriX+5, oriY+410,TEAL,1.0);
	}
	for(i=0;i<4;i++){
		servos[i][2]=49+i;//servos are 1 indexed!!
		servos[i][3]=':';
		servos[i][4]=' ';
		NOBOLD g_printString(servos[i],oriX+5, oriY+425+(15*(i)),WHITE,1.0);
		convertNumToString(_bob.servo_targets[i],4,*servos,25,i,6);
		NOBOLD g_printString(servos[i],oriX+5, oriY+425+(15*(i)),BLACK,1.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();
	
	
	
	
}
示例#19
0
int main()
{
	enable_servos();
	create_connect();
	set_each_analog_state(1,0,0,0,0,0,0,0);
	/*
	while(up_button()==0)
	{
	while(black_button() ==0) {}
	clawSB();
	armPosSB();
	while(black_button() ==0) {}
	clawGrab();
	clawOpen();
	armPosBlockGrab();
	while(black_button() ==0) {}
	clawClose();
	while(black_button() ==0) {}
	armPosBlockStack1();
	clawStack1();
	while(black_button() ==0) {}
	clawOpen();
	while(black_button() ==0) {}
	clawGrab();
	armPosBlockGrab();
	while(black_button() ==0) {}
	clawClose();
	while(black_button() ==0) {}
	/*set_servo_position(SERVO_ARM_LEFT_PORT, SERVO_ARM_LEFT_STACK2);
	set_servo_position(SERVO_ARM_RIGHT_PORT, SERVO_ARM_RIGHT_STACK2);
	set_servo_position(SERVO_CLAW_LEFT_PORT, SERVO_CLAW_LEFT_STACK2);
	set_servo_position(SERVO_CLAW_RIGHT_PORT, SERVO_CLAW_RIGHT_STACK2);
	sleep(2);
	armPosBlockStack2();
	clawStack2();
	while(black_button() ==0) {}
	clawOpen();
	}*/

	//reach first blocks
	armPosBlockStack1();
	clawStack1();
	clawOpen();
	detectBlock();
	//back up
	backFromTouch(0);
	//arm to correct pos
	armPosBlockGrab();
	clawGrab();
	//scoot up and grab
	scootUp(0);
	clawClose();  
	while(black_button() == 0) {}
	driveAtMmFor(200, 2);
	//drive with arm a bit up, place first pair of cubes
	armPosBlockStack1();
	clawStack1();
	detectPVC();
	set_servo_position(SERVO_ARM_LEFT_PORT, SERVO_ARM_LEFT_GRAB+300);
	set_servo_position(SERVO_ARM_RIGHT_PORT, SERVO_ARM_RIGHT_GRAB-300);
	set_servo_position(SERVO_CLAW_LEFT_PORT, SERVO_CLAW_LEFT_GRAB+100);
	set_servo_position(SERVO_CLAW_RIGHT_PORT, SERVO_CLAW_RIGHT_GRAB-100);
	backFromTouch(1);
	//correct pos
	armPosBlockGrab();
	clawGrab();
	//scoot up
	scootUp(1);
	clawOpen();
	driveAtMmFor(200, 2);
	while(black_button() == 0) {}
	clawClose();
	//drive with arm a bit up, place second pair on top of first
	armPosBlockStack1();
	clawStack1();
	set_servo_position(SERVO_ARM_LEFT_PORT, SERVO_ARM_LEFT_STACK1+300);
	set_servo_position(SERVO_ARM_RIGHT_PORT, SERVO_ARM_RIGHT_STACK1-300);
	set_servo_position(SERVO_CLAW_LEFT_PORT, SERVO_CLAW_LEFT_STACK1+200);
	set_servo_position(SERVO_CLAW_RIGHT_PORT, SERVO_CLAW_RIGHT_STACK1-200);
	detectBlock();
	backFromTouch(2);
	//correct pos
	clawStack1();
	armPosBlockStack1();
	clawOpen();
	driveAtMmFor(200, 2);
	armPosBlockGrab();
	clawGrab();
	while(black_button() == 0) {}
	clawClose();
	armPosBlockStack2();
	clawStack2();
	detectPVC();
	backFromTouch(3);
	clawOpen();
	driveAtMmFor(200, 2);
}