Revision 0:242dc469ac2b, committed 2018-06-06

Comitter:

haarkon

Date:

Wed Jun 06 20:53:45 2018 +0000

Commit message:

Programme de test des fonctions d'asservissement (en interne)

Changed in this revision

diff -r 000000000000 -r 242dc469ac2b Encoder_Nucleo_16_bits.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Encoder_Nucleo_16_bits.lib	Wed Jun 06 20:53:45 2018 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/teams/FRC-Hackathon/code/Encoder_Nucleo_16_bits/#6eeee6a11f47

diff -r 000000000000 -r 242dc469ac2b main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Jun 06 20:53:45 2018 +0000
@@ -0,0 +1,299 @@
+#include "mbed.h"
+#include "Nucleo_Encoder_16_bits.h"
+
+#define PI  3.1415926535898
+
+Nucleo_Encoder_16_bits  _Lencoder   (TIM4);
+Nucleo_Encoder_16_bits  _Rencoder   (TIM3);
+PwmOut                  _Lpwm       (PA_9);
+PwmOut                  _Rpwm       (PA_8);
+DigitalOut              _LdirA      (PC_9);
+DigitalOut              _LdirB      (PC_8);
+DigitalOut              _RdirA      (PC_6);
+DigitalOut              _RdirB      (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              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)
+
+Ticker      _tick;
+
+void controlLoop(void);
+
+/**
+ * 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;
+
+int main ()
+{
+    _tick.attach(&controlLoop, 0.001);
+
+    _Lpwm.period_us(50);
+    _Lpwm = 0;
+
+    _Rpwm.period_us(50);
+    _Rpwm = 0;
+
+    _Kp = 2.0;
+    _Ki = 0.4;
+    _Kd = 1.0;
+
+    pc.printf ("\n\rHelloWorld\n");
+    led1 = 1;
+    led2 = 0;
+    disquette = 0;
+    resetPosition();
+    wait(3);
+
+    setSpeed (80,80);
+    while (1);
+}
+
+float setProportionnalValue (float KpValue)
+{
+    _Kp = KpValue;
+    return _Kp;
+}
+
+float setintegralValue (float KiValue)
+{
+    _Ki = KiValue;
+    return _Ki;
+}
+
+float setDerivativeValue (float KdValue)
+{
+    _Kd = KdValue;
+    return _Kd;
+}
+
+void setSpeed (double left, double right)
+{
+    _SpeedG = left;
+    _SpeedD = right;
+}
+
+void getSpeed (double *vG, double *vD)
+{
+    *vG = _measSpeedG;
+    *vD = _measSpeedD;
+}
+
+void getPosition (double *x, double *y, double *theta)
+{
+    *x = _X;
+    *y = _Y;
+    *theta = _THETA;
+}
+
+void resetPosition (void)
+{
+    _X = 0;
+    _Y = 0;
+    _THETA = 0;
+}
+
+void 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;
+
+    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 =  _measDistD - _measDistG;
+
+    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;
+
+    pc.printf ("\rG = %5.1lf - %5.1lf = %5.1lf -> %5.1lf -> %4.2lf\n", _SpeedG, _measDistG, errorG, commandeG, _PwmValueG);
+    pc.printf ("\rD = %5.1lf - %5.1lf = %5.1lf -> %5.1lf -> %4.2lf\n", _SpeedD, _measDistD, errorD, commandeD, _PwmValueD);
+}

diff -r 000000000000 -r 242dc469ac2b mbed.bld
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Wed Jun 06 20:53:45 2018 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/5aab5a7997ee
\ No newline at end of file