ajout du projet

.gitignore

+.pio
+.pioenvs
+.piolibdeps
+.vscode/.browse.c_cpp.db*
+.vscode/c_cpp_properties.json
+.vscode/launch.json

lib/Ascenseur/Ascenseur.cpp

+#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);
+            }
+        }
+    }
+}
\ No newline at end of file

lib/Ascenseur/Ascenseur.h

+#ifndef ASCENSEUR_H
+#define ASCENSEUR_H
+
+/**
+ * Includes
+ */
+#include "mbed.h" // Framework mbed
+#include "TB6612FNG.h" // Contrôle du driver moteur
+#include "QEI.h" // Gestion des encodeurs
+
+/**
+ * Defines
+ */
+#define PULSES_PAR_ROTATION 320 // Nombre de pulsations par tour de l'axe
+#define ERREUR_PULSATIONS_TOLEREE 5 // Nombre de pulsations tolérées en fin de mouvement
+
+/**
+ * Interface de l'ascenseur
+ */
+class Ascenseur {
+    public:
+        Ascenseur(
+            PinName moteur1,
+            PinName moteur2,
+            PinName moteurPWM,
+            PinName moteurStandby,
+            PinName encodeurA,
+            PinName encodeurB,
+            PinName finCourse,
+            double Kp,
+            double Ki, 
+            double Kd
+        );
+        unsigned char enMouvement();
+        void doMouvement(int position);
+        void computePID();
+        void setConstantesPID(double newKp, double newKi, double newKd);
+        void resetPosition();
+        void updateCommandeMoteur();
+    
+    private:
+        TB6612FNG _moteur;
+        QEI _encodeur;
+        DigitalIn _finCourse;
+        double _Kp;
+        double _Ki;
+        double _Kd;
+        volatile unsigned char _enMouvement;
+        volatile int _consigne;
+        volatile int _pulsesActuelles;
+        volatile int _termeErreur;
+        volatile int _termeIntegrale;
+        volatile int _termeDerivee;
+        volatile int _nombreActionsPeriodiques;
+        volatile double _commandeMoteurActuelle;
+};
+
+#endif
\ No newline at end of file

lib/QEI/QEI.cpp

