void turn(float motor_speed, float angle, char side)
{
	float turn = 3.85*64*(angle/360);
	int angle_turned = 0;
	if (side == 'L')
	{
		MOTOR_SetSpeed(MOTOR_RIGHT, motor_speed);
		while (1)
		{
			reset_encoders();
			if (angle_turned >= turn)
			{
					MOTOR_SetSpeed(MOTOR_LEFT, 0);
				break;
			}
			THREAD_MSleep(30);
			angle_turned += ENCODER_Read(ENCODER_RIGHT);
		}
	}
	if (side == 'R')
		{
			MOTOR_SetSpeed(MOTOR_LEFT, motor_speed);
			while (1)
			{
				reset_encoders();
				if (angle_turned >= turn)
				{
						MOTOR_SetSpeed(MOTOR_LEFT, 0);
					break;
				}
				THREAD_MSleep(30);
				angle_turned += ENCODER_Read(ENCODER_LEFT);
			}
		}
}
Esempio n. 2
0
void set_expected_encoder_reads(int motor_speed){
	int average_encoder_read_right = 0, average_encoder_read_left = 0, total_encoder_read_right = 0, total_encoder_read_left = 0;
	int calculation_period = 3000;
	prompt_bumpers();
	reset_encoders();
	set_motors_speed(motor_speed);
	LCD_ClearAndPrint("Testing and setting encoder expected reads for %d ms...", calculation_period);
	THREAD_MSleep(calculation_period);
	total_encoder_read_right += ENCODER_Read(ENCODER_RIGHT);
	total_encoder_read_left += ENCODER_Read(ENCODER_LEFT);
	stop();
	EXPECTED_ENCODER_READ_LEFT = (int)floor(total_encoder_read_left/(calculation_period/__PID_TIME_INTERVAL__));
	EXPECTED_ENCODER_READ_RIGHT = (int)floor(total_encoder_read_right/(calculation_period/__PID_TIME_INTERVAL__));
	LCD_ClearAndPrint("Done!\nExpected encoder reads are:\nRight: %d\nLeft: %d\nPress any bumper to continue!",EXPECTED_ENCODER_READ_RIGHT,EXPECTED_ENCODER_READ_LEFT);
	prompt_bumpers();
	reset_encoders();
}
Esempio n. 3
0
void encoder_setup(void)
{
	cli();									//disables interrupts.
	reset_encoders();
	DDRD &= ~((1<<PD2)|(1<<PD3));			// Sets PD2 and PD3 to input and leaves the rest of PORTD/PIND alone. May not be necessary.
	EIMSK |= (1<<INT0)|(1<<INT1);			// Enables INT0 and INT1 but leaves the rest of this register unchanged.
	uint8_t temporary=EICRA;				// Necessary as ISC are two bit for each of INT0, INT1, INT2 and do not wish to overwrite INT2.
	temporary &= 0b11110000;				// leaves bits 7-4 unchanged, bits 3-0 all set to 0 for setting up INT0 and INT1.
	EICRA |= (1<<ISC00)|(1<<ISC10);			// Sets INT0 and INT 1 to trigger on either a rising or falling edge. For N holes/notches in the encoder wheel, there will be 2N ticks on the counter variables.
	EICRA |= temporary|EICRA;				// This preserves the original configuration of EICRA.
	sei();									// enables interrupts.
}
void move(float motor_speed, int expected_encoder_read, int move_dist_max)
{
	int previous_error_right = 0;
	int previous_error_left = 0;
	double total_error_right = 0;
	double total_error_left = 0;
	double current_time = 0;
	int move_dist_count = 0;
	MOTOR_SetSpeed(MOTOR_RIGHT, motor_speed);
	MOTOR_SetSpeed(MOTOR_LEFT, motor_speed+1);
	while(1)
	{
		reset_encoders();
		THREAD_MSleep(PID_ADJUST_TIME);
		int actual_right_encoder_read = ENCODER_Read(ENCODER_RIGHT);
		int actual_left_encoder_read = ENCODER_Read(ENCODER_LEFT);
		int actual_error_right = expected_encoder_read - actual_right_encoder_read;
		int actual_error_left = expected_encoder_read - actual_left_encoder_read;
		total_error_right += actual_error_right;
		total_error_left += actual_error_left;
		double right_P_factor = P_CONSTANT_RIGHT * actual_error_right;
		double left_P_factor = P_CONSTANT_LEFT * actual_error_left;
		double right_I_factor = total_error_right * I_CONSTANT_RIGHT;
		double left_I_factor = total_error_left * I_CONSTANT_LEFT;
		double right_D_factor = (previous_error_right - actual_error_right) * D_CONSTANT_RIGHT;
		double left_D_factor = (previous_error_left - actual_error_left) * D_CONSTANT_LEFT;
		int adjusted_speed_right = motor_speed + right_P_factor + right_I_factor + right_D_factor;
		int adjusted_speed_left = motor_speed + left_P_factor + left_I_factor + left_D_factor;
		MOTOR_SetSpeed(MOTOR_RIGHT, adjusted_speed_right);
		MOTOR_SetSpeed(MOTOR_LEFT, adjusted_speed_left);
		previous_error_right = actual_error_right;
		previous_error_left = actual_error_left;
		move_dist_count += expected_encoder_read;
		if (move_dist_count >= move_dist_max)
		{
			MOTOR_SetSpeed(MOTOR_RIGHT, 0);
			MOTOR_SetSpeed(MOTOR_LEFT, 0);
			total_error_right = 0;
			total_error_left = 0;
			break;
		}
	}
}
Esempio n. 5
0
void turn(float degree, char side, int speed){
	float distance_remaining = get_circle_arc_length(degree, __DISTANCE_BETWEEN_WHEELS__/2);
	int motor, reverse_motor, encoder;
	if(side=='L'){
		motor = MOTOR_RIGHT;
		reverse_motor = MOTOR_LEFT;
		encoder = ENCODER_RIGHT;
	}else if(side=='R'){
		motor = MOTOR_LEFT;
		reverse_motor = MOTOR_RIGHT;
		encoder = ENCODER_LEFT;
	}
	MOTOR_SetSpeed(motor, speed);
	MOTOR_SetSpeed(reverse_motor, -speed);
	while(distance_remaining >= 0){
		THREAD_MSleep(250);
		int encoder_read = ENCODER_Read(encoder);
		distance_remaining = distance_remaining - (encoder_read * __TURN_DISTANCE_PER_ENCODER__);
	}
	stop_and_wait(10);
	reset_encoders();
}