Revision 1:81896d606b4e, committed 2018-06-06

Comitter:

haarkon

Date:

Wed Jun 06 20:28:39 2018 +0000

Parent:

0:86c5a1f6e21d

Commit message:

Test des fonction de l'asservissement

Changed in this revision

diff -r 86c5a1f6e21d -r 81896d606b4e Encoder_Nucleo_16_bits.lib
--- a/Encoder_Nucleo_16_bits.lib	Tue Jun 05 08:39:37 2018 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1 +0,0 @@
-https://os.mbed.com/teams/FRC-Hackathon/code/Encoder_Nucleo_16_bits/#6eeee6a11f47

diff -r 86c5a1f6e21d -r 81896d606b4e PID.cpp
--- a/PID.cpp	Tue Jun 05 08:39:37 2018 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,172 +0,0 @@
-#include "PID.h"
-
-PID::PID (TIM_TypeDef *LTIM, TIM_TypeDef *RTIM, PinName LPWM, PinName RPWM, PinName LDIRA, PinName LDIRB, PinName RDIRA, PinName RDIRB, PinName LED1) : _Lencoder(LTIM), _Rencoder(RTIM), _Lpwm(LPWM), _Rpwm(RPWM), _LdirA(LDIRA), _LdirB(LDIRB), _RdirA(RDIRA), _RdirB(RDIRB), _Led1(LED1)
-{
-    Ticker      _tick;
-    _tick.attach(callback(this, &PID::controlLoop), 0.001);
-
-    _Led1 = 0;
-    
-    _Lpwm.period_us(50);
-    _Lpwm = 0;
-
-    _Rpwm.period_us(50);
-    _Rpwm = 0;
-
-    _Kp = 2.0;
-    _Ki = 0.4;
-    _Kd = 1.0;
-
-}
-
-float PID::setProportionnalValue (float KpValue)
-{
-    _Kp = KpValue;
-    return _Kp;
-}
-
-float PID::setintegralValue (float KiValue)
-{
-    _Ki = KiValue;
-    return _Ki;
-}
-
-float PID::setDerivativeValue (float KdValue)
-{
-    _Kd = KdValue;
-    return _Kd;
-}
-
-void PID::setSpeed (double left, double right)
-{
-    _SpeedG = left;
-    _SpeedD = right;
-}
-
-void PID::getSpeed (double *vG, double *vD)
-{
-    *vG = _measSpeedG;
-    *vD = _measSpeedD;
-}
-
-void PID::getPosition (double *x, double *y, double *theta)
-{
-    *x = _X;
-    *y = _Y;
-    *theta = _THETA;
-}
-
-void PID::resetPosition (void)
-{
-    _X = 0;
-    _Y = 0;
-    _THETA = 0;
-}
-
-void PID::controlLoop()
-{
-    long            PositionG, PositionD;
-    static float    integralErrorG = 0, integralErrorD = 0;
-    static float    oldErrorG, oldErrorD;
-    double          errorG, commandeG;
-    double          errorD, commandeD;
-    double          meanDist, diffDist, deltaX, deltaY, deltaTheta;
-    static long     oldPositionG=0, oldPositionD=0;
-
-    _Led1 = ! _Led1;
-    PositionG =  _Lencoder.GetCounter();
-    PositionD = -_Rencoder.GetCounter();
-
-    // As we use mm/s for speed unit and we convert all mesure to this unit
-
-    // Converting step to millimeters
-    _measDistG = ((double)PositionG - (double)oldPositionG) / 63.662;
-    _measDistD = ((double)PositionD - (double)oldPositionD) / 63.662;
-
-    // Converting position to speed
-    _measSpeedG = 1000.0 * _measDistG;
-    _measSpeedD = 1000.0 * _measDistD;
-
-    // Computing errors
-    errorG = _SpeedG - _measSpeedG;
-    errorD = _SpeedD - _measSpeedD;
-    integralErrorG += errorG;
-    integralErrorD += errorD;
-
-    // Limiting integral error
-    if (integralErrorG > 10000) {
-        _WheelStuckG = 1;
-        integralErrorG = 10000;
-    } else {
-        if (integralErrorG < -10000) {
-            _WheelStuckG = 1;
-            integralErrorG = -10000;
-        } else {
-            _WheelStuckG = 0;
-        }
-    }
-
-    if (integralErrorD > 10000) {
-        _WheelStuckD = 1;
-        integralErrorD = 10000;
-    } else {
-        if (integralErrorD < -10000) {
-            _WheelStuckD = 1;
-            integralErrorD = -10000;
-        } else {
-            _WheelStuckD = 0;
-        }
-    }
-
-    // Correcting system (empiric test tells me that Kp = 4, Ki = 1 and Kd = 2, but this is probably not the good setting)
-    commandeG = _Kp * errorG + _Ki * integralErrorG + _Kd * (errorG - oldErrorG);
-    commandeD = _Kp * errorD + _Ki * integralErrorD + _Kd * (errorD - oldErrorD);
-
-    // Convert to PWM
-    _PwmValueG = commandeG / 1300.0;
-    if (_PwmValueG >  1) _PwmValueG =  1;
-    if (_PwmValueG < -1) _PwmValueG = -1;
-
-    if (_PwmValueG < 0) {
-        _Lpwm  = -_PwmValueG;
-        _LdirA = 0;
-        _LdirB = 1;
-    } else {
-        _Lpwm = _PwmValueG;
-        _LdirA = 1;
-        _LdirB = 0;
-    }
-
-    _PwmValueD = commandeD/1300.0;
-    if (_PwmValueD >  1) _PwmValueD =  1;
-    if (_PwmValueD < -1) _PwmValueD = -1;
-
-    if (_PwmValueD < 0) {
-        _Rpwm  = -_PwmValueD;
-        _RdirA = 1;
-        _RdirB = 0;
-    } else {
-        _Rpwm = _PwmValueD;
-        _RdirA = 0;
-        _RdirB = 1;
-    }
-
-    //odometry (segments approx.)
-    meanDist = (_measDistG + _measDistD) / 2.0;
-    diffDist =  _measDistG - _measDistD;
-
-    deltaX = meanDist * cos (_THETA);
-    deltaY = meanDist * sin (_THETA);
-    deltaTheta = diffDist / 230;
-
-    _X += deltaX;
-    _Y += deltaY;
-    _THETA += deltaTheta;
-    if (_THETA > 3.141592653589793) _THETA -= 6.283185307179586;
-    if (_THETA < -3.141592653589793) _THETA += 6.283185307179586;
-
-    oldPositionG = PositionG;
-    oldPositionD = PositionD;
-    oldErrorG = errorG;
-    oldErrorD = errorD;
-}