+/**
+ * @author Aaron Berk
+ *
+ * @section LICENSE
+ *
+ * Copyright (c) 2010 ARM Limited
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ *
+ * @section DESCRIPTION
+ *
+ * Quadrature Encoder Interface.
+ *
+ * A quadrature encoder consists of two code tracks on a disc which are 90
+ * degrees out of phase. It can be used to determine how far a wheel has
+ * rotated, relative to a known starting position.
+ *
+ * Only one code track changes at a time leading to a more robust system than
+ * a single track, because any jitter around any edge won't cause a state
+ * change as the other track will remain constant.
+ *
+ * Encoders can be a homebrew affair, consisting of infrared emitters/receivers
+ * and paper code tracks consisting of alternating black and white sections;
+ * alternatively, complete disk and PCB emitter/receiver encoder systems can
+ * be bought, but the interface, regardless of implementation is the same.
+ *
+ *               +-----+     +-----+     +-----+
+ * Channel A     |  ^  |     |     |     |     |
+ *            ---+  ^  +-----+     +-----+     +-----
+ *               ^  ^
+ *               ^  +-----+     +-----+     +-----+
+ * Channel B     ^  |     |     |     |     |     |
+ *            ------+     +-----+     +-----+     +-----
+ *               ^  ^
+ *               ^  ^
+ *               90deg
+ *
+ * The interface uses X2 encoding by default which calculates the pulse count
+ * based on reading the current state after each rising and falling edge of
+ * channel A.
+ *
+ *               +-----+     +-----+     +-----+
+ * Channel A     |     |     |     |     |     |
+ *            ---+     +-----+     +-----+     +-----
+ *               ^     ^     ^     ^     ^
+ *               ^  +-----+  ^  +-----+  ^  +-----+
+ * Channel B     ^  |  ^  |  ^  |  ^  |  ^  |     |
+ *            ------+  ^  +-----+  ^  +-----+     +--
+ *               ^     ^     ^     ^     ^
+ *               ^     ^     ^     ^     ^
+ * Pulse count 0 1     2     3     4     5  ...
+ *
+ * This interface can also use X4 encoding which calculates the pulse count
+ * based on reading the current state after each rising and falling edge of
+ * either channel.
+ *
+ *               +-----+     +-----+     +-----+
+ * Channel A     |     |     |     |     |     |
+ *            ---+     +-----+     +-----+     +-----
+ *               ^     ^     ^     ^     ^
+ *               ^  +-----+  ^  +-----+  ^  +-----+
+ * Channel B     ^  |  ^  |  ^  |  ^  |  ^  |     |
+ *            ------+  ^  +-----+  ^  +-----+     +--
+ *               ^  ^  ^  ^  ^  ^  ^  ^  ^  ^
+ *               ^  ^  ^  ^  ^  ^  ^  ^  ^  ^
+ * Pulse count 0 1  2  3  4  5  6  7  8  9  ...
+ *
+ * It defaults
+ *
+ * An optional index channel can be used which determines when a full
+ * revolution has occured.
+ *
+ * If a 4 pules per revolution encoder was used, with X4 encoding,
+ * the following would be observed.
+ *
+ *               +-----+     +-----+     +-----+
+ * Channel A     |     |     |     |     |     |
+ *            ---+     +-----+     +-----+     +-----
+ *               ^     ^     ^     ^     ^
+ *               ^  +-----+  ^  +-----+  ^  +-----+
+ * Channel B     ^  |  ^  |  ^  |  ^  |  ^  |     |
+ *            ------+  ^  +-----+  ^  +-----+     +--
+ *               ^  ^  ^  ^  ^  ^  ^  ^  ^  ^
+ *               ^  ^  ^  ^  ^  ^  ^  ^  ^  ^
+ *               ^  ^  ^  +--+  ^  ^  +--+  ^
+ *               ^  ^  ^  |  |  ^  ^  |  |  ^
+ * Index      ------------+  +--------+  +-----------
+ *               ^  ^  ^  ^  ^  ^  ^  ^  ^  ^
+ * Pulse count 0 1  2  3  4  5  6  7  8  9  ...
+ * Rev.  count 0          1           2
+ *
+ * Rotational position in degrees can be calculated by:
+ *
+ * (pulse count / X * N) * 360
+ *
+ * Where X is the encoding type [e.g. X4 encoding => X=4], and N is the number
+ * of pulses per revolution.
+ *
+ * Linear position can be calculated by:
+ *
+ * (pulse count / X * N) * (1 / PPI)
+ *
+ * Where X is encoding type [e.g. X4 encoding => X=44], N is the number of
+ * pulses per revolution, and PPI is pulses per inch, or the equivalent for
+ * any other unit of displacement. PPI can be calculated by taking the
+ * circumference of the wheel or encoder disk and dividing it by the number
+ * of pulses per revolution.
+ */
+
+/**
+ * Includes
+ */
+#include "QEI.h"
+
+QEI::QEI(PinName channelA,
+         PinName channelB,
+         PinName index,
+         int pulsesPerRev,
+         Encoding encoding) : channelA_(channelA), channelB_(channelB),
+        index_(index) {
+
+    pulses_       = 0;
+    revolutions_  = 0;
+    pulsesPerRev_ = pulsesPerRev;
+    encoding_     = encoding;
+
+    //Workout what the current state is.
+    int chanA = channelA_.read();
+    int chanB = channelB_.read();
+
+    //2-bit state.
+    currState_ = (chanA << 1) | (chanB);
+    prevState_ = currState_;
+
+    //X2 encoding uses interrupts on only channel A.
+    //X4 encoding uses interrupts on      channel A,
+    //and on channel B.
+    channelA_.rise(this, &QEI::encode);
+    channelA_.fall(this, &QEI::encode);
+
+    //If we're using X4 encoding, then attach interrupts to channel B too.
+    if (encoding == X4_ENCODING) {
+        channelB_.rise(this, &QEI::encode);
+        channelB_.fall(this, &QEI::encode);
+    }
+    //Index is optional.
+    if (index !=  NC) {
+        index_.rise(this, &QEI::index);
+    }
+
+}
+
+void QEI::reset(void) {
+
+    pulses_      = 0;
+    revolutions_ = 0;
+
+}
+
+int QEI::getCurrentState(void) {
+
+    return currState_;
+
+}
+
+int QEI::getPulses(void) {
+
+    return pulses_;
+
+}
+
+int QEI::getRevolutions(void) {
+
+    return revolutions_;
+
+}
+
+// +-------------+
+// | X2 Encoding |
+// +-------------+
+//
+// When observing states two patterns will appear:
+//
+// Counter clockwise rotation:
+//
+// 10 -> 01 -> 10 -> 01 -> ...
+//
+// Clockwise rotation:
+//
+// 11 -> 00 -> 11 -> 00 -> ...
+//
+// We consider counter clockwise rotation to be "forward" and
+// counter clockwise to be "backward". Therefore pulse count will increase
+// during counter clockwise rotation and decrease during clockwise rotation.
+//
+// +-------------+
+// | X4 Encoding |
+// +-------------+
+//
+// There are four possible states for a quadrature encoder which correspond to
+// 2-bit gray code.
+//
+// A state change is only valid if of only one bit has changed.
+// A state change is invalid if both bits have changed.
+//
+// Clockwise Rotation ->
+//
+//    00 01 11 10 00
+//
+// <- Counter Clockwise Rotation
+//
+// If we observe any valid state changes going from left to right, we have
+// moved one pulse clockwise [we will consider this "backward" or "negative"].
+//
+// If we observe any valid state changes going from right to left we have
+// moved one pulse counter clockwise [we will consider this "forward" or
+// "positive"].
+//
+// We might enter an invalid state for a number of reasons which are hard to
+// predict - if this is the case, it is generally safe to ignore it, update
+// the state and carry on, with the error correcting itself shortly after.
+void QEI::encode(void) {
+
+    int change = 0;
+    int chanA  = channelA_.read();
+    int chanB  = channelB_.read();
+
+    //2-bit state.
+    currState_ = (chanA << 1) | (chanB);
+
+    if (encoding_ == X2_ENCODING) {
+
+        //11->00->11->00 is counter clockwise rotation or "forward".
+        if ((prevState_ == 0x3 && currState_ == 0x0) ||
+                (prevState_ == 0x0 && currState_ == 0x3)) {
+
+            pulses_++;
+
+        }
+        //10->01->10->01 is clockwise rotation or "backward".
+        else if ((prevState_ == 0x2 && currState_ == 0x1) ||
+                 (prevState_ == 0x1 && currState_ == 0x2)) {
+
+            pulses_--;
+
+        }
+
+    } else if (encoding_ == X4_ENCODING) {
+
+        //Entered a new valid state.
+        if (((currState_ ^ prevState_) != INVALID) && (currState_ != prevState_)) {
+            //2 bit state. Right hand bit of prev XOR left hand bit of current
+            //gives 0 if clockwise rotation and 1 if counter clockwise rotation.
+            change = (prevState_ & PREV_MASK) ^ ((currState_ & CURR_MASK) >> 1);
+
+            if (change == 0) {
+                change = -1;
+            }
+
+            pulses_ -= change;
+        }
+
+    }
+
+    prevState_ = currState_;
+
+}
+
+void QEI::index(void) {
+
+    revolutions_++;
+
+}

