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 vitesseTEST(robot &unRobot,short int TRANSITIONS)
{
MOTOR_SetSpeed(MOTOR_RIGHT, 50);
MOTOR_SetSpeed(MOTOR_LEFT, 50);
THREAD_MSleep(500);
//Distance parcourue en nombre de transitions effectués
unRobot.encodeurDroit = ENCODER_Read(ENCODER_RIGHT);
unRobot.encodeurGauche = ENCODER_Read(ENCODER_LEFT);
//Affichage des encodeurs et des vitesses
LCD_Printf("D: %i v: %i, G: %i v: %i \n",unRobot.encodeurDroit,50,unRobot.encodeurGauche,50);
}
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 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 encodeurGaucheTest()
{
MOTOR_SetSpeed(7,30);
THREAD_MSleep(100);
short int encodeurGauche = 0;
while(encodeurGauche <= 64)
{
encodeurGauche += ENCODER_Read(ENCODER_LEFT);
LCD_Printf("G: %i \n",encodeurGauche);
}
MOTOR_SetSpeed(7,0);
}
void encodeurDroitTest()
{
MOTOR_SetSpeed(8,30);
THREAD_MSleep(100);
short int encodeurDroit = 0;
while(encodeurDroit <= 64)
{
encodeurDroit+=ENCODER_Read(ENCODER_RIGHT);
LCD_Printf("D: %i \n",encodeurDroit);
}
MOTOR_SetSpeed(8,0);
}
void move_simple(float distance, int motor_speed){
float distance_remaining = distance;
bool move_is_done = false;
set_motors_speed(motor_speed);
while(!move_is_done){
THREAD_MSleep(__PID_TIME_INTERVAL__);
int encoder_read_right = ENCODER_Read(ENCODER_RIGHT);
distance_remaining = distance_remaining - (encoder_read_right * __DISTANCE_PER_ENCODER__);
if(distance_remaining < 0){
move_is_done = true;
}
}
}
void move(float distance, int motor_speed){
float distance_remaining = distance;
int encoder_error_right = 0, encoder_error_left = 0, total_error_right = 0, total_error_left = 0;
bool move_is_done = false;
set_motors_speed(motor_speed);
while(!move_is_done){
if(read_bumpers()){
stop_and_wait(250);
}else{
THREAD_MSleep(__PID_TIME_INTERVAL__);
int encoder_read_right = ENCODER_Read(ENCODER_RIGHT);
int encoder_read_left = ENCODER_Read(ENCODER_LEFT);
total_error_right += encoder_error_right;
total_error_left += encoder_error_left;
encoder_error_right = pid(encoder_read_right, MOTOR_RIGHT, EXPECTED_ENCODER_READ_RIGHT, encoder_error_right, total_error_right, motor_speed, __PID_PROPORTIONAL_GAIN__, __PID_INTEGRAL_GAIN__, __PID_DERIVATIVE_GAIN__, __PID_TIME_INTERVAL__);
encoder_error_left = pid(encoder_read_left, MOTOR_LEFT, EXPECTED_ENCODER_READ_LEFT, encoder_error_left, total_error_left, motor_speed, __PID_PROPORTIONAL_GAIN__, __PID_INTEGRAL_GAIN__, __PID_DERIVATIVE_GAIN__, __PID_TIME_INTERVAL__);
distance_remaining = distance_remaining - (encoder_read_right * __DISTANCE_PER_ENCODER__);
if(distance_remaining < 0){
move_is_done = true;
}
}
}
}
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();
}
//Function used to reset the count of encoders before starting any movement
//Encoders accumulate the number of bars read until you read them again
void reset_encoders()
{
ENCODER_Read(ENCODER_LEFT);
ENCODER_Read(ENCODER_RIGHT);
}