diff -r 86c5a1f6e21d -r 81896d606b4e PID.h
--- a/PID.h	Tue Jun 05 08:39:37 2018 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,191 +0,0 @@
-/**
- * @author Hugues Angelis
- *
- * @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
- *
- * motion control.
- *
- */
-
-#ifndef PID_H
-#define PID_H
-
-#define PI  3.1415926535898
-
-/**
- * Includes : Mbed Library + Nucleo_Encoder_16_bits library
- */
-#include "mbed.h"
-#include "Nucleo_Encoder_16_bits.h"
-
-/**
- * PID Motion control system (speed control).
- *  refere to wikipedia for more explainations : https://en.wikipedia.org/wiki/PID_controller
- *
- * @note This library require a tuning (mostly base on try/modify) and will not give a correct performance until you have tuned all 3 parameters Kp, Ki and Kd for each wheels of the robot
- *
- * @note You must use a QE (quadrature Encoder) connected to a 16 bits timer to get proper motion control. Pins A & B of the QE must be connected respectively to pin 1 and 2 of the timer. Typicaly on Nucleo F446RE TIM3 and TIM4 are perfect to do this job. In this case simply use TIM3 or TIM4 as timer parameter.
- *
- * @note You must also specify the number of pulses generated by the QE for a 1mm displacement of the wheel. This is the scale parameter
- *
- * @note Outputed PWM value evolve from 1 (full PWM fortward) to -1 (full PWM backward). This value can also be found in the global variable : _PwmValue. The PWM value is based on a 1.3 m/s maximum speed.
- *
- * @note As this motion control system is implemented in a microcontroler it is important to understand that there is a loop time for the motion control system and that this loop time MUST be constant. Kp, Ki and Kd are dependent of the loop time. Changing loop time means changing all the corrector's coefficients. Library use a Ticker to control loop time. The looptime can be set by software as long as it remain constant during the whole use of PID controler. Loop time is set to 1ms by default.
- *
- * @note The PID is initialized with Ki = 0, Kd = 0 and Kp = 1
- *       -  Increasing Kp will shorten your response time but will also create instability (at beggining overshoots, then instability).
- *       -  Adding a bit of Ki will allow your system to bring to 0 the static error (ie : will make the error, between the set point and your mesurment, tend to 0) but might create instability and increase setting time.
- *       -  Adding a bit of Kd will stabilize the response (with almost no bad effect, as long as Kd remains small).
- * @note More info can be found here : https://en.wikipedia.org/wiki/PID_controller
- */
-class PID
-{
-
-public :
-
-    /**
-     * Constructor (standard) with full bridge control
-     *
-     * @param LTIM (TIM_TypeDef*) : the Mbed 16 bits timer used to connect the A&B output of left QE
-     * @param RTIM (TIM_TypeDef*) : the Mbed 16 bits timer used to connect the A&B output of right QE
-     * @param LPWM (PinName) : the Mbed pin used to send PWM for the left wheel's full bridge
-     * @param RPWM (PinName) : the Mbed pin used to send PWM for the right wheel's full bridge
-     * @param LDIRA (PinName) : the Mbed pin used to send direction to the Left wheel IN1 full bridge pin
-     * @param LDIRB (PinName) : the Mbed pin used to send direction to the Left wheel IN2 full bridge pin
-     * @param RDIRA (PinName) : the Mbed pin used to send direction to the Right wheel IN1 full bridge pin
-     * @param RDIRB (PinName) : the Mbed pin used to send direction to the Right wheel IN2 full bridge pin
-     */
-    PID(TIM_TypeDef *LTIM, TIM_TypeDef *RTIM, PinName LPWM, PinName RPWM, PinName LDIRA, PinName LDIRB, PinName RDIRA, PinName RDIRB, PinName LED1);
-
-    /**
-     * Set the Kp value
-     *
-     * @param  Kp (float) : value of Kp
-     * @return The value of Kp (float)
-     * @note  Can also be accessed using the global variable _Kp
-     */
-    float setProportionnalValue (float KpValue);
-
-    /**
-     * Set the Ki value
-     *
-     * @param  Ki (float) : value of Ki
-     * @return The value of Ki (float)
-     * @note  Can also be accessed using the global variable _Ki
-     */
-    float setintegralValue (float KiValue);
-
-    /**
-     * Set the Kd value
-     *
-     * @param  Kd (float) : value of Kd
-     * @return The value of Kd (float)
-     * @note  Can also be accessed using the global variable _Kd
-     */
-    float setDerivativeValue (float KdValue);
-
-    /**
-     * Set the Set point value of the speed for integrated full bridge
-     *
-     * @param left  (double) : Set point value for left  wheel speed (in mm/s)
-     * @param right (double) : Set point value for right wheel speed (in mm/s)
-     */
-    void setSpeed (double left, double right);
-
-    /**
-     * Get position of the robot (in mm for X and Y and radian for Theta)
-     *
-     * @param x (double - passed by reference) : actual position of the center of the robot, in mm, along X axis (front of the robot at startup)
-     * @param y (double - passed by reference) : actual position of the center of the robot, in mm, along Y axis (left of the robot at startup)
-     * @param theta (double - passed by reference) : radian angle between the normal to the line passing through the 2 wheels and the angle of the robot at startup
-     * @note the position is updated each time a motion computation take place (ie : each milliseconds)
-     */
-    void getPosition (double *x, double *y, double *theta);
-
-    /**
-     * Reset position of the robot (in mm for X and Y and radian for Theta)
-     *
-     * @note the positionis equal to 0 on all 3 axis (cannot be undone)
-     */
-    void resetPosition (void);
-
-    /**
-     * Get speed of the two wheels of the robot
-     *
-     * @param vG (double - passed by reference) : actual speed in mm/s of the left wheel
-     * @param vD (double - passed by reference) : actual speed in mm/s of the right wheel
-     */
-    void getSpeed (double *vG, double *vD);
-
-    /**
-     * Global Variable of corrective values
-     * @note  works well with Kp = 2.0, Ki = 0.4 and Kd = 1
-     */
-    double                  _Kp, _Ki, _Kd;
-    /**
-     * Global Variable of speed
-     */
-    double                  _SpeedG, _SpeedD;
-    /**
-     * Global Variable of measured speed
-     */
-    double                  _measSpeedG, _measSpeedD;
-    /**
-     * Global Variable of measured displacement
-     */
-    double                  _measDistG, _measDistD;
-    /**
-     * Global Variable to indicate that required wheel speed is unreachable (set if speed is unreachable)
-     * @note    Must be cleared by user
-     */
-    int                     _WheelStuckG, _WheelStuckD;
-    /**
-     * Global Variable of wheel PWM value
-     */
-    double                  _PwmValueG, _PwmValueD;
-    /**
-     * Global Variable of gravity center of robot position (odometric, error increase with time)
-     */
-    double                  _X, _Y, _THETA;
-
-    /**
-     * Global Variable to indicate that required speed is unreachable (=1 if speed is unreachable)
-     * @note    Must be cleared by user
-     */
-    int                     RobotIsStuck;
-
-protected :
-
-    Nucleo_Encoder_16_bits  _Lencoder, _Rencoder;
-    PwmOut                  _Lpwm, _Rpwm;
-    DigitalOut              _LdirA, _LdirB, _RdirA, _RdirB;
-    DigitalOut              _Led1;
-
-private :
-
-    void    controlLoop();
-
-};
-#endif //PID_H
\ No newline at end of file

