Exemple #1
0
//right angle turn function
void rightAngleBwdMav(int direction, int debug ) {
	mav(RIGHT_MOTOR, 0);
	mav(LEFT_MOTOR, 0);
	msleep(100);
	int clickNbrTarget = RIGHT_ANGLE_CLICKS_BACK;
	int initial_position = get_motor_position_counter(RIGHT_MOTOR);
	if (direction == RIGHT) {
		clickNbrTarget = RIGHT_ANGLE_CLICKS_BACK;
		initial_position = get_motor_position_counter(LEFT_MOTOR);
	}
	int current_position = initial_position;
	if (direction == RIGHT) {
		mav(LEFT_MOTOR,SPEED_BWD);
	} else if (direction == LEFT) {
		mav(RIGHT_MOTOR, SPEED_BWD) ;
	}
	while ((current_position - initial_position) >= clickNbrTarget) {
		msleep(25);
		current_position = get_motor_position_counter(RIGHT_MOTOR);
		if (direction == RIGHT) {
			current_position = get_motor_position_counter(LEFT_MOTOR);
		}
		if (debug == DEBUG) {
			printf("right angle bwd mav Init %d , curr %d, tgt %d\n", initial_position,current_position , clickNbrTarget );
		}
	} 
	mav(RIGHT_MOTOR, 0);
	mav(LEFT_MOTOR, 0);
	reset_motors();
}
Exemple #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");
	}
}
Exemple #3
0
//right angle turn function
void rightAngleBwdMrp(int direction, int debug ) {
	mav(RIGHT_MOTOR, 0);
	mav(LEFT_MOTOR, 0);
	if (debug == DEBUG) {
		printf("starting right angle mrp bwd \n");
	}
	msleep(100);
	int initial_position = get_motor_position_counter(RIGHT_MOTOR);
	if (direction == RIGHT) {
		initial_position = get_motor_position_counter(LEFT_MOTOR);
	}
	int current_position = initial_position;
	if (direction == RIGHT) {
		mrp(LEFT_MOTOR, SPEED_BWD, RIGHT_ANGLE_CLICKS_BACK_MRP);
		bmd(LEFT_MOTOR);
		current_position = get_motor_position_counter(LEFT_MOTOR);
	} else if (direction == LEFT) {
		mrp(RIGHT_MOTOR, SPEED_BWD, RIGHT_ANGLE_CLICKS_BACK_MRP);
		bmd(RIGHT_MOTOR);
		current_position = get_motor_position_counter(RIGHT_MOTOR);
	}
		if (debug == DEBUG) {
			printf("right angle mrp bwd Init %d , curr %d, tgt %d\n", initial_position,current_position, RIGHT_ANGLE_CLICKS_BACK_MRP);
	}
	mav(RIGHT_MOTOR, 0);
	mav(LEFT_MOTOR, 0);
	reset_motors(); 
	
	
}
Exemple #4
0
//right angle turn function
void rightAngleFwdMrp(int direction, int debug ) {
	mav(RIGHT_MOTOR, 0);
	mav(LEFT_MOTOR, 0);
	msleep(100);
	int initial_position = get_motor_position_counter(RIGHT_MOTOR);
	if (direction == RIGHT) {
		initial_position = get_motor_position_counter(LEFT_MOTOR);
	}
	int current_position = initial_position;
	if (direction == RIGHT) {
		mrp(LEFT_MOTOR, SPEED_FWD, RIGHT_ANGLE_CLICKS_MRP_RIGHT);
		bmd(LEFT_MOTOR);
		if (debug == DEBUG) {
			printf("right angle mrp fwd RIGHT Init %d , curr %d, tgt %d\n", initial_position,current_position, RIGHT_ANGLE_CLICKS_MRP_RIGHT);
		}
		current_position = get_motor_position_counter(LEFT_MOTOR);
	} else if (direction == LEFT) {
		mrp(RIGHT_MOTOR, SPEED_FWD, RIGHT_ANGLE_CLICKS_MRP_LEFT);
		bmd(RIGHT_MOTOR);
		if (debug == DEBUG) {
			printf("right angle mrp fwd LEFT Init %d , curr %d, tgt %d\n", initial_position,current_position, RIGHT_ANGLE_CLICKS_MRP_LEFT);
		}
		current_position = get_motor_position_counter(RIGHT_MOTOR);
	}
		
	mav(RIGHT_MOTOR, 0);
	mav(LEFT_MOTOR, 0);
	reset_motors();
}
void lower_bay(void) {
    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);
}
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);
}
Exemple #7
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 raise_bay(void) {
    while (get_motor_position_counter(kMotorPortBayLeft) > 30 || get_motor_position_counter(kMotorPortBayRight) > 30) {
        motor(kMotorPortBayLeft, -50);
        motor(kMotorPortBayRight, -50);
        msleep(200);
        //motor(kMotorPortBayLeft, 0);
        //motor(kMotorPortBayRight, 0);
        //msleep(200);
        printf("pos: %i/%i\n", get_motor_position_counter(kMotorPortBayLeft), get_motor_position_counter(kMotorPortBayRight));
    }
    motor(kMotorPortBayLeft, 0);
    motor(kMotorPortBayLeft, 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);
}
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);
}
Exemple #11
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
}
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:    get_motor_position_counter
 * Signature: (I)I
 */
