#include "Asser_Moteurs.h"
#include "Geometrie_robot.h"
#include "Commande_vitesse.h"


float avance_mm_s, orientation_radian_s;

float get_avance_mm_s(){
    return avance_mm_s;
}

float get_orientation_radian_s(){
    return orientation_radian_s;
}

/// @brief Commande de la vitesse dans le référentiel du robot
/// @param avance_mm_s : Vitesse d'avance
/// @param orientation_radian_s : Rotation en radian/s 
void commande_vitesse(float _avance_mm_s, float _orientation_radian_s){
    float vitesse_roue_gauche, vitesse_roue_droite;
    avance_mm_s = _avance_mm_s;
    orientation_radian_s = _orientation_radian_s;

    vitesse_roue_gauche = avance_mm_s - (orientation_radian_s * DISTANCE_ROUES_CENTRE_MM);
    vitesse_roue_droite = avance_mm_s + (orientation_radian_s * DISTANCE_ROUES_CENTRE_MM);

    AsserMoteur_setConsigne_mm_s(MOTEUR_A, vitesse_roue_droite);
    AsserMoteur_setConsigne_mm_s(MOTEUR_B, vitesse_roue_gauche);

}

/// @brief Arrête le robot.
void commande_vitesse_stop(){
    AsserMoteur_setConsigne_mm_s(MOTEUR_A, 0);
    AsserMoteur_setConsigne_mm_s(MOTEUR_B, 0);
}