Beispiel #1
0
void Right_AngleMAV(int Direction, double timeLimit){
	long rightClicksRight=0.6*TICKS_PER_REVOLUTION_RIGHT;
	long leftClicksRight=0.6*TICKS_PER_REVOLUTION_LEFT;
	
	clear_motor_position_counter(LEFT_PORT_WHEEL);
	clear_motor_position_counter(RIGHT_PORT_WHEEL);

	double startTURN90Time = seconds();
	while((seconds() - startTURN90Time <= timeLimit) && (get_motor_position_counter(LEFT_PORT_WHEEL) < leftClicksRight) && (get_motor_position_counter(LEFT_PORT_WHEEL) > -leftClicksRight)){	
	if(Direction==RIGHT){
		mrp(RIGHT_PORT_WHEEL, (int)CRUISING_SPEED_TURNS_RIGHT, -rightClicksRight);
		mrp(LEFT_PORT_WHEEL, (int)CRUISING_SPEED_TURNS_LEFT, leftClicksRight);
		bmd(RIGHT_PORT_WHEEL);
		bmd(LEFT_PORT_WHEEL);
	}
	else if(Direction==LEFT){
		mrp(RIGHT_PORT_WHEEL, (int)CRUISING_SPEED_TURNS_RIGHT, rightClicksRight);
		mrp(LEFT_PORT_WHEEL, (int)CRUISING_SPEED_TURNS_LEFT, -leftClicksRight);
		bmd(RIGHT_PORT_WHEEL);
		bmd(LEFT_PORT_WHEEL);
	}
	else{
		printf("Invalid input. LEFT and RIGHT globals accepted only\n");
	}
	}//Close While
}
Beispiel #2
0
void Forty_Five_Degree_AngleMAV(int Direction, double timeLimit){
	long rightClicksRight=0.35*TICKS_PER_REVOLUTION_RIGHT; //39
	long leftClicksRight=0.35*TICKS_PER_REVOLUTION_LEFT;
		
	clear_motor_position_counter(LEFT_PORT_WHEEL);
	clear_motor_position_counter(RIGHT_PORT_WHEEL);
	
	if(Direction==RIGHT){
		double startTURN45Time = seconds();
		while((seconds() - startTURN45Time <= timeLimit) && ((get_motor_position_counter(LEFT_PORT_WHEEL) < leftClicksRight) && (get_motor_position_counter(RIGHT_PORT_WHEEL) > -rightClicksRight))){
			
			mav(RIGHT_PORT_WHEEL, -CRUISING_SPEED_TURNS);
			mav(LEFT_PORT_WHEEL, CRUISING_SPEED_TURNS);
			msleep(35);
		
		}
	}
	else if(Direction==LEFT){
		double startTURN45Time = seconds();
		while((seconds() - startTURN45Time <= timeLimit) && ((get_motor_position_counter(RIGHT_PORT_WHEEL) < rightClicksRight) && (get_motor_position_counter(LEFT_PORT_WHEEL) > -leftClicksRight))){
			
			mav(RIGHT_PORT_WHEEL, CRUISING_SPEED_TURNS);
			mav(LEFT_PORT_WHEEL, -CRUISING_SPEED_TURNS);
			msleep(35);
		
		}
	}
	else{
		printf("Invalid input. LEFT and RIGHT globals accepted only\n");
	}
}
Beispiel #3
0
void turnRight(){
	clear_motor_position_counter(LM);
	clear_motor_position_counter(RM);
	mtp(LM, 1500,HALF_CIRCLE);
	mtp(RM, -1500, -1 * HALF_CIRCLE);
	bmd(LM);
	bmd(RM);
}
Beispiel #4
0
void turnBack(){
	clear_motor_position_counter(LM);
	clear_motor_position_counter(RM);
	mtp(LM, 1500, ONE_CIRCLE);
	mtp(RM, -1500, -1 * ONE_CIRCLE);
	bmd(LM);
	bmd(RM);
}
Beispiel #5
0
// move:
void moveStraight(int distance)
{
	clear_motor_position_counter(c_ileftMotor);
	clear_motor_position_counter(c_irightMotor);
	mtp(c_ileftMotor, c_iforwardSpeed, distance);
	mtp(c_irightMotor, c_iforwardSpeed, distance);
	bmd(c_ileftMotor);
	bmd(c_irightMotor);
}
void lego_drive_until_black_line(int direction, int speed, int line_size) {
	int left_ticks_traveled = 0;
	int right_ticks_traveled = 0;
	float right_error;
	float left_error;
	float desired_value;
	
	int left_threshold;
	int right_threshold;
	
	int right_sensor_value;
	int left_sensor_value;
	
	int left_multiplier;
	int right_multiplier;
	
	if (line_size == SMALL_LINE) {
		left_threshold = LEFT_TOPHAT_SMALL_BLACK;
		right_threshold = RIGHT_TOPHAT_SMALL_BLACK;
	} else {
		left_threshold = LEFT_TOPHAT_LARGE_BLACK;
		right_threshold = RIGHT_TOPHAT_LARGE_BLACK;
	}
	
	clear_motor_position_counter(LEFT_MOTOR);
	clear_motor_position_counter(RIGHT_MOTOR);
	
	motor(LEFT_MOTOR, speed * direction);
	motor(RIGHT_MOTOR, speed * direction);
	
	while(TRUE) {
		left_sensor_value = analog(LEFT_TOPHAT);
		right_sensor_value = analog(RIGHT_TOPHAT);
		if (left_sensor_value > left_threshold || right_sensor_value > right_threshold) {
			break;
		}
		
		left_ticks_traveled = get_motor_position_counter(LEFT_MOTOR);
		right_ticks_traveled = get_motor_position_counter(RIGHT_MOTOR);
		desired_value = (left_ticks_traveled + right_ticks_traveled) / 2.0;
		
		left_error = desired_value - left_ticks_traveled;
		right_error = desired_value - right_ticks_traveled;
		
		
		left_multiplier = (int) ((left_error * l_kP) + 0.5);
		right_multiplier = (int) ((right_error * r_kP) + 0.5);
		
		motor(LEFT_MOTOR, (speed * direction) + left_multiplier);
		motor(RIGHT_MOTOR, (speed * direction) + right_multiplier);
	}
	freeze(LEFT_MOTOR);
	freeze(RIGHT_MOTOR);
}
Beispiel #7
0
void collectBall(){
	motor(FM, FM_COLLECT);
	clear_motor_position_counter(LM);
	clear_motor_position_counter(RM);
	mtp(LM, LM_SP_STR2, 800);
	mtp(RM, RM_SP_STR2, 800);
	bmd(LM);
	bmd(RM);
	motor(FM, 0);
	// move and collect rest balls
}
Beispiel #8
0
void collectBall(){
	motor(FM, FM_COLLECT);
	clear_motor_position_counter(LM);
	clear_motor_position_counter(RM);
	mtp(LM, 1500, 1000);
	mtp(RM, 1500, 1000);
	bmd(LM);
	bmd(RM);
	motor(FM, 0);
	msleep(2500);
	// move and collect rest balls
}
Beispiel #9
0
int main(int argc, char** argv) {
    clear_motor_position_counter(1);
    clear_motor_position_counter(2);
    motor(1, 30);
    motor(2, 30);
    while (!side_button()) {
        printf("pos: %i/%i\n", get_motor_position_counter(1), get_motor_position_counter(2));
        msleep(10);
    }
    alloff();
    return 0;
}
void lego_drive_centimeters(float centimeters, int direction, int speed) {
	int left_ticks_traveled = 0;
	int right_ticks_traveled = 0;
	int ticks = (int) (centimeters * TICKS_PER_CENTIMETER);
	
	float desired_value;
	
	int left_multiplier;
	int right_multiplier;
	
	error current_error, previous_error, error_delta, errors_sum;
	
	clear_motor_position_counter(LEFT_MOTOR);
	clear_motor_position_counter(RIGHT_MOTOR);
	
	previous_error.left = 0;
	previous_error.right = 0;
	errors_sum.left = 0;
	errors_sum.right = 0;
	
	motor(LEFT_MOTOR, speed * direction);
	motor(RIGHT_MOTOR, speed * direction);
	
	while(TRUE) {
		if (abs(left_ticks_traveled) >= ticks || abs(right_ticks_traveled) >= ticks) {
			break;
		}
		
		left_ticks_traveled = get_motor_position_counter(LEFT_MOTOR);
		right_ticks_traveled = get_motor_position_counter(RIGHT_MOTOR);
		desired_value = (left_ticks_traveled + right_ticks_traveled) / 2.0;
		
		current_error.left = desired_value - left_ticks_traveled;
		current_error.right = desired_value - right_ticks_traveled;
		error_delta.left = current_error.left - previous_error.left;
		error_delta.right = current_error.right - previous_error.right;
		errors_sum.left = (errors_sum.left * 0.7) +	current_error.left;
		errors_sum.right = (errors_sum.right * 0.7) + current_error.right;
		
		left_multiplier = (int) (((current_error.left * l_kP) + 0.5) + ((errors_sum.left * l_kI) + 0.5) + ((error_delta.left * l_kD) + 0.5));
		right_multiplier = (int) (((current_error.right * r_kP) + 0.5) + ((errors_sum.right * r_kI) + 0.5) + ((error_delta.right * r_kD) + 0.5));
		
		motor(LEFT_MOTOR, (speed * direction) + left_multiplier);
		motor(RIGHT_MOTOR, (speed * direction) + right_multiplier);
		
		previous_error = current_error;
	}
	freeze(LEFT_MOTOR);
	freeze(RIGHT_MOTOR);
	msleep(100);
}
Beispiel #11
0
void _arm_hold()
{
	clear_motor_position_counter(1);
	clear_motor_position_counter(3);
	while (1)
	{
		mav(1, 100);
		mav(3, 100);
		msleep(1);
		mav(1, -100);
		mav(3, -100);
		msleep(1);
	}
}
Beispiel #12
0
void release() { // open claw until touch sensor #14 is activated
	mav(3, -400);
	while(digital(14) ==0) {
	}
	off(3);
	clear_motor_position_counter(3);
}
void servo_booster(int servo_port, int motor_port, int servo_position, int motor_position)
{
	clear_motor_position_counter(motor_port);
	motor(motor_port, 100);
	set_servo_position(servo_port, servo_position);
	while (get_motor_position_counter(motor_port) < motor_position);
	off(motor_port);
}
/*
 * Class:     Motor
 * Method:    clear_motor_position_counter
 * Signature: (I)I
 */
