Diff: PID.h

Revision:

5:d01614d14cd1

Parent:

3:93fb84c4e9bc

Child:

7:78cdcb637927

--- a/PID.h	Wed May 23 15:10:19 2018 +0000
+++ b/PID.h	Tue Jun 05 07:24:40 2018 +0000
@@ -65,14 +65,16 @@
     /**
      * Constructor (standard) with full bridge control
      *
-     * @param _TIM (TIM_TypeDef*) : the Mbed 16 bits timer used to connect the A&B output of QE
-     * @param outPWM (PinName) : the Mbed pin used to send PWM on the full bridge
-     * @param outA (PinName) : the Mbed pin used to send direction to the full bridge
-     * @param outB (PinName) : the Mbed pin used to send direction (in case of 3 pins full bridge)
-     * @param scale (double) : the number of pulses on the QE for 1mm displacement of the wheel
-     * @param loopTime (double) : the time between 2 computations
+     * @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 * TIM, PinName outPWM, PinName outA, PinName outB, double scale = 63.661977236758, double loopTime = 0.001);
+    PID(TIM_TypeDef *LTIM, TIM_TypeDef *RTIM, PinName LPWM, PinName RPWM, PinName LDIRA, PinName LDIRB, PinName RDIRA, PinName RDIRB);
 
     /**
      * Set the Kp value
@@ -102,50 +104,85 @@
     float setDerivativeValue (float KdValue);
     
     /**
-     * compute the PWM value of a motion controled system
-     *
-     * @param  Setpoint value (in mm/s)
-     * @return  PWM value (between -1 and 1)
-     * @note    This function is provided for users that don't use a 3 wires full bridge
-     */
-    float computePWM (float setPoint); 
-    
-    /**
      * Set the Set point value of the speed for integrated full bridge
      *
-     * @param : Set point value (in mm/s)
+     * @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 setSpeed (float setPoint);
+    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);
     
     /**
-     * Get position of the wheel (in step of the Quadrature Encoder)
-     *
-     * @note One step is a fouth of a period of A or B signal
-     * @note Position is updated each time a motion computation take place
-     * @return The number of step from beggining
+     * 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
      */
-    long getPosition (void);
-    
-    float   _Kp, _Ki, _Kd, _PwmValue;
-    long    _Position;
+    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                     RobotIsStuck;  
 
 protected :
 
     Ticker                  _tick;
-    Nucleo_Encoder_16_bits  _encoder;
-    PwmOut                  _pwm;
-    DigitalOut              _outA, _outB;
+    Nucleo_Encoder_16_bits  _Lencoder, _Rencoder;
+    PwmOut                  _Lpwm, _Rpwm;
+    DigitalOut              _LdirA, _LdirB, _RdirA, _RdirB;
 
 private :    
 
     void    controlLoop();
     
-    float   _consigne;
-    double  _scale, _loopTime;
 };
 #endif //PID_H
\ No newline at end of file