diff -r 86c5a1f6e21d -r 81896d606b4e PID.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PID.lib	Wed Jun 06 20:28:39 2018 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/teams/FRC-Hackathon/code/PID/#4553677e8b99

diff -r 86c5a1f6e21d -r 81896d606b4e main.cpp
--- a/main.cpp	Tue Jun 05 08:39:37 2018 +0000
+++ b/main.cpp	Wed Jun 06 20:28:39 2018 +0000
@@ -3,11 +3,11 @@
 #include "math.h"
 #include "PID.h"
 
-PID         motor       (TIM4, TIM3, PA_9, PA_8, PC_9, PC_8, PC_6, PC_5, PA_5);
+PID         motor       (TIM4, TIM3, PA_9, PA_8, PC_9, PC_8, PC_6, PC_5);
 
 Serial      pc          (PA_2, PA_3, 921600);                                   // Create a serial link to PC for communication
 
-//DigitalOut              led1        (PA_5);                                     // Added Led1 for test purpose
+DigitalOut              led1        (PA_5);                                     // Added Led1 for test purpose
 DigitalOut              led2        (PD_2);                                     // Added Led2 for test purpose
 DigitalOut              disquette   (PA_12);                                    // Added baloon destructor command (without it, you might see baloon destructor motor be set to full speed)
 
