diff --git a/Cerveau/Cerveau.ino b/Cerveau/Cerveau.ino index c0d73b6..6bb44a8 100644 --- a/Cerveau/Cerveau.ino +++ b/Cerveau/Cerveau.ino @@ -383,7 +383,7 @@ enum etat_action_t Strategie(void){ break; case STRAT_ALLER_GRADINS_1: - etat_action = deplacement_absolu(730, 550, M_PI/2., 0); + etat_action = deplacement_absolu(800, 800, -M_PI/2., 0); if(etat_action == ACTION_TERMINEE){ etat_strategie = STRAT_RECULE_BANDEROLE; return ACTION_TERMINEE; @@ -429,10 +429,10 @@ void compar_cinematique(int consigne_x_mm, int consigne_y_mm, float consigne_ori // Distance à parcourir float distance_calculee = sqrt(sq(compar_X) + sq(compar_Y)); - float angle_robot_vers_destination = M_PI_2 - atan2(compar_Y, compar_X); + float angle_robot_vers_destination = M_PI + atan2f(compar_Y, compar_X); - chassis_emission->translation_x_mm = sin(angle_robot_vers_destination - Angle_Robot_RAD) * distance_calculee; - chassis_emission->translation_y_mm = cos(angle_robot_vers_destination - Angle_Robot_RAD) * distance_calculee; + chassis_emission->translation_x_mm = cos(angle_robot_vers_destination - triangulation_reception.angle_rad) * distance_calculee; + chassis_emission->translation_y_mm = sin(angle_robot_vers_destination - triangulation_reception.angle_rad) * distance_calculee; chassis_emission->rotation_z_rad = 0; chassis_emission->status = MOUVEMENT_EN_COURS; }