Example #1
0
void GotoBox()
{
	while (get_create_lbump(.1) == 0 && get_create_rbump(.1) == 0)//drive backwards until hit pvc w/ bumper
	{
		create_drive_straight(200); //unless arm is strong enough to hold
	}
	create_stop();
	ClearWall();	//move away from pvc to turn
	if(Side == 0)	//turn towards outer starting box -- now facing our outer starting box w/ blocks in claw
	{
		TurnCCW();
	}
	if(Side == 1)
	{
		TurnCW();
	}
	create_stop();
	ForwardTouch();	//forward till front touch sensor ==1
	create_drive_straight(132);  //one cube length (5 inches) + 5 mm safety // drive backwards
	sleep(1);
	Release();	// realease claw w/ 2 blocks -- block now in starting box
	create_drive_straight(-75);
	sleep(1);
	create_stop();  //make sure cubes are touching pvc
}
Example #2
0
int main()
{
	create_connect();
	init_servo();


	//light_it_up(LIGHT_PORT);
	lightstart(LIGHT_PORT,120.0);

	create_full();
	create_drive_segment(HIGH_SPEED, -150);
	start_process(set_top);
	create_drive_arc(HIGH_SPEED, -200, 155);
	create_drive_segment(HIGH_SPEED, -365);
	create_drive_arc(HIGH_SPEED, 90, -64);
	create_drive_segment(HIGH_SPEED, -85);
	create_cease();
	set_top();
	align_twall();
	grab_top_de();
	create_spin_angle(400,-165);
	create_drive_segment(HIGH_SPEED, -320);
	//align_wall();
	create_stop();
	create_sync();
	dump_kelp_de();
	create_drive_segment(HIGH_SPEED,30);//go away from starting box
	create_spin_angle(HIGH_SPEED, -78);//turn to gate part 1
	create_drive_segment(HIGH_SPEED, 938);//go to gate
	create_drive_arc(HIGH_SPEED,185,84);//arc to gate
	//create_drive_time(HIGH_SPEED,1800);//go through the gate
	/*
	create_drive_segment(HIGH_SPEED,1470);
	create_drive_segment(HIGH_SPEED,-100);
	create_spin_angle(HIGH_SPEED,78);
	*/
	create_cease();
	create_drive_straight(HIGH_SPEED);//go onto oppponent's side of board
	msleep(1400);
	create_stop();
	create_arc(350,550);//arc to the opponent's mpa
	msleep(2500L);
	create_stop();//pause to keep the creat
	create_drive_straight(HIGH_SPEED);//go onto oppponent's side of board
	msleep(750);
	create_stop();
	create_drive_straight(-HIGH_SPEED);//go onto oppponent's side of board
	msleep(5000);
	create_stop();
	/*
	create_drive_segment(500,-500);//go past the IC
	create_drive_arc(320,-250,25);//arc to
	create_drive_arc(320,250,-8);
	*/
	//create_drive_segment(HIGH_SPEED,-750);
	//create_drive_arc(HIGH_SPEED,100,84);
	//create_drive_segment(HIGH_SPEED,1300);
}
int main()
{
	int i;
	TurnCCW();//don't know what turn function we're using
	for(i=0; i < 5; i++)
	{
		create_drive_straight(100);
		sleep(2);
		CenterCamera2();
	}
	for(create_drive_straight(-200); !digital(TouchSensor);){}
}
Example #4
0
void ClearWall()
{
	if(get_create_lbump(.1) == 1 || get_create_rbump(.1) == 1)
	{
		create_drive_straight(-132);  //one cube length (5 inches) + 5 mm safety
		sleep(1);
		create_stop();
	}
	if(digital(TouchSensor) == 1)
	{
		create_drive_straight(132);
		sleep(1);
		create_stop();
	}
}
Example #5
0
int main()
{	
  printf("Line Follow."); // announce the program	
  sleep(1.0); // wait 1 second

  create_connect(); // Open the connection between CBC and Create	
  int ana0 = 0, ana1 = 0; // variables to store the results of the analog sensors
	create_full(); // We don't care about safety
	while(get_create_lbump(0) == 0 && get_create_rbump(0) == 0){ //while the bumper is not pressed
		ana0 = analog10(0); // left sensor
		ana1 = analog10(1); // right sensor
		
		printf("analog 0: %d\n", ana0);// print results
		printf("analog 1: %d\n", ana1);
		if((ana0 < 200) && (ana1 > 200)) // if the left sensor is off and right is on black line
		{
			create_spin_CW(128); // spin the create Clock Wise
		}else if((ana1 < 200) && (ana0 > 200)) // else if reversed
		{
			create_spin_CCW(128); // spin Counter Clock Wise
		}else if((ana0 < 200) && (ana1 < 200)) // else neither is on the line
		{
			create_drive_straight(180); // drive straight
		}
		
	}
  create_stop(); // Stop the Create
  create_disconnect(); // Disconnect the Create
}
Example #6
0
int main() 
{
	create_connect();
	create_drive_straight(500);
	sleep(2);
	create_drive_direct(500, -500);
	msleep(440);
	create_drive_straight(500);
	msleep(4200);
	create_drive_direct(-500, 500);
	msleep(375);
	create_drive_straight(500);
	msleep(1500);
	create_stop();
	create_disconnect();
}
Example #7
0
//generic move forward function calibrated in inches
void forward (int inches){
	create_drive_straight(SPEED_FWD);
	
	int time_conversion = inches * FORWARD_CONSTANT;
	msleep(time_conversion);
	create_stop();
}
Example #8
0
//generic move backward function calibrated in inches
void backward (int inches){
	create_drive_straight(SPEED_BWD);
	
	int time_conversion = inches * BACKWARD_CONSTANT;
	msleep(time_conversion);
	create_stop();
}
Example #9
0
/*
 * Class:     cbccore_low_Create
 * Method:    create_drive_straight
 * Signature: (I)V
 */
