Esempio n. 1
0
void ServosWidget::enable()
{
  // Enable current servo
  const int port = ui->dial->label();
	enable_servo(port);
	publish();
  
  // Update enable/disable button visibility
	ui->enable->hide();
	ui->disable->show();
}
Esempio n. 2
0
// ============================================================================================
// for initialization of swarm dynamics function
// ============================================================================================
void init_variables()
{
	int i;

	if(special) 
		send_message(MESSAGE_COMMAND, ALL_DIRECTION, ALL, "i");

	for(i=0;i<NUM_NEIGHBORS;i++)
	{
		agent0.neix[i] = 0.0;	agent0.neiy[i] = 0.0;
		agent1.neix[i] = 0.0;	agent1.neiy[i] = 0.0;
		connected[i] = false;
	}

	// for chaotic motion
	agent0.px = 22.0; agent0.py = 40.0; agent0.vx = 0.186; agent0.vy = -4.8; agent0.hd = 4.75;

	//for ordered motion
	if(sec_counter < STGtime4)
	{
		agent1.px =  2.7; agent1.py = 75.6; agent1.vx = 3.8;   agent1.vy = 1.3;	 agent1.hd = 0.0;
	}
	else
	{
		agent1.px = 22.0; agent1.py = 40.0; agent1.vx = 0.186; agent1.vy = -4.8; agent1.hd = 4.75;
	}

	wave_flg = false; 
	wave_ping = false;
	column_flg = false;
	rhythm_on = false;

	agent2.flg = false;

	agent3.tim2 = 20.0;

	global_amp = 0;
	
	enable_servo();
	
	set_servo_position(0);
}
Esempio n. 3
0
int main()
{
	create_connect();
	enable_servo(ccport);
	
	create_full();	
	start_morse();
	morse("Botball", 1);
	/*	motor(1,100);
	motor(0,-100);
	motor(1,0);
	msleep(5000);
	motor(3,100);
	wait_for_morse();
	motor(0,0);
	motor(3,0);
	*/
	
	create_disconnect();
}
Esempio n. 4
0
int main()
{
	printf("starting connor\n");
	enable_servo(1);
	set_servo_position(1,100);
	//disable_servo(1)
	create_connect(); 
	while (! digital(11))
	{
		printf("%d\n",  digital(11));
		create_drive_direct(-150,-150);
	}
	printf("starting connor\n");
	set_servo_position(1,1400);
	while (get_create_lbump() == 0 && get_create_rbump() == 0)
		{
		create_drive_direct(150, 150); // drive straight at 150mm/s
		}
	disable_servos();
}
Esempio n. 5
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;

}
Esempio n. 6
0
int main()
{
	int delay=10;
	int i=0;
	enable_servo(2); 
	printf("%d enabled\n",get_servo_enabled(0));
	printf("%d enabled\n",get_servo_enabled(1));
	printf("%d enabled\n",get_servo_enabled(2));
	printf("%d enabled\n",get_servo_enabled(3));
	
	set_servo_position(2,100); 
	for(i=0;i<200;i++)
	{
		printf("%d position %d\n",get_servo_position(2), i); 
		msleep(delay);
	}
	msleep(1500);
	
	set_servo_position(2,1500); 
	printf("%d position\n",get_servo_position(2)); 
	msleep(1500); 
	
}
Esempio n. 7
0
// catch balls:
void catchBalls(int color){
	
	enable_servo(ASV);
	enable_servo(SSV);
	set_servo_position(ASV,700);
	set_servo_position(SSV,200);	
	msleep(500);
	
	camera_open();
	camera_update();
	
	while(get_object_center_column(color,0) != OFFSET_X){
		
		while(get_object_center_column(color,0) > OFFSET_X){
			
			motor(LM,10);
			motor(RM,-10);
			camera_update();
		
			}
	
			motor(LM,-10);
			motor(RM,10);
			camera_update();

	}
		
	while(get_object_center_row(color,0) != OFFSET_Y){
		
		while(get_object_center_row(color,0) > OFFSET_Y){
			
			motor(LM,-10);
			motor(RM,-10);
			camera_update();
		
		}
	
			motor(LM,10);
			motor(RM,10);
			camera_update();
		
	}
	
	set_servo_position(ASV,1900);
	msleep(1500);
	set_servo_position(ASV,1700);
	msleep(500);
	
	camera_update();
	
	if( checkBalls(GREEN) == 0 ){
		
		set_servo_position(SSV,180);
		msleep(300);
		set_servo_position(SSV,0);
		
	}else{
		
		set_servo_position(ASV,0);
		
	}

}
Esempio n. 8
0
VI void enable_servos()
{
	for(int i = 0; i < 4; ++i) enable_servo(i);
}
Esempio n. 9
0
void ServoTestWizardPage::servoOn(const quint16 &port)
{
	enable_servo(port);
}
Esempio n. 10
0
int main()
{
	msleep(2500);
	set_analog_pullup(ET_s , 0); 
	extra_buttons_show(1); // show three extra buttons
	set_a_button_text("COORDS"); // set the text of various buttons
	set_b_button_text("POM SIZE");
	set_c_button_text("BOTGUY SIZE");
	set_x_button_text("CUBE SIZE"); 

	lego.left.port = 0; // set motor ports
	lego.right.port = 2;
	
	camera_open(LOW_RES);
	camera_update();
	while (a_button() == 0) // press the a button to set the coordinates
	{
		camera_update();
		target.green.x = get_object_center(0 , 0).x; // sets target coordinates (x)
		target.green.y = get_object_center(0 , 0).y; // sets target coordinates (y)
		printf("(%d , %d)\n" , target.green.x , target.green.y);
		msleep(10);
	}
	while (b_button() == 0)
	{
		camera_update();
		target.green.size = get_object_area(0 , 0);
		target.orange.size = get_object_area(1 , 0);
		printf("Orange Size = %d" , target.orange.size);
		printf(" Green Size = %d\n" , target.green.size);
	}
	enable_servo(arm_servo); 
	enable_servo(push_servo);
	enable_servo(basket_servo);
	set_servo_position(arm_servo , ARM_UP);
	set_servo_position(push_servo , P_DOWN);
	set_servo_position(basket_servo , B_UP);
	printf("(%d , %d)\n" , target.green.x , target.green.y);
	while(a_button() == 0)
	{
		printf("%d , %d , %d\n" , get_left() , get_middle() , get_right());
	}
	while (1) // line follow until poms are seen
	{
		blob_update();
		t_line_follow();
		if (current.orange.size > target.orange.size && current.green.size > target.green.size)
			break;
	}
	blob_update(); 
	get_pom(); // pick up a pom
	pom_push(); // push it into the basket
	while (1) // turn to next pom
	{
		blob_update();
		mav(lego.left.port , 300); 
		mav(lego.right.port , -300);
		msleep(10);
		if (current.green.size > target.green.size)
			break;
	}
	blob_update();
	get_pom(); // pick up pom
	pom_push(); // push it into the basket
	avoid_cubeguy(); // avoid the cube or botguy
	avoid_booster();
	while (1)
	{
		blob_update();
		t_line_follow();
		if (current.orange.size > target.orange.size && current.green.size > target.green.size)
			break;
	}
	blob_update();
	pom_push();
	while (1)
	{
		blob_update();
		mav(lego.left.port , 300);
		mav(lego.right.port , -300);
		msleep(10);
		if (current.green.size > target.green.size)
			break;
	}
	blob_update();
	get_pom();
	pom_push();
	avoid_cubeguy();
	int start_time = seconds();
	int t;
	while (1)
	{
		t_line_follow();
		if (seconds() < start_time + t)
			break;
	}
	while (1)
	{

	}
}
Esempio n. 11
0
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;
}
Esempio n. 12
0
// ============================================================================================
// receive packet
// ============================================================================================
// this repeats code in key_press
void rx_pkt(Xgrid::Packet *pkt)
{
    uint8_t port = pkt->rx_node;	//pkt->rx_node
    char command;

    if(port >= 0 && port < NUM_NEIGHBORS) {

        if(pkt->type == MESSAGE_COMMAND)
        {
            char* char_ptr = (char*) pkt->data;
            command = *char_ptr;

            connected[port] = true;

            switch(command)
            {
            case 'Z':
                reboot_on = true;

            case 'D':
                speedup_on = false;
                communication_on = false;
                disable_servo();
                break;
            case 'R':
                speedup_on = true;
                communication_on = true;
                enable_servo();
                break;

            case 'i':
                init_variables();
                break;
            case 'r':
                sec_counter = 0;
                break;
            case 'I':
                init_variables();
                sec_counter = 0;
                break;

            case 'a':
                wave_flg = true;
                wave_port = port;
                break;
            case 'b':
                column_flg = true;
                break;

            case 'c':
                sync = false;
                break;

            case '1':
                sec_counter = STGtime1;
                init_variables();
                break;
            case '2':
                sec_counter = STGtime2;
                init_variables();
                break;
            case '3':
                sec_counter = STGtime3;
                init_variables();
                break;
            case '4':
                sec_counter = STGtime4;
                init_variables();
                break;
            case '5':
                sec_counter = STGtime5;
                init_variables();
                break;
            case '6':
                sec_counter = STGtime6;
                init_variables();
                break;
            case '7':
                sec_counter = STGtime7;
                init_variables();
                break;
            case '8':
                sec_counter = STGtime8;
                init_variables();
                break;
            case '9':
                sec_counter = STGtime9;
                init_variables();
                break;
            case '0':
                sec_counter = LASTtime;
                init_variables();
                break;

            }
        }

        if(pkt->type == MESSAGE_NUMDATA)
        {
            connected[port] = true;

            point* pt_ptr = (point*) pkt->data;

            agent0.neix[port] = pt_ptr->x0;
            agent0.neiy[port] = pt_ptr->y0;

            agent1.neix[port] = pt_ptr->x1;
            agent1.neiy[port] = pt_ptr->y1;
        }

        //LED_PORT.OUTTGL = LED_USR_2_PIN_bm;	//green LED
    }
}
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
	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_to_position (0, -900, -1200);
	
	
	while(! loop_done)
	{
		if (get_motor_done (0))
		{
			loop_done = 1;
		}

		msleep(100);
	}
	
	create_stop ();
	
	loop_done = 0;
	
	angle0 = get_create_total_angle (.05);
	create_drive_direct (50,-50);
	
	while(! loop_done)
	{
		printf("a0: %d, a: %d, d: %d\n", angle0, get_create_total_angle (.05), task_A_done);
		if (angle0 - get_create_total_angle (.05) >= 20)
		{
			loop_done = 1;
			create_stop ();
		}
		msleep(100);
	}
	
	loop_done = 0;
	
	create_stop ();
	
	task_A_done = 0;
	task_B_done = 0;
	
	angle0 = get_create_total_angle (.05);
	create_drive_direct (-50,50);
	
	while(! loop_done)
	{
		printf("a0: %d, a: %d, d: %d\n", angle0, get_create_total_angle (.05), task_A_done);
		if (angle0 - get_create_total_angle (.05) <= -20)
		{
			loop_done = 1;
			create_stop ();
		}
		msleep(100);
	}
	
	loop_done = 0;
	
	create_stop ();
	
	task_A_done = 0;
	task_B_done = 0;
	
	
	// ------------------------------------------------------------------------
	//step:2 knock over box and turn
	//lift arm
	//turn 90
	
	printf("-----Step 2-----\n");
	move_to_position (0, -900, -3100);
	msleep (900);
	
	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 >= 125)
		{
			create_drive_direct (-120, -181);
		}
		if (dangle >= 155)
		{
			task_A_done = 1;
			create_stop ();
		}
		
		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);
	}
	
	loop_done = 0;
	
	create_stop ();
	
	task_A_done = 0;
	task_B_done = 0;
	


	// ------------------------------------------------------------------------
	//step:4 aim claw to point of botguy
	//turn 20
	//lower arm
	
	printf("-----Step 4-----\n");
	
	angle0 = 0;
	
	angle0 = get_create_total_angle (.05);
	create_drive_direct (70, -70);
	printf("a0: %d, a: %d\n", angle0, get_create_total_angle(0.05));
	
	//while(angle0 - get_create_total_angle (.05) < 4)
	if (0)
	{
		printf("a0: %d, a: %d\n", angle0, get_create_total_angle(0.05));
		msleep(50);
	}
	
	move_to_position (0, 400, -900);
	
	distance0 = get_create_distance (.05);
	
	create_drive_direct (20, 20);
	
	while (! loop_done)
		{
			if (get_create_distance (.05) - distance0 > 50)
			{	
			loop_done = 1; 
			create_stop ();
			}
		}
	
	set_servo_position (0 ,400);
	
	loop_done = 0;
	
	task_A_done = 0;
	task_B_done = 0;
	
	create_drive_straight (-20);
	move_to_position (0, 600, -300);
	
	while (! get_motor_done (0))
	{
		msleep (50);
	}
	
	create_drive_straight (-50);
	
	msleep (2000);
	
	//while (! loop_done)
	if (0)
	{
		if (digital (10))
		{
			create_stop ();
			
			loop_done = 1;
		}
		if (get_create_distance (.05) - distance0 < -100)
		{
			create_stop ();
			
			loop_done = 1;
		}
	}
	
	
	
	loop_done = 0;
	
	
	
	// ------------------------------------------------------------------------
	//step:5 grab botguy and lift him up (not complete)
	printf("-----Step 5-----\n");
	
	create_drive_direct (60, -60);
	
	while(angle0 - get_create_total_angle (.05) < 3)
	{
		msleep(50);
	}
	
	create_stop ();
	
	set_servo_position (0, 1510);
	move_to_position (0, -1000, -3000);
	
	msleep (5000);
	
	
	
	// ------------------------------------------------------------------------
	//step:6 move backwards untill the bumper hits the transport (not completed)
	
	printf("-----Step 6-----\n");
	
	create_drive_direct (130, 280);
	
	distance0 = get_create_distance (.05);
	
		
	while (! loop_done)
	{
		printf("d0: %d, d: %d, d: %d\n", distance0, get_create_distance (.05), loop_done);
		if (get_create_distance (.05) - distance0 > 700)
		{
			loop_done = 1;
		}
		msleep(50);
	}
	
	loop_done = 0;
	
	move_to_position (0, -900, -500);
	create_drive_straight (-100);
	
	msleep (1000);
	
	set_servo_position (0, 200);

	create_disconnect ();

	
	printf("-----Step 6-B-----\n");
	
	create_drive_direct (-400, -150);
	
	distance0 = get_create_distance (.05);
	
	while(! loop_done)
	{
		printf("d0: %d, d: %d, d: %d\n", distance0, get_create_distance (.05), loop_done);
		if (get_create_distance (.05) - distance0 < -100)
		{
			
			loop_done = 1;
			
			create_stop ();
		}
		msleep(50);
	}
	
	loop_done = 0;
	// ------------------------------------------------------------------------
	//step:7 drop botguy.
	
	printf("-----Step 7-----\n");
	
	move_to_position (0, -900, -300);
	
	msleep (6000);
	
	set_servo_position (0, 200);
	
	
	create_disconnect ();

}
Esempio n. 14
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;
}
Esempio n. 15
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 ();
}
Esempio n. 16
0
int main()
{
	int m1=0,m2=3,pageno=0;
	double createDriveTime;
	printf("Start!\n");
	extra_buttons_show();
	while (1)
	{
		while (pageno == 0)
		{
			set_a_button_text("Forward");
			set_b_button_text("Backward");
			set_c_button_text("All Off");
			set_x_button_text("OpenHand");
			set_y_button_text("CloseHand");
			set_z_button_text("Create Page");
			
			if (a_button_clicked())
			{
				printf("Moving forward...\n");
				motor(m1,100);
				motor(m2,100);
				while (!c_button_clicked())
				{
					msleep(1);
				}
				ao();
			}
			if (b_button_clicked())
			{
				printf("Moving backward...\n");
				motor(m1,-100);
				motor(m2,-100);
				while (!c_button_clicked())
				{
					msleep(1);
				}
				ao();
			}
			if (x_button_clicked())
			{
				enable_servo(3);
				set_servo_position(3,0);
				msleep(300);
				disable_servo(3);
			}
			if (y_button_clicked())
			{
				enable_servo(3);
				set_servo_position(3,1300);
				msleep(300);
				disable_servo(3);
			}
			if (z_button_clicked())
			{
				pageno=1;
				create_connect();
			}
		}

		while (pageno == 1)
		{
			set_a_button_text("Create Fwd");
			set_b_button_text("Create Bwd");
			set_c_button_text("Stop");
			set_x_button_text("Turn R");
			set_y_button_text("Turn L");
			set_z_button_text("M S Page");
			
			if (a_button_clicked())
			{
				createDriveTime = 0;
				printf("Create Moving forward w/ spd 100...\n");
				create_drive_straight(100);
				createDriveTime = seconds();
				while (!c_button_clicked())
				{
					msleep(1);
				}
				create_stop();
				printf("Time: %f seconds\n",seconds() - createDriveTime );
			}
			if (b_button_clicked())
			{
				createDriveTime = 0;
				printf("Create Moving backward w/ spd 100...\n");
				create_drive_straight(-100);
				createDriveTime = seconds();
				while (!c_button_clicked())
				{
					msleep(1);
				}
				create_stop();
				printf("Time: %f seconds\n",seconds() - createDriveTime );
			}
			if (x_button_clicked())
			{
				create_drive(-50,0);
				while (!c_button_clicked())
				{
					msleep(1);
				}
				create_stop();
			}
			if (y_button_clicked())
			{
				create_drive(50,0);
				while (!c_button_clicked())
				{
					msleep(1);
				}
				create_stop();
			}
			if (z_button_clicked())
			{
				pageno=0;
				create_disconnect();
			}
		}
	}
	
	return 0;
}
Esempio n. 17
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;
}
Esempio n. 18
0
void claw_init(){
	set_servo_position(SERV_WRIST,300);
	set_servo_position(SERV_CLAW,805);
	enable_servo(SERV_WRIST);
	enable_servo(SERV_CLAW);
}