Diff: PID.h

Revision:

8:4553677e8b99

Parent:

7:78cdcb637927

Child:

9:e7c306f7e579

--- a/PID.h	Tue Jun 05 12:27:28 2018 +0000
+++ b/PID.h	Tue Jun 05 13:33:28 2018 +0000
@@ -32,6 +32,8 @@
 #ifndef PID_H
 #define PID_H
 
+#define PI  3.1415926535898
+
 /**
  * Includes : Mbed Library + Nucleo_Encoder_16_bits library
  */
@@ -44,21 +46,22 @@
  *
  * @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 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 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 
+ * @note More info can be found here : https://en.wikipedia.org/wiki/PID_controller
  */
-class PID {
+class PID
+{
 
 public :
 
@@ -84,7 +87,7 @@
      * @note  Can also be accessed using the global variable _Kp
      */
     float setProportionnalValue (float KpValue);
-    
+
     /**
      * Set the Ki value
      *
@@ -93,7 +96,7 @@
      * @note  Can also be accessed using the global variable _Ki
      */
     float setintegralValue (float KiValue);
-    
+
     /**
      * Set the Kd value
      *
@@ -102,7 +105,7 @@
      * @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
      *
@@ -110,7 +113,7 @@
      * @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)
      *
@@ -135,7 +138,7 @@
      * @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
@@ -171,15 +174,18 @@
      * Global Variable to indicate that required speed is unreachable (=1 if speed is unreachable)
      * @note    Must be cleared by user
      */
-    int                     RobotIsStuck;  
+    int                     RobotIsStuck;
 
-private :    
+protected :
 
-    void    controlLoop();
-    Ticker                  _tick;
     Nucleo_Encoder_16_bits  _Lencoder, _Rencoder;
     PwmOut                  _Lpwm, _Rpwm;
     DigitalOut              _LdirA, _LdirB, _RdirA, _RdirB;
-    
+
+private :
+
+    Ticker      _tick;
+    void    controlLoop();
+
 };
 #endif //PID_H
\ No newline at end of file