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);
}
}
}
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();
}
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;
}
}
}
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();
}
