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