Ascenseur.cpp 2.53 KB
Newer Older
Potron Corentin's avatar
Potron Corentin committed
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
#include "Ascenseur.h"

Ascenseur::Ascenseur(PinName moteur1, PinName moteur2, PinName moteurPWM, PinName moteurStandby, PinName encodeurA, PinName encodeurB, PinName finCourse, double Kp, double Ki, double Kd) :_moteur(moteur1, moteur2, moteurPWM, moteurStandby), _encodeur(encodeurA, encodeurB, NC, PULSES_PAR_ROTATION), _finCourse(finCourse) {
        _Kp = Kp;
        _Ki = Ki;
        _Kd = Kd;
        _moteur.stop();
        _encodeur.reset();
        _finCourse.mode(PullUp);
}

unsigned char Ascenseur::enMouvement() {
    return _enMouvement;
}

void Ascenseur::doMouvement(int position) {
    _moteur.start();
    _enMouvement = 1;
    _consigne = position;
    _pulsesActuelles = 0;
    _termeErreur = 0;
    _termeIntegrale = 0;
    _termeDerivee = 0;
    _commandeMoteurActuelle = 0;
    _nombreActionsPeriodiques = 0;
}

void Ascenseur::computePID() {
    _pulsesActuelles = _encodeur.getPulses(); // On fige le nombre de pulsations pour le ca
    _termeDerivee = (_consigne - _pulsesActuelles) - _termeErreur; // Calcul de la dérivée de l'erreur
    _termeErreur = _consigne - _pulsesActuelles; // Calcul de la nouvelle erreur
    _termeIntegrale += _termeErreur; // On somme dans l'intégrale la nouvelle erreur
    _commandeMoteurActuelle = _Kp * (double)(_termeErreur) + _Ki * (double)(_termeIntegrale) + _Kd * (double)(_termeDerivee);  // Calcul PID
    _nombreActionsPeriodiques++;  // On incrémente de 1 le nombre d'actions périodiques
}

void Ascenseur::setConstantesPID(double newKp, double newKi, double newKd) {
    _Kp = newKp;
    _Ki = newKi;
    _Kd = newKd;
}

void Ascenseur::resetPosition() {
    _moteur.start();
    _moteur.fwd(1);
    while (_finCourse) {
        wait_ms(1);
    }
    _moteur.stop();
    _encodeur.reset();
}

void Ascenseur::updateCommandeMoteur() {
    // Fin du mouvement => les deux dernières erreurs ont pas changé + on est pas au début du mouvement + erreur finale est faible
    if (_termeDerivee == 0 && _nombreActionsPeriodiques > 2 && abs(_termeErreur) < ERREUR_PULSATIONS_TOLEREE) {
        _enMouvement = 0;
        _moteur.stop();
    // Sinon, on met à jour la commande des moteurs
    } else {
        if (_commandeMoteurActuelle < 0) {
            if (_commandeMoteurActuelle < -1) {
                _moteur.fwd(1);
            } else {
                _moteur.fwd(abs(_commandeMoteurActuelle));
            }
        } else {
            if (_commandeMoteurActuelle > 1) {
                _moteur.rev(1);
            } else {
                _moteur.rev(_commandeMoteurActuelle);
            }
        }
    }
}