JNIEXPORT void JNICALL Java_cbccore_low_Create_create_1drive_1straight(JNIEnv *env, jobject obj, jint speed)
{
#ifdef CBC
    create_drive_straight(speed);
#else
    printf("Java_cbccore_low_Create_create_1drive_1straight stub\n");
#endif
}
	void ForwardTouch ()
	{
	while (analog(TouchSensor) == 0)
	{
	create_drive_straight(-500);
	}
	create_stop();
	}
Example #11
0
	void CenterCamera() //CAMERA MUST BE AT CENTER OF CREATE, POINTING RIGHT RESPECT TO CREATE
	{
		create_drive_straight(-200);
		while (track_x (0,0) > 84)	//WHAT TRACK # IS RED? MAKE SURE IT DOESNT DETECT BOTGUY
		{
			track_update();
		}
		create_stop();
		if (track_x (0,0) < 75)
		{
			create_drive_straight(200);
			while (track_x (0,0) < 75)
			{
				track_update();
			}
			create_stop();
		}
	}
Example #12
0
void kill() {
    printf("killing in %i.", wait * 1000);
    msleep(wait * 1000);
    printf("killing program.\n");
    disable_servos();
    create_drive_straight(0);
    motor(0, 0);
    motor(1, 0);
    motor(2, 0);
    motor(3, 0);
    exit(1);
}
Example #13
0
// script to move dist mm at speed mm/sec
void create_script_move(int dist, int speed) { 
    int sdist, ddist;
    create_drive_straight(speed);
    create_distance();
    sdist=gc_distance;
    ddist=sdist+dist;
    if(dist>0)
      while(gc_distance<ddist)create_distance();
    else
      while(gc_distance>ddist)create_distance();
    create_stop();
}
Example #14
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;
}
Example #15
0
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()
{
	set_each_analog_state(0,0,0,1,0,0,0,0);// This line sets analog ports to be pullup (0) or floating (1)
	create_connect();
	enable_servos();
	set_create_total_angle(0);
	set_create_distance(0);
	//AUO=start_process(ArmUpOpen);
	ArmUp();
	set_servo_position(
	sleep(3);
	CenterCamera();
	create_drive_straight(-200);
	while(digital(8) != 1)
	{}
	create_stop();
	sleep(1);
	ArmDownClose();
	sleep(3);
	beep();
	create_drive_straight(200);
	sleep(2);
	create_stop();
}
	void drivemm(float mmspeed, float seconds)//speed is mm/s
	{
	#ifdef CREATE
	create_drive_straight(mmspeed);
	sleep(seconds);
	create_stop();
	printf("%d mm/s * %d seconds = %d mm.\n", mmspeed, seconds, mmspeed*seconds);
	#endif
	#ifdef CBC
	mav(0,mmspeed);
	mav(1,mmspeed);
	sleep(seconds);
	off(0);
	off(1);
	#endif
	
	}
Example #18
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();
	}
}
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);}
	

}
Example #20
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;
}
Example #21
0
int main()
{
	int stupid_int = 0;	
	int value = 0;
	int i = 0;
	int angle_of_pole = 90;
	camera_open(LOW_RES);
	camera_update();
	create_connect();	

	
	set_create_total_angle(0);
	value = get_create_total_angle();
	printf("Angle is %d\n", value);
	//------------------------
// part 1: spin until past all booster sections
	
	while(get_create_total_angle() > -85){
		create_spin_CW(75);
	}
	
	//---------------------
	//end part 1
	camera_update();
	sleep(2);
	point2 mcenter = get_object_center(1,0);
	set_servo_position(0,OPEN_POSITION); //this is opening the claw so it won't go to a random position when enabling the servos
	enable_servos();
//--------------------------------------
// part 2: find booster section using function i wrote at bottom of code then line up with B2(pole)
	
	find_item();
	printf("I saw a booster section!\n");
	while(digital(10) == 0) {	
		create_drive_straight(-50);
		if(digital(10) == 1){
			create_stop();
		}
	}
		
		while(stupid_int == 0) {
			if (digital(10) == 1){
					set_servo_position(0,CLOSE_POSITION_FOR_BOOSTER_SECTIONS);	
					stupid_int == 1;
					msleep(200);
				printf("I have grabbed a booster section!\n");
			}
		}
		move_to_position(0, -900, UPPOSITION);
		set_create_total_angle(0);
	while(get_create_total_angle() < angle_of_pole){
		create_spin_CCW(75);
	}
	
	//---------------------
	//end part 2
	
	set_create_total_angle(0);
// part 3: go forward until i find B2 (not complete)
	
	set_servo_position(1,OPEN_POSITION); 
	angle_of_pole = 60;
//end part 3
	
	//part 4: repeat everything
	/*while(get_create_total_angle() > -80){
		create_spin_CW(75);
	}
	
	//---------------------
	//end part 1
	camera_update();
	sleep(2);
	point2 mcenter = get_object_center(0,0);
	set_servo_position(0,OPEN_POSITION); //this is opening the claw so it won't go to a random position when enabling the servos
//--------------------------------------
// part 2: find booster section using function i wrote at bottom of code then line up with B2(pole)
	
	find_item();
	printf("I saw a booster section!\n");
	while(digital(10) == 0) {	
		create_drive_straight(-50);
		if(digital(10) == 1){
			create_stop();
		}
	}
		
		while(stupid_int == 0) {
			if (digital(10) == 1){
					set_servo_position(0,CLOSE_POSITION_FOR_BOOSTER_SECTIONS);	
					stupid_int == 1;
					msleep(200);
				printf("I have grabbed a booster section!\n");
			}
		}
		move_to_position(0, -900, UPPOSITION);
		set_create_total_angle(0);
	while(get_create_total_angle() < angle_of_pole){
		create_spin_CCW(75);
	}
	
	//---------------------
	//end part 2
	
	set_create_total_angle(0);
// part 3: go forward until i find B2 (not complete)
	
	set_servo_position(1,OPEN_POSITION); 
	angle_of pole = 50;
//end part 3
	while(get_create_total_angle() > -70){
		create_spin_CW(75);
	}
	
	//---------------------
	//end part 1
	camera_update();
	sleep(2);
	point2 mcenter = get_object_center(0,0);
	set_servo_position(0,OPEN_POSITION); //this is opening the claw so it won't go to a random position when enabling the servos
//--------------------------------------
// part 2: find booster section using function i wrote at bottom of code then line up with B2(pole)
	
	find_item();
	printf("I saw a booster section!\n");
	while(digital(10) == 0) {	
		create_drive_straight(-50);
		if(digital(10) == 1){
			create_stop();
		}
	}
		
		while(stupid_int == 0) {
			if (digital(10) == 1){
					set_servo_position(0,CLOSE_POSITION_FOR_BOOSTER_SECTIONS);	
					stupid_int == 1;
					msleep(200);
				printf("I have grabbed a booster section!\n");
			}
		}
		move_to_position(0, -900, UPPOSITION);
		set_create_total_angle(0);
	while(get_create_total_angle() < angle_of_pole){
		create_spin_CCW(75);
	}
	
	//---------------------
	//end part 2
	
	set_create_total_angle(0);
// part 3: go forward until i find B2 (not complete)
	
	set_servo_position(1,OPEN_POSITION); 
	angle_of pole = 50;
//end part 3
	while(get_create_total_angle() > -100){
		create_spin_CW(75);
	}
	
	//---------------------
	//end part 1
	camera_update();
	sleep(2);
	point2 mcenter = get_object_center(0,0);
	set_servo_position(0,OPEN_POSITION); //this is opening the claw so it won't go to a random position when enabling the servos
//--------------------------------------
// part 2: find booster section using function i wrote at bottom of code then line up with B2(pole)
	
	find_item();
	printf("I saw a booster section!\n");
	while(digital(10) == 0) {	
		create_drive_straight(-50);
		if(digital(10) == 1){
			create_stop();
		}
	}
		
		while(stupid_int == 0) {
			if (digital(10) == 1){
					set_servo_position(0,CLOSE_POSITION_FOR_BOOSTER_SECTIONS);	
					stupid_int == 1;
					msleep(200);
				printf("I have grabbed a booster section!\n");
			}
		}
		move_to_position(0, -900, UPPOSITION);
		set_create_total_angle(0);
	while(get_create_total_angle() < angle_of_pole){
		create_spin_CCW(75);
	}
	
	//---------------------
	//end part 2
	
	set_create_total_angle(0);
// part 3: go forward until i find B2 (not complete)
	
	set_servo_position(1,OPEN_POSITION); 
	angle_of pole = 40;
//end part 3
	while(get_create_total_angle() > -60){
		create_spin_CW(75);
	}
	
	//---------------------
	//end part 1
	camera_update();
	sleep(2);
	point2 mcenter = get_object_center(0,0);
	set_servo_position(0,OPEN_POSITION); //this is opening the claw so it won't go to a random position when enabling the servos
//--------------------------------------
// part 2: find booster section using function i wrote at bottom of code then line up with B2(pole)
	
	find_item();
	printf("I saw a booster section!\n");
	while(digital(10) == 0) {	
		create_drive_straight(-50);
		if(digital(10) == 1){
			create_stop();
		}
	}
		
		while(stupid_int == 0) {
			if (digital(10) == 1){
					set_servo_position(0,CLOSE_POSITION_FOR_BOOSTER_SECTIONS);	
					stupid_int == 1;
					msleep(200);
				printf("I have grabbed a booster section!\n");
			}
		}
		move_to_position(0, -900, UPPOSITION);
		set_create_total_angle(0);
	while(get_create_total_angle() < angle_of_pole){
		create_spin_CCW(75);
	}
	
	//---------------------
	//end part 2
	
	set_create_total_angle(0);
// part 3: go forward until i find B2 (not complete)
	
	set_servo_position(1,OPEN_POSITION); 
	angle_of pole = 60;
//end part 3*/
//end part 4
}
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 ();

}
Example #23
0
int main() 
{
	create_connect();
	create_drive_straight(1000);
	sleep(2);
}
Example #24
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;
}
void CenterCamera() //CAMERA MUST BE AT CENTER OF CREATE, POINTING Right RESPECT TO CREATE
{
	track_update();
	if (track_x (0,0) > 81)
	{
		create_drive_straight(-200);
		while (track_x (0,0) > 81)	//WHAT TRACK # IS RED? MAKE SURE IT DOESNT DETECT BOTGUY
		{
			track_update();
		}
		create_stop();
		drivemm(inchestomm(4.9), 1);
		set_create_total_angle(0);
		create_drive(101.3,1);  //uses int. does it round?
		sleep(2.057252314);
		create_stop();
		get_create_total_angle(.1);
		printf("asdb");
		if (get_create_total_angle(.1) != 90)
		{
			set_create_total_angle(get_create_total_angle(.1) - 90);
			printf("%f\n",get_create_total_angle(.1));
			if (get_create_total_angle(.1) > 0) //CCW ADDS TO TOTAL ANGLE
			{
				create_spin_CW(100);
				while (get_create_total_angle(.1) < 0);
				{
					printf("aeqweqw");
				}
				create_stop();
			}
			if (get_create_total_angle(.1) < 0)
			{
				create_spin_CCW(100);
				while (get_create_total_angle(.1) > 0);
				{
					printf("BBBB");
				}
				create_stop();
			}
		}
	}
	/* (track_x (0,0) < 79)
	{
	create_drive_straight(200);
	while (track_x (0,0) < 79)
	{
	track_update();
	}
	create_stop();
	beep();
	set_create_distance(0);
	drivemm(-inchestomm(5), 1);
	create_drive(206.972051,1);
	sleep(1);
	create_stop();
	create_drive_straight(-200);
	while(digital(8 == 0))
	{
	}
	create_stop();
	}*/
}
int drive(x)
{
	create_drive_straight(x);
}
Example #27
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 ();
}
Example #28
0
void Cubes (void)
{
	if (Side == 0)
	{
		//Right of Botguy
		//keep offense in box until scorer gets into position
		//start with claw facing out
		ForwardTouch(); //until it hits luggage    DOES BAR TOUCH SENSOR REGISTER LUGGAGE TOUCH?
		TurnCCW();		//turn left -- now facing airplanes
		CenterCamera();	//move forwards and center the camera on the blocks -- now facing airplanes and in front of blocks
		TurnCCW();		//turn left to face blocks with claw -- now in front of and facing blocks
		ForwardTouch();	//move forward until it hits blocks
		Grab();			//grab blocks
		/*create_drive_straight(100);
		sleep(2);//to avoid crashing with our luggage carts
		create_stop();
		TurnCCW();*/
		//OFFENSE SHOULD BE OUT OF STARTING BOX BY NOW
		//FORWARD BAR SENSOR SHOULD NOT BE OBSTRUCTED
		//CREATE SHOULD DRIVE BACKWARDS
		GotoBox();
		create_drive_straight(200);
		sleep(1);
		create_stop();
		TurnCW();
		create_drive_straight(-300);
		sleep(1);
		create_stop();
		TurnCW();
		CenterCamera(); 
		TurnCCW();
		ForwardTouch();
		Grab();
		TurnCCW();
		create_drive_straight(-500);
		sleep(2.2352);
		TurnCW();
		GotoBox();
		Release();
	}
	if (Side == 1) //Left of Botguy. START WITH CREATE FACING OUT
	{
		ForwardTouch(); //Switch to distance
		TurnCW();
		ForwardTouch();
		create_drive_straight(50);
		sleep(1);
		create_stop();
		TurnCW();
		CenterCamera();
		TurnCCW();
		ForwardTouch();
		Grab();
		TurnCCW();
		/*create_drive_straight(100);
		sleep(1);
		create_stop();
		TurnCW();*/
		while (get_create_lbump(.1) == 0 && get_create_rbump(.1) == 0)
		{
			create_drive_straight(200); //unless arm is strong enough to hold
		}
		ClearWall();
		TurnCW();
		ForwardTouch();
		create_drive_straight(132);  //one cube length (5 inches) + 5 mm safety
		sleep(1);
		create_stop();
		create_drive_straight(-75);
		sleep(1);
		create_stop();
		Release();
		
	}
}