JNIEXPORT jint JNICALL Java_cbccore_low_Motor_get_1motor_1position_1counter(JNIEnv* env, jobject obj, jint port)
{
#ifdef CBC
	get_motor_position_counter(port);
#else
	printf("Java_cbccore_low_Motor_enable_1servos 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);
}
Exemple #15
0
double SensorsWidget::rawValue(const int &i) const
{
	double val = 0;
	if(i < 8) val = analog10(i);
	else if(i < 12) val = get_motor_position_counter(i - 8);
	else if(i == 12) val = accel_x();
	else if(i == 13) val = accel_y();
	else if(i == 14) val = accel_z();
	return val;
}
Exemple #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();
}
Exemple #17
0
void CombinedMotorWidget::update()
{
#ifdef A_KOVAN
	publish();
#endif
	int port = ui->motors->currentIndex();
	ui->position->setText(QString::number(get_motor_position_counter(port)));
	ui->positionStop->setEnabled(get_motor_done(port) ? false : true);
	ui->go->setEnabled(get_motor_done(port) ? true : false);
}
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);
}
Exemple #19
0
void backward(float distance){
	if(distance < 0.) {
		forward(-distance);
      	return;
    }

  	// Calculate the # of ticks the robot must move for each wheel
	long ticks = CMtoBEMF(distance);
	long totalLeftTicks = get_motor_position_counter(MOT_LEFT) - ticks;
	long totalRightTicks = get_motor_position_counter(MOT_RIGHT) - ticks;

  	// Start motors
	motor(MOT_LEFT, -SPDl);
	motor(MOT_RIGHT, -SPDr);

  	// Keep moving until both motors reach their desired # of ticks
	while(get_motor_position_counter(MOT_LEFT) > totalLeftTicks
          && get_motor_position_counter(MOT_RIGHT) > totalRightTicks) {
		if (get_motor_position_counter(MOT_LEFT) <= totalLeftTicks)
			off(MOT_LEFT);
		if (get_motor_position_counter(MOT_RIGHT) <= totalRightTicks)
			off(MOT_RIGHT);
	}

	off(MOT_LEFT);
  	off(MOT_RIGHT);
}
Exemple #20
0
void moveBackwardRoutine(double distanceInInches,int speed, int debug) {
	//convert inches to clicks
	int clicks =(int) (156.25l * distanceInInches);
	int initial_position_right = get_motor_position_counter(RIGHT_MOTOR);
	int initial_position_left = get_motor_position_counter(LEFT_MOTOR);
	int current_position_right = get_motor_position_counter(RIGHT_MOTOR);
	int current_position_left = get_motor_position_counter(LEFT_MOTOR);
	int differential  = 0 ;
	while (current_position_left >= (initial_position_left - clicks) ||
		current_position_right >= (initial_position_right - clicks) ) {
		//first let's see if one motor is going ahead of the other
		differential = current_position_left - initial_position_left - 
				(current_position_right - initial_position_right);
		if (differential > -25 && differential < 25 ) {
			mav(RIGHT_MOTOR, speed);
			mav(LEFT_MOTOR, speed);
		} else if (differential > 0 ) {
			mav(RIGHT_MOTOR, (int) (speed*ADJUST_SPEED));
			mav(LEFT_MOTOR, (speed * 1.1));
		} else {
			mav(RIGHT_MOTOR, (speed * 1.1));
			mav(LEFT_MOTOR, (int) (speed*ADJUST_SPEED));
		}
		msleep(25);
		current_position_right = get_motor_position_counter(RIGHT_MOTOR);
		current_position_left = get_motor_position_counter(LEFT_MOTOR);
	}
	//turn off motors completely
	mav(RIGHT_MOTOR, 0);
	mav(LEFT_MOTOR,0);
	ao();
	reset_motors();
}
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);
}
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;
}
Exemple #23
0
double PidTunerWidget::getFeedbackValue()
{
#ifndef NOT_A_KOVAN
	publish();
#endif
	int position = get_motor_position_counter(ui->motor->currentIndex());

	double vel = (1.0-LPF_ALPHA) * m_vel_1
		+ LPF_ALPHA * (1000 / UPDATE_MS) * (position - m_position_1) / MOTOR_SCALING;

	m_position_1 = position;
	m_vel_1 = vel;

	return vel;
}
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);
}
Exemple #25
0
void Move_X_Millimeters(int Direction, float distMillimeters, double timeLimit){ 
	
	float rightClicksStraight = TICKS_PER_REVOLUTION_RIGHT*(distMillimeters/MILLIMETERS_PER_REVOLUTION);
	float leftClicksStraight = TICKS_PER_REVOLUTION_LEFT*(distMillimeters/MILLIMETERS_PER_REVOLUTION);
	
	clear_motor_position_counter(LEFT_PORT_WHEEL);
	clear_motor_position_counter(RIGHT_PORT_WHEEL);
	
	if(Direction==FORWARD){
		//printf("CRUISING_SPEED_FWD_LEFT is: %f:\n", CRUISING_SPEED_FWD_LEFT);
		double ticksDistance = TICKS_PER_REVOLUTION * (distMillimeters/MILLIMETERS_PER_REVOLUTION);
		
		clear_motor_position_counter(RIGHT_PORT_WHEEL);
		clear_motor_position_counter(LEFT_PORT_WHEEL);
		
		int motor_left_position_start = get_motor_position_counter(LEFT_PORT_WHEEL);
		int motor_right_position_start = get_motor_position_counter(RIGHT_PORT_WHEEL);
		
		double startFWDTime = seconds();
		
		while((get_motor_position_counter(LEFT_PORT_WHEEL) < (motor_left_position_start + ticksDistance)) 
			&& (seconds() - startFWDTime <= timeLimit)){
			
			int mvt_counter_left = get_motor_position_counter(LEFT_PORT_WHEEL)-motor_left_position_start;
			int mvt_counter_right = get_motor_position_counter(RIGHT_PORT_WHEEL)-motor_right_position_start;
			
			int wheel_difference = mvt_counter_right - mvt_counter_left;
			
			if(wheel_difference < CORRECTION_THRESHOLD && wheel_difference > -CORRECTION_THRESHOLD){//If both are within the same range, wheels are going straight
				mav(LEFT_PORT_WHEEL, CRUISING_SPEED_FWD);
				mav(RIGHT_PORT_WHEEL, CRUISING_SPEED_FWD);
				msleep(5);
				printf("STRAIGHT: left: %d right: %d diff: %d\n", mvt_counter_left, mvt_counter_right, wheel_difference);
			}
			else if(mvt_counter_right > mvt_counter_left){//right wheel is ahead of left wheel
				//mav(LEFT_PORT_WHEEL, 750);
				mav(RIGHT_PORT_WHEEL, CRUISING_SPEED_FWD * CORRECTION_REDUCTION); //correction speeed is 80% of fwd speed
				printf("RIGHT diff: %d\n", wheel_difference);
				msleep(35);
			}
			else if(mvt_counter_left > mvt_counter_right){//left wheel is ahead of right wheel
				mav(LEFT_PORT_WHEEL, CRUISING_SPEED_FWD * CORRECTION_REDUCTION); //correction speeed is 80% of fwd speed
				//mav(RIGHT_PORT_WHEEL, 750);
				printf("LEFT diff: %d\n", wheel_difference);
				msleep(35);
			}
			msleep(15);
		}	
		ao();
	}
	if(Direction==BACK){
		double ticksDistance = -TICKS_PER_REVOLUTION * (distMillimeters/MILLIMETERS_PER_REVOLUTION);
		
		clear_motor_position_counter(RIGHT_PORT_WHEEL);
		clear_motor_position_counter(LEFT_PORT_WHEEL);
		
		int motor_left_position_start = get_motor_position_counter(LEFT_PORT_WHEEL);
		int motor_right_position_start = get_motor_position_counter(RIGHT_PORT_WHEEL);
		
		double startBACKTime = seconds();
		while((get_motor_position_counter(LEFT_PORT_WHEEL) > (motor_left_position_start + ticksDistance))
			&& (seconds() - startBACKTime <= timeLimit)){
			
			printf("BACK\n");	
				
			int mvt_counter_left = get_motor_position_counter(LEFT_PORT_WHEEL)-motor_left_position_start;
			int mvt_counter_right = get_motor_position_counter(RIGHT_PORT_WHEEL)-motor_right_position_start;
			
			int wheel_difference = mvt_counter_right - mvt_counter_left;
			
			if(wheel_difference < CORRECTION_THRESHOLD && wheel_difference > -CORRECTION_THRESHOLD){//If both are within the same range, wheels are going straight
				mav(LEFT_PORT_WHEEL, CRUISING_SPEED_BWD);
				mav(RIGHT_PORT_WHEEL, CRUISING_SPEED_BWD);
				msleep(5);
				printf("left: %d right: %d diff: %d\n", mvt_counter_left, mvt_counter_right, wheel_difference);
			}
			else if(mvt_counter_right > mvt_counter_left){ //right wheel is ahead of left
				//mav(LEFT_PORT_WHEEL, 750);
				mav(LEFT_PORT_WHEEL, CRUISING_SPEED_BWD * CORRECTION_REDUCTION); //correction speed is 80% of BWD Speed
				printf("correcting right diff: %d\n", wheel_difference);
				msleep(35);
			}
			else if(mvt_counter_left > mvt_counter_right){// left wheel is ahead of right
				mav(RIGHT_PORT_WHEEL, CRUISING_SPEED_BWD * CORRECTION_REDUCTION); //correction speed is 80% of BWD Speed
				//mav(RIGHT_PORT_WHEEL, 750);
				printf("correcting left diff: %d\n", wheel_difference);
				msleep(35);
			}
			msleep(15);
		}	
		ao();
	}
}
Exemple #26
0
void findBall(){
	display_clear();
	int temptLMSpeed = LM_MINS;			// changeable minimum speed for left motor
	int temptRMSpeed = RM_MINS;			// changeable minimum speed for right morot
	int lastLMSpeed, lastRMSpeed;	// motor speed
	int lastLMPos, lastRMPos;		// motor positions
	int errorX = 0, errorY = 0;		// difference of current ball coordinate and target coordinate
	int objArea;					// area of the largest object in sight
	point2 objCen; 					// object center coordinate
	
	camera_update();
	int objNum = get_object_count(GREEN);
	while(objNum == 0){
		camera_update();
		objNum = get_object_count(GREEN);
	}
	// find a ball in sight
	
	objCen = get_object_center(GREEN, 0);
	errorX = OFFSET_X - objCen.x;
	errorY = OFFSET_Y - objCen.y;
	
	while(!(a_button_clicked())){
		camera_update();
		objArea = get_object_area(GREEN, 0);
		while(objArea < 200){
			ao();
			camera_update();
			objArea = get_object_area(GREEN, 0);
		}
		// make sure really a ball is in sight
		
		objCen = get_object_center(GREEN, 0);
		errorX = OFFSET_X - objCen.x;
		errorY = OFFSET_Y - objCen.y;
		// find differences between coordinates
		
		if(errorX > -3 && errorX < 2 && errorY > -3 && errorY < 2)
			break;
		int turnLM = -1 * errorX * P_X + errorY * P_Y;
		int turnRM = errorX * P_X + errorY * P_Y;
		// turn for LM, RM
		
		if(turnLM == lastLMSpeed && lastLMPos == get_motor_position_counter(LM))
			temptLMSpeed = temptLMSpeed + 1;
		if(turnLM == lastLMSpeed && lastLMPos != get_motor_position_counter(LM))
			temptLMSpeed = temptLMSpeed - 1;
		if(turnLM == -1 * lastLMSpeed && lastLMPos != get_motor_position_counter(LM))
			temptLMSpeed = temptLMSpeed - 2;
		if(turnRM == lastRMSpeed && lastRMPos == get_motor_position_counter(RM))
			temptRMSpeed = temptRMSpeed + 1;
		if(turnRM == lastRMSpeed && lastRMPos != get_motor_position_counter(RM))
			temptRMSpeed = temptRMSpeed - 1;
		if(turnRM == -1 * lastRMSpeed && lastRMPos != get_motor_position_counter(RM))
			temptRMSpeed = temptRMSpeed - 2;
		lastLMSpeed = turnLM;
		lastLMPos = get_motor_position_counter(LM);
		lastRMSpeed = turnRM;
		lastRMPos = get_motor_position_counter(RM);
		// check if speed is too high or low, thus make modification to minimum speed
		
		if(turnLM > -1 * temptLMSpeed && turnLM < 0)
			turnLM = -1 * temptLMSpeed;
		if(turnLM > 0 && turnLM < temptLMSpeed)
			turnLM = temptLMSpeed;
		if(turnRM > -1 * temptRMSpeed && turnRM < 0)
			turnRM = -1 * temptRMSpeed;
		if(turnRM > 0 && turnRM < temptRMSpeed)
			turnRM = temptRMSpeed;
		// set minimum speed: avoid unmoving
		
		motor(LM, turnLM);
		motor(RM, turnRM);
		// move
	}
	
	ao();
	catchBall(GREEN);
	// stop and catch
}
Exemple #27
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;
}
Exemple #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();
}
Exemple #29
0
//moves forward with light sensor
void moveForwardRoutine(double distanceInInches, int checkIRSensor, int speed, int debug) {
	//checkLightSensor	do not check light sensor: see #define values
	//                do check light sensor and stop when it is over black or void
	//convert inches to clicks
	int speed_adjust_up = (int) (speed / ADJUST_SPEED);
	int speed_adjust_down = (int) (speed * ADJUST_SPEED);
	printf("speed down %d, speed up: %d\n", speed_adjust_down, speed_adjust_up);

	int clicks =(int) (156.25l * distanceInInches);
	int initial_position_right = get_motor_position_counter(RIGHT_MOTOR);
	int initial_position_left = get_motor_position_counter(LEFT_MOTOR);
	int current_position_right = get_motor_position_counter(RIGHT_MOTOR);
	int current_position_left = get_motor_position_counter(LEFT_MOTOR);
	float differential  = 0 ;
	//we will keep moving until both motors have covered their distance or
	//the light sensor is tripped
	while (current_position_left <= (initial_position_left + clicks) ||
		current_position_right <= (initial_position_right + clicks)   )	{
		//let's get out if the sensor is on
		if (checkIRSensor == CHECK_IR_SENSOR && analog(IR_SENSOR) > IR_SENSOR_THRESHOLD) 			{
			if (debug == DEBUG) {
				printf("exiting because of light sensor");
			}
			break;
		}
		//first let's see if one motor is going ahead of the other
		differential = (current_position_left - initial_position_left) 
					* MOTOR_CORRECTION_FACTOR_LEFT ;
		differential = differential - (current_position_right - initial_position_right);
//			if (debug == DEBUG) {
//				printf("sensor value: %d lightsensor %d \n", analog(IR_SENSOR),checkIRSensor);
//			}
		if (differential > -25 && differential < 25 ) {
		//counter are around the same 
			mav(RIGHT_MOTOR, speed);
			mav(LEFT_MOTOR, speed);
		} else if (differential < 0 ) {
		//right has moved ahead, let's slow down right until left catches up
			mav(RIGHT_MOTOR,speed_adjust_down);
			mav(LEFT_MOTOR, speed_adjust_up);
			if (debug == DEBUG) {
				printf("adjusting LEFT L: %d R: %d, diff %f\n", (current_position_left - 				initial_position_left), (current_position_right - initial_position_right), differential);
			}
		} else {
		//left has moved ahead, let's slow down left until right catches up
			mav(RIGHT_MOTOR, speed_adjust_up);
			mav(LEFT_MOTOR, speed_adjust_down);
			if (debug == DEBUG) {
				printf("adjusting RIGHT L: %d R: %d diff %f\n", (current_position_left - initial_position_left), (current_position_right - initial_position_right), differential);
			}
		}
		msleep(25);
		current_position_right = get_motor_position_counter(RIGHT_MOTOR);
		current_position_left = get_motor_position_counter(LEFT_MOTOR);
	}
	//turn off motors completely
	mav(RIGHT_MOTOR, 0);
	mav(LEFT_MOTOR,0);
	ao();
	reset_motors();
}