lib/QEI/QEI.h

+/**
+ * @author Aaron Berk
+ *
+ * @section LICENSE
+ *
+ * Copyright (c) 2010 ARM Limited
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ *
+ * @section DESCRIPTION
+ *
+ * Quadrature Encoder Interface.
+ *
+ * A quadrature encoder consists of two code tracks on a disc which are 90
+ * degrees out of phase. It can be used to determine how far a wheel has
+ * rotated, relative to a known starting position.
+ *
+ * Only one code track changes at a time leading to a more robust system than
+ * a single track, because any jitter around any edge won't cause a state
+ * change as the other track will remain constant.
+ *
+ * Encoders can be a homebrew affair, consisting of infrared emitters/receivers
+ * and paper code tracks consisting of alternating black and white sections;
+ * alternatively, complete disk and PCB emitter/receiver encoder systems can
+ * be bought, but the interface, regardless of implementation is the same.
+ *
+ *               +-----+     +-----+     +-----+
+ * Channel A     |  ^  |     |     |     |     |
+ *            ---+  ^  +-----+     +-----+     +-----
+ *               ^  ^
+ *               ^  +-----+     +-----+     +-----+
+ * Channel B     ^  |     |     |     |     |     |
+ *            ------+     +-----+     +-----+     +-----
+ *               ^  ^
+ *               ^  ^
+ *               90deg
+ *
+ * The interface uses X2 encoding by default which calculates the pulse count
+ * based on reading the current state after each rising and falling edge of
+ * channel A.
+ *
+ *               +-----+     +-----+     +-----+
+ * Channel A     |     |     |     |     |     |
+ *            ---+     +-----+     +-----+     +-----
+ *               ^     ^     ^     ^     ^
+ *               ^  +-----+  ^  +-----+  ^  +-----+
+ * Channel B     ^  |  ^  |  ^  |  ^  |  ^  |     |
+ *            ------+  ^  +-----+  ^  +-----+     +--
+ *               ^     ^     ^     ^     ^
+ *               ^     ^     ^     ^     ^
+ * Pulse count 0 1     2     3     4     5  ...
+ *
+ * This interface can also use X4 encoding which calculates the pulse count
+ * based on reading the current state after each rising and falling edge of
+ * either channel.
+ *
+ *               +-----+     +-----+     +-----+
+ * Channel A     |     |     |     |     |     |
+ *            ---+     +-----+     +-----+     +-----
+ *               ^     ^     ^     ^     ^
+ *               ^  +-----+  ^  +-----+  ^  +-----+
+ * Channel B     ^  |  ^  |  ^  |  ^  |  ^  |     |
+ *            ------+  ^  +-----+  ^  +-----+     +--
+ *               ^  ^  ^  ^  ^  ^  ^  ^  ^  ^
+ *               ^  ^  ^  ^  ^  ^  ^  ^  ^  ^
+ * Pulse count 0 1  2  3  4  5  6  7  8  9  ...
+ *
+ * It defaults
+ *
+ * An optional index channel can be used which determines when a full
+ * revolution has occured.
+ *
+ * If a 4 pules per revolution encoder was used, with X4 encoding,
+ * the following would be observed.
+ *
+ *               +-----+     +-----+     +-----+
+ * Channel A     |     |     |     |     |     |
+ *            ---+     +-----+     +-----+     +-----
+ *               ^     ^     ^     ^     ^
+ *               ^  +-----+  ^  +-----+  ^  +-----+
+ * Channel B     ^  |  ^  |  ^  |  ^  |  ^  |     |
+ *            ------+  ^  +-----+  ^  +-----+     +--
+ *               ^  ^  ^  ^  ^  ^  ^  ^  ^  ^
+ *               ^  ^  ^  ^  ^  ^  ^  ^  ^  ^
+ *               ^  ^  ^  +--+  ^  ^  +--+  ^
+ *               ^  ^  ^  |  |  ^  ^  |  |  ^
+ * Index      ------------+  +--------+  +-----------
+ *               ^  ^  ^  ^  ^  ^  ^  ^  ^  ^
+ * Pulse count 0 1  2  3  4  5  6  7  8  9  ...
+ * Rev.  count 0          1           2
+ *
+ * Rotational position in degrees can be calculated by:
+ *
+ * (pulse count / X * N) * 360
+ *
+ * Where X is the encoding type [e.g. X4 encoding => X=4], and N is the number
+ * of pulses per revolution.
+ *
+ * Linear position can be calculated by:
+ *
+ * (pulse count / X * N) * (1 / PPI)
+ *
+ * Where X is encoding type [e.g. X4 encoding => X=44], N is the number of
+ * pulses per revolution, and PPI is pulses per inch, or the equivalent for
+ * any other unit of displacement. PPI can be calculated by taking the
+ * circumference of the wheel or encoder disk and dividing it by the number
+ * of pulses per revolution.
+ */
+
+#ifndef QEI_H
+#define QEI_H
+
+/**
+ * Includes
+ */
+#include "mbed.h"
+
+/**
+ * Defines
+ */
+#define PREV_MASK 0x1 //Mask for the previous state in determining direction
+//of rotation.
+#define CURR_MASK 0x2 //Mask for the current state in determining direction
+//of rotation.
+#define INVALID   0x3 //XORing two states where both bits have changed.
+
+/**
+ * Quadrature Encoder Interface.
+ */
+class QEI {
+
+public:
+
+    typedef enum Encoding {
+
+        X2_ENCODING,
+        X4_ENCODING
+
+    } Encoding;
+
+    /**
+     * Constructor.
+     *
+     * Reads the current values on channel A and channel B to determine the
+     * initial state.
+     *
+     * Attaches the encode function to the rise/fall interrupt edges of
+     * channels A and B to perform X4 encoding.
+     *
+     * Attaches the index function to the rise interrupt edge of channel index
+     * (if it is used) to count revolutions.
+     *
+     * @param channelA mbed pin for channel A input.
+     * @param channelB mbed pin for channel B input.
+     * @param index    mbed pin for optional index channel input,
+     *                 (pass NC if not needed).
+     * @param pulsesPerRev Number of pulses in one revolution.
+     * @param encoding The encoding to use. Uses X2 encoding by default. X2
+     *                 encoding uses interrupts on the rising and falling edges
+     *                 of only channel A where as X4 uses them on both
+     *                 channels.
+     */
+    QEI(PinName channelA, PinName channelB, PinName index, int pulsesPerRev, Encoding encoding = X2_ENCODING);
+
+    /**
+     * Reset the encoder.
+     *
+     * Sets the pulses and revolutions count to zero.
+     */
+    void reset(void);
+
+    /**
+     * Read the state of the encoder.
+     *
+     * @return The current state of the encoder as a 2-bit number, where:
+     *         bit 1 = The reading from channel B
+     *         bit 2 = The reading from channel A
+     */
+    int getCurrentState(void);
+
+    /**
+     * Read the number of pulses recorded by the encoder.
+     *
+     * @return Number of pulses which have occured.
+     */
+    int getPulses(void);
+
+    /**
+     * Read the number of revolutions recorded by the encoder on the index channel.
+     *
+     * @return Number of revolutions which have occured on the index channel.
+     */
+    int getRevolutions(void);
+
+private:
+
+    /**