90 lines
2.6 KiB
C
90 lines
2.6 KiB
C
#include "QEI.h"
|
|
#include "Moteurs.h"
|
|
#include "Asser_Moteurs.h"
|
|
|
|
#define ASSERMOTEUR_GAIN_P 300000.f
|
|
#define ASSERMOTEUR_GAIN_I 30000.f
|
|
|
|
float consigne_mm_s[3]; // Consigne de vitesse (en mm/s)
|
|
float commande_I[3]; // Terme integral
|
|
|
|
void AsserMoteur_Init(){
|
|
QEI_init();
|
|
Moteur_Init();
|
|
for(unsigned int i =0; i< 2; i ++){
|
|
commande_I[i]=0;
|
|
consigne_mm_s[i]=0;
|
|
}
|
|
}
|
|
|
|
/// @brief Défini une consigne de vitesse pour le moteur indiqué.
|
|
/// @param moteur : Moteur à asservir
|
|
/// @param _consigne_mm_s : consigne de vitesse en mm/s
|
|
void AsserMoteur_setConsigne_mm_s(enum t_moteur moteur, float _consigne_mm_s){
|
|
consigne_mm_s[moteur] = _consigne_mm_s;
|
|
|
|
}
|
|
|
|
/// @brief Envoie la consigne du moteur
|
|
/// @param moteur : Moteur à asservir
|
|
float AsserMoteur_getConsigne_mm_s(enum t_moteur moteur){
|
|
return consigne_mm_s[moteur];
|
|
}
|
|
|
|
|
|
|
|
float AsserMoteur_getVitesse_mm_s(enum t_moteur moteur, int step_ms){
|
|
enum QEI_name_t qei;
|
|
float distance, temps;
|
|
switch (moteur)
|
|
{
|
|
case MOTEUR_A: qei = QEI_A_NAME; break;
|
|
case MOTEUR_B: qei = QEI_B_NAME; break;
|
|
default: break;
|
|
}
|
|
distance = QEI_get_mm(qei);
|
|
temps = step_ms / 1000.0f;
|
|
|
|
return distance / temps;
|
|
}
|
|
|
|
/// @brief Indique si le robot est à l'arrêt
|
|
/// @param step_ms : pas de temps (utilisé pour déterminer les vitesses)
|
|
/// @return 1 si le robot est immobile, 0 s'il est en mouvement.
|
|
uint32_t AsserMoteur_RobotImmobile(int step_ms){
|
|
const float seuil_vitesse_immobile_mm_s = 0.1;
|
|
if(AsserMoteur_getVitesse_mm_s(MOTEUR_A, step_ms) < seuil_vitesse_immobile_mm_s &&
|
|
AsserMoteur_getVitesse_mm_s(MOTEUR_B, step_ms) < seuil_vitesse_immobile_mm_s ){
|
|
return 1;
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
/// @brief Fonction d'asservissement des moteurs, à appeler périodiquement
|
|
/// @param step_ms
|
|
void AsserMoteur_Gestion(int step_ms){
|
|
// Pour chaque moteur
|
|
for(uint moteur=MOTEUR_A; moteur<MOTEUR_B+1; moteur++ ){
|
|
float erreur; // Erreur entre la consigne et la vitesse actuelle
|
|
float commande_P; // Terme proportionnel
|
|
float commande;
|
|
|
|
// Calcul de l'erreur
|
|
erreur = consigne_mm_s[moteur] - AsserMoteur_getVitesse_mm_s(moteur, step_ms);
|
|
|
|
// Calcul du terme propotionnel
|
|
commande_P = erreur * ASSERMOTEUR_GAIN_P;
|
|
|
|
// Calcul du terme integral
|
|
commande_I[moteur] = commande_I[moteur] + (erreur * ASSERMOTEUR_GAIN_I * step_ms);
|
|
|
|
commande = commande_P + commande_I[moteur];
|
|
|
|
//Saturation de la commande
|
|
if(commande > 32760) {commande = 32760;}
|
|
if(commande < -32760) {commande = -32760;}
|
|
|
|
Moteur_SetVitesse(moteur, commande);
|
|
|
|
}
|
|
} |