Revision 0:3827dab9a2eb, committed 2019-03-20

Comitter:

hamaint

Date:

Wed Mar 20 00:25:00 2019 +0000

Commit message:

version shlagg c++

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Asservissement.cpp	Wed Mar 20 00:25:00 2019 +0000
@@ -0,0 +1,254 @@
+/**
+ * Includes
+ */
+#include "Asservissement.h"
+
+
+
+Asservissement::Asservissement(L298 pontH, QEI encodA, QEI encodB, double freq_asserv=100):_pontH(pontH), _encodA(encodA), _encodB(encodB), _freq_correction(freq_asserv)
+{
+    _dist = 0;
+    _or_deg = 0;
+}
+
+void Asservissement::move(AUTORISATION rep)
+{
+    //uniquement appelé par le créateur de trajectoire
+    //c'est à ca que ça sert le "autorisation"
+    if (rep == OUI)
+    {
+        
+    
+    double or_dist = _or_deg * _dist_roues * M_PI / 360;
+    double a,b,err_dist,err_ori,pid_dist,pid_ori,pid_a,pid_b;
+    
+    //trouver une condition à laquelle sortir de la boucle
+    //IDEE on incremente un compteur si les deux err sont en dessous d'une
+    // limite #double _err_statique sinon il retombe à 0 et si pendant 
+    //LIMITE_TEMPS = 3/FREQ_CORRECTION on atteind compteur = limite_temps 
+    //alors on sort de la boucle (ca avait bien marché quand j'avais fait des 
+    // test en mode yolo
+    //pourrait etre interessant de voir si le createur de trajectoire peut
+    // pas y faire quelques chose lui aussi
+    
+    while(1) 
+    {
+        a = _encodA.getPulses() * _resA;
+        b = _encodB.getPulses() * _resB;
+        
+        err_dist = _dist - (a + b) /2;
+        err_ori = or_dist - (a - b) /2;
+
+        pid_dist = calculPID(err_dist,LINEIQUE);
+        pid_ori = calculPID(err_ori,ANGULAIRE);
+
+        pid_a = - pid_dist + pid_ori;
+        pid_b = - pid_dist - pid_ori;
+        
+        if (pid_a>_lim_vitesseA)
+            pid_a = _lim_vitesseA;
+        else if (pid_a<-_lim_vitesseA)
+            pid_a = -_lim_vitesseA;
+
+        if (pid_b>_lim_vitesseB)
+            pid_b=_lim_vitesseB;
+        else if (pid_b<-_lim_vitesseB)
+            pid_b = -_lim_vitesseB;
+
+
+        _pontH.A(pid_a);
+        _pontH.B(pid_b);
+        wait(_freq_correction);
+        
+    }
+    }
+    
+}
+
+void Asservissement::move(double dist, double or_deg)
+{
+    
+    double or_dist = or_deg * _dist_roues * M_PI / 360;
+    double a,b,err_dist,err_ori,pid_dist,pid_ori,pid_a,pid_b;
+    
+    //trouver une condition à laquelle sortir de la boucle
+    //IDEE on incremente un compteur si les deux err sont en dessous d'une
+    // limite #double _err_statique sinon il retombe à 0 et si pendant 
+    //LIMITE_TEMPS = 3/FREQ_CORRECTION on atteind compteur = limite_temps 
+    //alors on sort de la boucle (ca avait bien marché quand j'avais fait des 
+    // test en mode yolo
+    //pourrait etre interessant de voir si le createur de trajectoire peut
+    // pas y faire quelques chose lui aussi
+    
+    //peut être qu'il faut faire un truc en mode, le calculateur de trajectoire
+    //defini en permanance une nouvelle trajectoire, vue que c'est dans la mm
+    //class, on pourrait plutôt jouer sur le fait que move utilise des variables
+    //privées _dist et _or_deg
+    //c'est pour ca que j'ai fait une fonction move qui prend rien en parametre
+    //elle sera appellée par le générateur de trajectoire
+    while(1) 
+    {
+        a = _encodA.getPulses() * _resA;
+        b = _encodB.getPulses() * _resB;
+        
+        err_dist = dist - (a + b) /2;
+        err_ori = or_dist - (a - b) /2;
+
+        pid_dist = calculPID(err_dist,LINEIQUE);
+        pid_ori = calculPID(err_ori,ANGULAIRE);
+
+        pid_a = - pid_dist + pid_ori;
+        pid_b = - pid_dist - pid_ori;
+        
+        if (pid_a>_lim_vitesseA)
+            pid_a = _lim_vitesseA;
+        else if (pid_a<-_lim_vitesseA)
+            pid_a = -_lim_vitesseA;
+
+        if (pid_b>_lim_vitesseB)
+            pid_b=_lim_vitesseB;
+        else if (pid_b<-_lim_vitesseB)
+            pid_b = -_lim_vitesseB;
+
+
+        _pontH.A(pid_a);
+        _pontH.B(pid_b);
+        wait(_freq_correction);
+        
+    }
+    /*
+
+    float orientation_dist = orientation_deg * (float)DISTANCE_ENTRE_ROUES * 3.14 / 360;
+
+    while(1)
+    {
+        pulse_a = (float)encodeur_a.getPulses() * res_a;
+        pulse_b = (float)encodeur_b.getPulses()* res_b;
+
+        err_dist = distance - (pulse_a + pulse_b) /2;
+        err_ori = orientation_dist - (pulse_a - pulse_b) /2;
+
+        printf("%f\t%f\t%f\t%f\n\r",err_dist,err_ori,pulse_a,pulse_b);
+
+        pid_dist = PID(err_dist,LINEIQUE);
+        pid_ori = PID(err_ori,ANGULAIRE);
+
+        pid_a = - pid_dist + pid_ori;
+        pid_b = - pid_dist - pid_ori;
+
+
+        //printf("%f\n\r",pulse_a);
+
+        n_commande_a = pid_a;
+        n_commande_b = pid_b;
+
+
+        if (n_commande_a>1)
+            n_commande_a=LIMITE_VITESSE;
+        else if (n_commande_a<-LIMITE_VITESSE)
+            n_commande_a = -LIMITE_VITESSE;
+
+        if (n_commande_b>LIMITE_VITESSE)
+            n_commande_b=LIMITE_VITESSE;
+        else if (n_commande_b<-LIMITE_VITESSE)
+            n_commande_b = -LIMITE_VITESSE;
+
+
+        //moteur_a(n_commande_a);
+        //moteur_b(n_commande_b);
+        wait(FREQ_CORRECTION);
+
+    }
+
+
+    */
+}
+
+//donne le coefficient à balancer dirrecteument
+double Asservissement::calculPID(double erreur,PID type)
+{
+    static double errPrev = 0;
+    static double errSum = 0;
+    static double errDif = 0;
+
+    double P,I,D;
+
+    errSum += erreur;
+
+    errDif = erreur - errPrev;
+    errPrev = erreur;
+
+    if (type == LINEIQUE) {
+        P = erreur * _Pl;
+        I = errSum * _Il;
+        D = errDif * _Dl;
+    } else if (type == ANGULAIRE) {
+        P = erreur * _Pa;
+        I = errSum * _Ia;
+        D = errDif * _Da;
+    }
+
+    return P + I + D;
+}
+
+void Asservissement::setPID(PID type, double P, double I, double D)
+{
+    if (type == LINEIQUE) {
+        _Pl = P;
+        _Il = I;
+        _Dl = D;
+    } else if (type == ANGULAIRE) {
+        _Pa = P;
+        _Ia = I;
+        _Da = D;
+    }
+}
+
+
+void Asservissement::setDimensions(int dist_roues, int dimA, int dimB, int ticA, int ticB)
+{
+        _dist_roues = dist_roues;
+        _dimA = dimA;
+        _dimB = dimB;
+        _ticA = ticA;
+        _ticB = ticB;
+        
+        _resA = _dimA*3.14/_ticA;
+        _resB = _dimB*3.14/_ticB;
+}
+
+
+void Asservissement::setDimensions(int dist_roues, int dim, int tic)
+{
+        _dist_roues = dist_roues;
+        _dimA = dim;
+        _dimB = dim;
+        _ticA = tic;
+        _ticB = tic;
+        
+        _resA = _dimA*3.14/_ticA;
+        _resB = _resB;
+}
+
+
+void Asservissement::setCalculateur(double lim_vitesseA, double lim_vitesseB, double accA, double accB, double deccA, double deccB)
+{
+        _lim_vitesseA = lim_vitesseA;
+        _lim_vitesseB = lim_vitesseB;
+        _accA = accA;
+        _accB = accB;
+        _deccA = deccA;
+        _deccB = deccB;
+}
+
+
+void Asservissement::setCalculateur(double lim_vitesse, double acc, double decc)
+{
+        _lim_vitesseA = lim_vitesse;
+        _lim_vitesseB = lim_vitesse;
+        _accA = acc;
+        _accB = acc;
+        _deccA = decc;
+        _deccB = decc;
+}
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Asservissement.h	Wed Mar 20 00:25:00 2019 +0000
@@ -0,0 +1,312 @@
+/**
+ * @author Thomas Hamain
+ *
+ * @section DESCRIPTION
+ *  [EMPTY]
+ *
+ */
+
+#ifndef ASSERVISSEMENT_H
+#define ASSERVISSEMENT_H
+
+# define M_PI           3.14159265358979323846
+
+/**
+ * Includes
+ */
+#include "mbed.h"
+#include "QEI.h"
+#include "L298.h"
+#include "math.h"
+
+/**
+ * Class [EMPTY] on parle en mm
+ */
+class Asservissement
+{
+public:
+
+    typedef enum AUTORISATION {
+
+        OUI,
+        NON
+
+    }AUTORISATION;
+
+    typedef enum PID {
+
+        LINEIQUE,
+        ANGULAIRE
+
+    } PID;
+    /**
+     * Constructeur.
+     * [EMPTY]
+     *
+     * @param [EMPTY]
+     */
+    Asservissement(L298 pontH, QEI encodA, QEI encodB, double freq_asserv);
+
+
+    double calculPID(double erreur,PID type);
+
+
+    void move(double dist, double or_deg);
+    
+    void move(AUTORISATION rep);
+
+
+    void setPID(PID type, double P, double I, double D);
+
+
+    void setDimensions(int dist_roues, int dimA, int dimB, int ticA, int ticB);
+
+
+    void setDimensions(int dist_roues, int dim, int tic);
+
+
+    void setCalculateur(double lim_vitesseA, double lim_vitesseB, double accA, double accB, double deccA, double deccB);
+
+
+    void setCalculateur(double lim_vitesse, double acc, double decc);
+
+    //[FONCTION] un truc qui defini les trajectoire
+    // doit inclure une option permettant de choir l'acceleration + decceleration
+    // et une vitesse limite
+    // il devra avoir un espece d'interrupt timer et poser un flag pour que la
+    // boucle dans le move() s'arrete si besoin faudra y penser
+
+    //[FONCTION] un truc pour calibrer automatiquement
+    //IDEE DE FONCTIONNEMENT, 2 boucles, boucle 1 change de coefficient, boucle 2
+    //demande à l'utilisateur de combien il veut incrémenter le coefficient
+
+
+
+
+
+
+
+
+
+
+    //Fonctions de set get
+    //PID Lineique
+    double getPl()
+    {
+        return _Pl;
+    }
+    void setPl(double n)
+    {
+        _Pl=n;
+    }
+
+    double getIl()
+    {
+        return _Il;
+    }
+    void setIl(double n)
+    {
+        _Il=n;
+    }
+
+    double getDl()
+    {
+        return _Dl;
+    }
+    void setDl(double n)
+    {
+        _Dl=n;
+    }
+
+    //PID Angulaire
+    double getPa()
+    {
+        return _Pa;
+    }
+    void setPla(double n)
+    {
+        _Pa=n;
+    }
+
+    double getIa()
+    {
+        return _Ia;
+    }
+    void setIa(double n)
+    {
+        _Ia=n;
+    }
+
+    double getDa()
+    {
+        return _Da;
+    }
+    void setDa(double n)
+    {
+        _Da=n;
+    }
+
+    //echantillonnage
+    double getfreq_correction()
+    {
+        return _freq_correction;
+    }
+    void setfreq_correction(double n)
+    {
+        _freq_correction=n;
+    }
+
+    //dimensions du système
+    double getdist_roues()
+    {
+        return _dist_roues;
+    }
+    void setdist_roues(double n)
+    {
+        _dist_roues=n;
+    }
+
+    double getdimA()
+    {
+        return _dimA;
+    }
+    void setdimA(double n)
+    {
+        _dimA=n;
+    }
+
+    double getdimB()
+    {
+        return _dimB;
+    }
+    void setdimB(double n)
+    {
+        _dimB=n;
+    }
+
+    double getticA()
+    {
+        return _ticA;
+    }
+    void setticA(double n)
+    {
+        _ticA=n;
+    }
+
+    double getticB()
+    {
+        return _ticA;
+    }
+    void setticB(double n)
+    {
+        _ticA=n;
+    }
+
+    //valeurs pour le calculateur de trajectoires
+    double getlim_vitesseA()
+    {
+        return _lim_vitesseA;
+    }
+    void setlim_vitesseA(double n)
+    {
+        _lim_vitesseA=n;
+    }
+
+    double getlim_vitesseB()
+    {
+        return _lim_vitesseB;
+    }
+    void setlim_vitesseB(double n)
+    {
+        _lim_vitesseB=n;
+    }
+
+    double get_accA()
+    {
+        return _accA;
+    }
+    void set_accA(double n)
+    {
+        _accA=n;
+    }
+
+    double get_accB()
+    {
+        return _accB;
+    }
+    void setP_accB(double n)
+    {
+        _accB=n;
+    }
+
+    double getdeccA()
+    {
+        return _deccA;
+    }
+    void setdeccA(double n)
+    {
+        _deccA=n;
+    }
+
+    double getdeccB()
+    {
+        return _deccB;
+    }
+    void setdeccB(double n)
+    {
+        _deccB=n;
+    }
+
+
+private:
+
+    //variables importantes
+    L298 _pontH;
+    QEI _encodA;
+    QEI _encodB;
+    double _dist;
+    double _or_deg;
+
+    //coeffs pour PID lineique
+    double _Pl; //0.01
+    double _Il; //0.0001
+    double _Dl;
+
+    //coeffs pour PID angulaire
+    double _Pa; //0.01
+    double _Ia; //0.0001
+    double _Da;
+
+    //echantillonnage #BOZ
+    double _freq_correction; //0.01
+
+    //probablement ballec
+    //double _err_statique;
+    //double LIMITE_TEMPS 3/FREQ_CORRECTION (3sec)
+
+    //dimensions du système
+    int _dist_roues; //distance entre les roues 270
+    int _dimA; //dimension roue a 70
+    int _dimB; //dimension roue a 70
+    int _ticA; //nombre de tic par tour encodeur a (il faudra faire des calculs lors de la déclaration parce que ca correspond pas vraiment par rapport a la datasheet) 1200
+    int _ticB; //nombre de tic par tour encodeur b 1200
+    double _resA; //mm/tic calculé par (double)DIMENSION_ROUE_A*3.14/(double)TIC_PAR_TOUR_A
+    double _resB;
+
+    //valeurs pour le calculateur de trajectoires (valeurs symétriques)
+    //valeurs modifiées par calculateur de trajectoires
+    double _lim_vitesseA; //limites de vitesse A et B
+    double _lim_vitesseB;
+    double _accA; //acceleration pour A et B (pas étudié)
+    double _accB;
+    double _deccA; //decceleration pour A et B (pas étudié)
+    double _deccB;
+
+
+
+
+};
+
+
+
+#endif /* ASSERVISSEMENT_H */
\ No newline at end of file