@@ -20,7 +20,7 @@
 //    led1 = 1;
     led2 = 0;
     disquette = 0;
-    
+
     motor.resetPosition();
 
     wait (5);
@@ -30,54 +30,141 @@
         // Square danse !!!
         motor.getPosition (&x,&y, &theta);
         motor.getSpeed (&vG, &vD);
-        
-        pc.printf ("\rEtape = %d : x = %5.1lf, y = %5.1lf, theta = %5.1lf - speedG = %5.1lf, speedD = %5.1lf", etatmvt, x, y, theta, vG, vD);
+
+        pc.printf ("\rEtape = %d : x = %5.1lf, y = %5.1lf, theta = %5.1lf - speedG = %5.1lf, speedD = %5.1lf", etatmvt, x, y, 180*theta/PI, vG, vD);
 
         switch (etatmvt) {
             case 0 :
                 // On avance de 50cm (coordonnés X = 500, Y = 0)
-                motor.setSpeed (400,400);
-                if (x >= 500) etatmvt = 1;
+                motor.setSpeed (300,300);
+                if (x > 500) {
+                    etatmvt = 1;
+                    pc.printf("\n");
+                }
                 break;
             case 1 :
                 // On tourne à droite de 90° (sens antitrigo => négatif) => Angle = -PI/2
-                motor.setSpeed (400,-400);
-                if (theta <= -PI/2.0) etatmvt = 2;
+                motor.setSpeed (150,-150);
+                if (theta < (-PI/2.0)) {
+                    etatmvt = 2;
+                    pc.printf("\n");
+                }
                 break;
             case 2 :
                 // On avance de 50cm (coordonnés X = 500, Y = -500)
-                motor.setSpeed (400,400);
-                if (y <= -500) etatmvt = 3;
+                motor.setSpeed (300,300);
+                if (y < -500)  {
+                    etatmvt = 3;
+                    pc.printf("\n");
+                }
                 break;
             case 3 :
                 // On tourne à droite de 90° (sens antitrigo => négatif) => Angle = -PI (Attention comme -PI = +PI, on teste le rebouclage)
-                motor.setSpeed (400,-400);
-                if (theta >= 0) etatmvt = 4;
+                motor.setSpeed (150,-150);
+                if (theta > 0)  {
+                    etatmvt = 4;
+                    pc.printf("\n");
+                }
                 break;
             case 4 :
                 // On avance de 50cm (coordonnés X = 0, Y = -500)
-                motor.setSpeed (400,400);
-                if (x <= 0) etatmvt = 5;
+                motor.setSpeed (300,300);
+                if (x < 0)  {
+                    etatmvt = 5;
+                    pc.printf("\n");
+                }
                 break;
             case 5 :
                 // On tourne à droite de 90° (sens antitrigo => négatif) => Angle = PI/2
-                motor.setSpeed (400,-400);
-                if (theta <= PI/2.0) etatmvt = 6;
+                motor.setSpeed (200,-200);
+                if (theta < (PI/2.0))  {
+                    etatmvt = 6;
+                    pc.printf("\n");
+                }
                 break;
             case 6 :
                 // On avance de 50cm (coordonnés X = 0, Y = 0)
-                motor.setSpeed (400,400);
-                if (y <= 0) etatmvt = 7;
+                motor.setSpeed (300,300);
+                if (y > 0)  {
+                    etatmvt = 7;
+                    pc.printf("\n");
+                }
                 break;
             case 7 :
                 // On tourne à droite de 90° (sens antitrigo => négatif) => Angle = 0
-                motor.setSpeed (400,-400);
-                if (theta <= 0) etatmvt = 0;
+                motor.setSpeed (200,-200);
+                if (theta < 0) {
+                    etatmvt = 8;
+                    pc.printf("\n");
+                }
+                break;
+            case 8 :
+                // On avance de 50cm (coordonnés X = 500, Y = 0)
+                motor.setSpeed (300,300);
+                if (x > 500) {
+                    etatmvt = 9;
+                    pc.printf("\n");
+                }
+                break;
+            case 9 :
+                // On tourne à droite de 90° (sens antitrigo => négatif) => Angle = -PI/2
+                motor.setSpeed (-150,150);
+                if (theta > (PI/2.0)) {
+                    etatmvt = 10;
+                    pc.printf("\n");
+                }
+                break;
+            case 10 :
+                // On avance de 50cm (coordonnés X = 500, Y = -500)
+                motor.setSpeed (300,300);
+                if (y > 500)  {
+                    etatmvt = 11;
+                    pc.printf("\n");
+                }
+                break;
+            case 11 :
+                // On tourne à droite de 90° (sens antitrigo => négatif) => Angle = -PI (Attention comme -PI = +PI, on teste le rebouclage)
+                motor.setSpeed (-150,150);
+                if (theta < 0)  {
+                    etatmvt = 12;
+                    pc.printf("\n");
+                }
+                break;
+            case 12 :
+                // On avance de 50cm (coordonnés X = 0, Y = -500)
+                motor.setSpeed (300,300);
+                if (x < 0)  {
+                    etatmvt = 13;
+                    pc.printf("\n");
+                }
+                break;
+            case 13 :
+                // On tourne à droite de 90° (sens antitrigo => négatif) => Angle = PI/2
+                motor.setSpeed (-200,200);
+                if (theta > (-PI/2.0))  {
+                    etatmvt = 14;
+                    pc.printf("\n");
+                }
+                break;
+            case 14 :
+                // On avance de 50cm (coordonnés X = 0, Y = 0)
+                motor.setSpeed (300,300);
+                if (y < 0)  {
+                    etatmvt = 15;
+                    pc.printf("\n");
+                }
+                break;
+            case 15 :
+                // On tourne à droite de 90° (sens antitrigo => négatif) => Angle = 0
+                motor.setSpeed (-200,200);
+                if (theta > 0) {
+                    etatmvt = 0;
+                    pc.printf("\n");
+                }
                 break;
         }
-
-//        led1 = !led1;
+        led1 = !led1;
         led2 = !led2;
-        wait (0.2);
+        wait (0.005);
     }
 }
\ No newline at end of file