JNIEXPORT jint JNICALL Java_cbccore_low_Motor_clear_1motor_1position_1counter(JNIEnv* env, jobject obj, jint port)
{
#ifdef CBC
    return clear_motor_position_counter(port);
#else
    printf("Java_cbccore_low_Motor_clear_1motor_1position_1counter stub\n");
    return -1;
#endif
}
void lego_drive_until_opening(int direction, int speed) {
	int left_ticks_traveled = 0;
	int right_ticks_traveled = 0;
	float right_error;
	float left_error;
	float desired_value;
	
	int left_multiplier;
	int right_multiplier;
	
	int left_sensor_value;
	int right_sensor_value;
	
	clear_motor_position_counter(LEFT_MOTOR);
	clear_motor_position_counter(RIGHT_MOTOR);
	
	motor(LEFT_MOTOR, speed * direction);
	motor(RIGHT_MOTOR, speed * direction);
	
	while(TRUE) {
		left_sensor_value = analog_et(LEFT_ET);
		right_sensor_value = analog_et(RIGHT_ET);
		
		if (left_sensor_value < ET_NO_WALL && right_sensor_value < ET_NO_WALL) {
			break;
		}
		
		left_ticks_traveled = get_motor_position_counter(LEFT_MOTOR);
		right_ticks_traveled = get_motor_position_counter(RIGHT_MOTOR);
		desired_value = (left_ticks_traveled + right_ticks_traveled) / 2.0;
		
		left_error = desired_value - left_ticks_traveled;
		right_error = desired_value - right_ticks_traveled;
		
		
		left_multiplier = (int) ((left_error * l_kP) + 0.5);
		right_multiplier = (int) ((right_error * r_kP) + 0.5);
		
		motor(LEFT_MOTOR, (speed * direction) + left_multiplier);
		motor(RIGHT_MOTOR, (speed * direction) + right_multiplier);
	}
	freeze(LEFT_MOTOR);
	freeze(RIGHT_MOTOR);
}
Beispiel #16
0
void LinkDrive(float distance, float speed) {
  mav(RIGHT_PORT, speed*LINK_RIGHT_OFFSET);
  mav(LEFT_PORT, speed*LINK_LEFT_OFFSET);
  clear_motor_position_counter(RIGHT_PORT);

  while(get_motor_position_counter(RIGHT_PORT) < distance*CM_TO_TICKS) {
  }

  ao();
}
Beispiel #17
0
void turn_right()
{

	motor(0,0);
	motor(2,0);
	clear_motor_position_counter(0);
	mtp(0, 1000, 1050); 
	block_motor_done(0);

}
Beispiel #18
0
void turn_right()
{

	motor(0,0);
	motor(2,0);
	clear_motor_position_counter(0);
	mtp(0, 1000, 850); // this is what got changed from 1050 to 950 to 850
	block_motor_done(0);

}
Beispiel #19
0
void turn_left()
{

	motor(0,0);
	motor(2,0);
	clear_motor_position_counter(2);
	mtp(2, 1500, 1300);
	block_motor_done(2);

}
void lego_spin_degrees(int degrees, int direction, int speed) {
	int left_ticks_traveled = 0;
	int right_ticks_traveled = 0;
	float right_error;
	float left_error;
	float desired_value;
	
	int left_multiplier;
	int right_multiplier;
	
	int ticks = (int) ((float) (degrees) * TICKS_PER_DEGREE);
	
	clear_motor_position_counter(LEFT_MOTOR);
	clear_motor_position_counter(RIGHT_MOTOR);
	
	motor(LEFT_MOTOR, speed * direction);
	motor(RIGHT_MOTOR, -speed * direction);
	
	while(TRUE) {
		if (abs(left_ticks_traveled) >= ticks || abs(right_ticks_traveled) >= ticks) {
			break;
		}
		
		left_ticks_traveled = abs(get_motor_position_counter(LEFT_MOTOR));
		right_ticks_traveled = abs(get_motor_position_counter(RIGHT_MOTOR));
		desired_value = (left_ticks_traveled + right_ticks_traveled) / 2.0;
		
		left_error = desired_value - left_ticks_traveled;
		right_error = desired_value - right_ticks_traveled;
		
		
		left_multiplier = (int) ((left_error * spin_l_kP) + 0.5);
		right_multiplier = (int) ((right_error * spin_r_kP) + 0.5);
		
		motor(LEFT_MOTOR, (speed * direction) + left_multiplier);
		motor(RIGHT_MOTOR, (-speed * direction) + right_multiplier);
	}
	freeze(LEFT_MOTOR);
	freeze(RIGHT_MOTOR);
	
	msleep(100);
}
Beispiel #21
0
void moveFor(){
	int frontDis = analog_et(FSS);
	int backDis = analog_et(BSS);
	int disDiff = frontDis - backDis;
	int turnLM = MOVE_FOR_TP - disDiff * KDp;
	int turnRM = MOVE_FOR_TP + disDiff * KDp;
	
	while(frontDis > 295){
		motor(LM, turnLM);
		motor(RM, turnRM);
		frontDis = analog_et(FSS);
		backDis = analog_et(BSS);
		printf("%d\t%d\n", frontDis, backDis);
		disDiff = frontDis - backDis;
		turnLM = MOVE_BACK_TP - disDiff * KDp;
		turnRM = MOVE_BACK_TP + disDiff * KDp;
	}
	clear_motor_position_counter(LM);
	clear_motor_position_counter(RM);
	mtp(LM, 800, 500);
	mtp(RM, 800, 500);
	bmd(LM);
	bmd(RM);
	clear_motor_position_counter(LM);
	clear_motor_position_counter(RM);
	mtp(LM, 800, 260);
	mtp(RM, -800, -260);
	bmd(LM);
	bmd(RM);
	while(analog(RSS) < RSS_OFFSET){
		motor(RM, 82);
		motor(LM, 90);
	}
	while(analog(RSS) > RSS_OFFSET){
		motor(RM, 82);
		motor(LM, 90);
	}
	while(analog(RSS) < RSS_OFFSET){
		motor(RM, 82);
		motor(LM, 90);
	}
}
void lego_spin_until(int direction, int speed, int sensor, int (*comparator)(int, int), int threshold) {
	int left_ticks_traveled = 0;
	int right_ticks_traveled = 0;
	float right_error;
	float left_error;
	float desired_value;
	
	int left_multiplier;
	int right_multiplier;
	
	clear_motor_position_counter(LEFT_MOTOR);
	clear_motor_position_counter(RIGHT_MOTOR);
	
	motor(LEFT_MOTOR, speed * direction);
	motor(RIGHT_MOTOR, -speed * direction);
	
	while(TRUE) {
		if (comparator(analog(sensor), threshold)) {
			break;
		}
		display_printf(0, 0, "%4i", analog(sensor));
		
		left_ticks_traveled = abs(get_motor_position_counter(LEFT_MOTOR));
		right_ticks_traveled = abs(get_motor_position_counter(RIGHT_MOTOR));
		desired_value = (left_ticks_traveled + right_ticks_traveled) / 2.0;
		
		left_error = desired_value - left_ticks_traveled;
		right_error = desired_value - right_ticks_traveled;
		
		
		left_multiplier = (int) ((left_error * spin_l_kP) + 0.5);
		right_multiplier = (int) ((right_error * spin_r_kP) + 0.5);
		
		motor(LEFT_MOTOR, (speed * direction) + left_multiplier);
		motor(RIGHT_MOTOR, (-speed * direction) + right_multiplier);
	}
	freeze(LEFT_MOTOR);
	freeze(RIGHT_MOTOR);
}
void operate_winch(int position) {
	raise_winch();
	clear_motor_position_counter(WINCH_MOTOR);
	if (position == WINCH_GROUND_POSITION) {
		motor(WINCH_MOTOR, 100);
		while (get_motor_position_counter(WINCH_MOTOR) < WINCH_GROUND_POSITION);
		freeze(WINCH_MOTOR);
	} else if (position == WINCH_MIDDLE_POSITION) {
		motor(WINCH_MOTOR, 100);
		while (get_motor_position_counter(WINCH_MOTOR) < WINCH_MIDDLE_POSITION);
		freeze(WINCH_MOTOR);
	}
	adjust_motor(WINCH_MOTOR, position);
}
Beispiel #24
0
int main()
{
	printf("Starting test of Blue and Green gripper.\n");
	set_servo_position(BLUE_GRIPPER, BLUE_GRIPPER_START_POSITION);
	set_servo_position(GREEN_GRIPPER, GREEN_GRIPPER_START_POSITION);
	enable_servos();
	press_A_to_continue();
	
	set_servo_position(BLUE_GRIPPER, BLUE_GRIPPER_CLOSED_POSITION);
	press_A_to_continue();
	
	set_servo_position(GREEN_GRIPPER, GREEN_GRIPPER_CLOSED_POSITION);
	press_A_to_continue();
	
	printf("Starting test of spinner.\n");
	
	clear_motor_position_counter(BLUE_SPINNER);
	motor(BLUE_SPINNER, 100);
	while (get_motor_position_counter(BLUE_SPINNER) < 225) ;
	off(BLUE_SPINNER);
	press_A_to_continue();
	
	clear_motor_position_counter(BLUE_SPINNER);
	motor(BLUE_SPINNER, 100);
	while (get_motor_position_counter(BLUE_SPINNER) < 225) ;
	off(BLUE_SPINNER);
	press_A_to_continue();
	
	clear_motor_position_counter(BLUE_SPINNER);
	motor(BLUE_SPINNER, 100);
	while (get_motor_position_counter(BLUE_SPINNER) < 225) ;
	off(BLUE_SPINNER);
	press_A_to_continue();
	
	return 0;
}
void main ()
{
	int loop_done = 0;
	int task_A_done = 0;
	int task_B_done = 0;
	int angle0 = 0;
	
	
	//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);
	
	angle0 = get_create_total_angle (.1);
	create_drive_direct (150,-150);
	
	move_to_position (0, -900, -1550);
	
	while(! loop_done)
	{
		printf("a0: %d, a: %d, d: %d\n", angle0, get_create_total_angle (.1), task_A_done);
		if (angle0 - get_create_total_angle (.1) < 90)
		{
			create_drive_direct (150,-150);
		}
		else
		{
			task_A_done = 1;
		}
		if (get_motor_done (0))
		{
			
		}
		else
		{
			task_B_done = 1;
		}
		if (task_A_done && task_B_done)
		{
			loop_done = 1;
		}
		msleep(0.1);
	}
	
	create_stop ();
	create_disconnect ();
	
	//put specific commands to control robot in here
//	loop_done = 0;
//	task_A_done = 0;
//	task_B_done = 0;
//	while(! loop_done)
//	{
//		if (/*task A is not done*/)
//		{
//			//continue doing task A
//		}
//		else
//		{
//			task_A_done = 1;
//		}
//		if (/*task B is not done*/)
//		{
//			//continue doing task B
//		}
//		else
//		{
//			task_B_done = 1;
//		}
//		if (task_A_done && task_B_done)
//		{
//			loop_done = 1;
//		}
//	}
}
void lego_spin_onto_black_line(int direction, int speed, int low_threshold, int high_threshold) {
	int left_ticks_traveled = 0;
	int right_ticks_traveled = 0;
	float right_error;
	float left_error;
	float desired_value;
	
	int left_multiplier;
	int right_multiplier;
	
	int i;
	
	clear_motor_position_counter(LEFT_MOTOR);
	clear_motor_position_counter(RIGHT_MOTOR);
	
	motor(LEFT_MOTOR, speed * direction);
	motor(RIGHT_MOTOR, -speed * direction);
	
	for (i = 0; i < 3; i++) {
		while(TRUE) {
			if (analog_et(RIGHT_ET) >= low_threshold) {
				break;
			}
			
			left_ticks_traveled = abs(get_motor_position_counter(LEFT_MOTOR));
			right_ticks_traveled = abs(get_motor_position_counter(RIGHT_MOTOR));
			desired_value = (left_ticks_traveled + right_ticks_traveled) / 2.0;
		
			left_error = desired_value - left_ticks_traveled;
			right_error = desired_value - right_ticks_traveled;
		
		
			left_multiplier = (int) ((left_error * spin_l_kP) + 0.5);
			right_multiplier = (int) ((right_error * spin_r_kP) + 0.5);
		
			motor(LEFT_MOTOR, (speed * direction) + left_multiplier);
			motor(RIGHT_MOTOR, (-speed * direction) + right_multiplier);
			display_printf(0, 0, "L: %4i", ((speed * direction) + left_multiplier));
			display_printf(0, 1, "R: %4i", ((speed * direction) + right_multiplier));
		}
		msleep(10);
		while(TRUE) {
			if (analog_et(RIGHT_ET) <= high_threshold) {
				break;
			}
			left_ticks_traveled = abs(get_motor_position_counter(LEFT_MOTOR));
			right_ticks_traveled = abs(get_motor_position_counter(RIGHT_MOTOR));
			desired_value = (left_ticks_traveled + right_ticks_traveled) / 2.0;
		
			left_error = desired_value - left_ticks_traveled;
			right_error = desired_value - right_ticks_traveled;
		
		
			left_multiplier = (int) ((left_error * spin_l_kP) + 0.5);
			right_multiplier = (int) ((right_error * spin_r_kP) + 0.5);
		
			motor(LEFT_MOTOR, (-speed * direction) + left_multiplier);
			motor(RIGHT_MOTOR, (speed * direction) + right_multiplier);
			display_printf(0, 0, "L: %4i", ((speed * direction) + left_multiplier));
			display_printf(0, 1, "R: %4i", ((speed * direction) + right_multiplier));
		}
	}
	freeze(LEFT_MOTOR);
	freeze(RIGHT_MOTOR);
}
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 ();

}
Beispiel #28
0
void MoveForwardMillimeters(int mmToGo, int Speed)
{ 
	//Calculate the number of ticks to go
	float RightTicksToGo = TICKS_PER_REVOLUTION_RIGHT*(mmToGo/MILLIMETERS_PER_REVOLUTION);
	float LeftTicksToGo = TICKS_PER_REVOLUTION_LEFT*(mmToGo/MILLIMETERS_PER_REVOLUTION);
	
	//Calculate mm per tick
	float RightTicksPerMM = TICKS_PER_REVOLUTION_RIGHT/MILLIMETERS_PER_REVOLUTION;
	float LeftTicksPerMM = TICKS_PER_REVOLUTION_LEFT/MILLIMETERS_PER_REVOLUTION;
	
	//Set Motor Ticks position to zero
	clear_motor_position_counter(LEFT_PORT_WHEEL);
	clear_motor_position_counter(RIGHT_PORT_WHEEL);
	
	//Set Motor Positions to a variable .. should be zero here because we just cleared them
	int LeftMotorStartPosition = get_motor_position_counter(LEFT_PORT_WHEEL);
	int RightMotorStartPosition = get_motor_position_counter(RIGHT_PORT_WHEEL);
	
	//Calculate Motor Final Tick Position for MTP Command
	int LeftMotorFinalPosition = LeftMotorStartPosition + LeftTicksToGo;
	int RightMotorFinalPosition = RightMotorStartPosition + RightTicksToGo;
	
	
	//Get time of start movement
	//Not used yet.. for timeout functionality
	double StartMovementTime = seconds();
		
	//Loop until movment complete as measured by the left wheel
	while(get_motor_position_counter(LEFT_PORT_WHEEL) < LeftMotorFinalPosition) 
	{
		//Calculate Wheel Distance Moved each loop
		int LeftDistanceMoved = (get_motor_position_counter(LEFT_PORT_WHEEL)-LeftMotorStartPosition)/LeftTicksPerMM;
		int RightDistanceMoved = (get_motor_position_counter(RIGHT_PORT_WHEEL)-RightMotorStartPosition)/RightTicksPerMM;
		
		//Calculate Wheel Split
		//Positive Values Right Wheel is Ahead, Negative Values Left Wheel is ahead
		int WheelSplit = RightDistanceMoved - LeftDistanceMoved;
		
		//See if Wheel Split is exceeding the Correction Threshhold in either direction
		if(WheelSplit < CORRECTION_THRESHOLD && WheelSplit > -CORRECTION_THRESHOLD)
		{
			//Wheels are going straight
			motor(RIGHT_PORT_WHEEL, 100);
			motor(LEFT_PORT_WHEEL, 100);
			
			//move_to_position(RIGHT_PORT_WHEEL, Speed, RightMotorFinalPosition);
			//move_to_position(LEFT_PORT_WHEEL, Speed, LeftMotorFinalPosition);
			printf("STRAIGHT: left: %d right: %d split: %d\n", LeftDistanceMoved, RightDistanceMoved, WheelSplit);
		}
		else if(WheelSplit >= CORRECTION_THRESHOLD)
		{
			//Right wheel is ahead of left wheel
			//correction reduction applied to right wheel
			
			motor(RIGHT_PORT_WHEEL, 100*CORRECTION_REDUCTION);
			motor(LEFT_PORT_WHEEL, 100);
			
			//move_to_position(RIGHT_PORT_WHEEL, Speed * CORRECTION_REDUCTION, RightMotorFinalPosition);
			//move_to_position(LEFT_PORT_WHEEL, Speed, LeftMotorFinalPosition);
			printf("CORRECT RIGHT\n");
		}
		else if(WheelSplit <= -CORRECTION_THRESHOLD)
		{
			//Left wheel is ahead of right wheel
			//correction reduction applied to left wheel
			
			motor(RIGHT_PORT_WHEEL, 100);
			motor(LEFT_PORT_WHEEL, 100*CORRECTION_REDUCTION);
			//move_to_position(RIGHT_PORT_WHEEL, Speed, RightMotorFinalPosition);
			//move_to_position(LEFT_PORT_WHEEL, Speed * CORRECTION_REDUCTION, LeftMotorFinalPosition);
			printf("CORRECT LEFT\n");
		}
		msleep(150);
	}
	//Turn All Motors Off 
	ao();
}
Beispiel #29
0
int main()
{
	/* FIRST CALIBRATION: based on poms position adjust starting position
	*/
	
	printf("test 3.11, calibration value: \nclaw down : %d, \nright ang forward right: %d, \nright ang forward left  : %d, \nback right angle %d ...\n", DOWN_SERVO, RIGHT_ANGLE_CLICKS_MRP_RIGHT, RIGHT_ANGLE_CLICKS_MRP_LEFT, RIGHT_ANGLE_CLICKS_BACK_MRP);
	double start_time = seconds();
	enable_servos();
	clear_motor_position_counter(RIGHT_MOTOR);
	clear_motor_position_counter(LEFT_MOTOR);
	wait_for_light(1);
	shut_down_in(118);	
	clawUp();
	//**************************************
	//* PART 1 : get first set of poms to lower storage area
	//*
	//**************************************
	moveBackward(1, NO_DEBUG);
	moveForwardHighSpeed(23, DEBUG);
	clawDown();
	//moveForward(8,NO_DEBUG);
	//we have the Poms
	rightAngleBwd(RIGHT, NO_DEBUG);
	//bump against the upper storage area
	moveBackwardHighSpeed(15, NO_DEBUG);
	moveForwardHighSpeed(17, NO_DEBUG);
	
	rightAngleBwd(RIGHT, NO_DEBUG);
	//bump against upper PVC side
	moveBackwardHighSpeed(18, NO_DEBUG);
	//move towards dropping poms
	moveForward(1.5, NO_DEBUG);
	msleep(300);
	rightAngleFwd(LEFT, NO_DEBUG);
	msleep(300);
	clawUp();
	//uses IR sensor to stop the forward movement
	moveForwardTilBlackLine(15, NO_DEBUG);
	printf("====> elapsed time end of part 1: %f\n", (seconds() - start_time));

	//**************************************
	//* END OF PART 1 : get first set of poms to lower storage area
	//*
	//**************************************

	//**************************************
	//* PART 2 : get the blue cube to the corner
	//*
	//**************************************
	moveBackwardHighSpeed(25, NO_DEBUG);
	//recalibrate against top PVC next to botguy
	rightAngleBwd(LEFT, NO_DEBUG);
	moveBackwardHighSpeed(11, NO_DEBUG);
		
	// SECOND CALIBRATION: based on blackline/blue position
	
	
	moveForward(7.5, NO_DEBUG);
	
	rightAngleFwd(RIGHT, NO_DEBUG);
	//get the cube now

	//====>>>>> may have to be adjust the day of competition based on position of 2nd
	//====>>>>> set of poms
	moveForwardHighSpeed(15, NO_DEBUG);
	//====>>>>>

	clawAlmostDownCube();
	twentyTwoAngleFwd(LEFT, NO_DEBUG);
	clawUp();
	//====>>>>> calibration to get to the corner 22.5 fwd, 22.5 more forward, aiming at bottom left hand corner
	moveForwardHighSpeed(2.5, NO_DEBUG);
	twentyTwoAngleFwd(LEFT, NO_DEBUG);
	//clawAlmostDownCube();
	moveForwardHighSpeed(30, NO_DEBUG);
	
	//====>>>>> 
	//moveForwardHighSpeed(2, NO_DEBUG);
	
	//====>>>>>
	moveBackwardHighSpeed(9, NO_DEBUG);
	fortyFiveAngleFwd(LEFT, NO_DEBUG);

	printf("====> elapsed time end of part 2: %f\n", (seconds() - start_time));
	//**************************************
	//* END OF PART 2 : get the blue cube in the corner
	//*
	//**************************************

	//**************************************
	//* PART 3 : prepare to get second set of poms
	//*
	//**************************************
	//recalibrate against side PVC
	rightAngleBwd(RIGHT, NO_DEBUG);
	moveBackwardHighSpeed(25, NO_DEBUG);		
	moveForward(5, NO_DEBUG);
	//calibrating with top PVC
	rightAngleBwd(LEFT,NO_DEBUG);
	moveBackwardHighSpeed(23,NO_DEBUG);
	// THIRD CALIBRATION: based on second set of Poms
	//below is the number of inches from the side PVC
	//
	moveForwardHighSpeed(10, NO_DEBUG);
	rightAngleBwd(RIGHT, NO_DEBUG);
	//recalibrate with side pvc
	moveBackwardHighSpeed(5,NO_DEBUG);
	printf("====> elapsed time end of part 3: %f\n", (seconds() - start_time));

	//**************************************
	//* END OF PART 3 : prepare to get second set of poms
	//*
	//**************************************
	
	//**************************************
	//* PART 4 : bring second set of poms home
	//*
	//**************************************
	//grabs the poms, bring down claw and go home
	moveForwardHighSpeed(12.5,NO_DEBUG);
	clawDown();
	moveForwardHighSpeed(59.5, NO_DEBUG);
	//here is the code to recalibrate one more time... but do we have enough time?
	/*//recalibrate against top PVC
	rightAngleBwd(LEFT,NO_DEBUG);
	
	moveBackwardHighSpeed(10, NO_DEBUG);
	//move towards dropping poms
	
	moveForwardHighSpeed(10, NO_DEBUG);
	//msleep(500);
	printf("==> moving righ angle bwd\n");
	rightAngleFwd(LEFT, NO_DEBUG);
	*/
	//recalibrate against top PVC
	//moveForwardHighSpeed(41,NO_DEBUG);
	
	//last calibration before dropping second set of poms
	rightAngleBwd(LEFT,NO_DEBUG);
	moveBackwardHighSpeed(14, NO_DEBUG);
	//move towards dropping poms
	moveForwardSlow(1.5, NO_DEBUG);
	msleep(300);
	rightAngleFwd(LEFT, NO_DEBUG);
	msleep(300);
	clawUp();
	moveForwardTilBlackLine(15, DEBUG);

	//**************************************
	//* END OF PART 4 : bring second set of poms home
	//*
	//**************************************

	printf("====> elapsed time: %f\n", (seconds() - start_time));
	ao();
	disable_servos();
	return 0;
}
Beispiel #30
0
void CombinedMotorWidget::clearPosition()
{
	clear_motor_position_counter(ui->motors->currentIndex());
}