test
Dependencies: Encoder_Nucleo_16_bits
Diff: PID.h
- Revision:
- 5:d01614d14cd1
- Parent:
- 3:93fb84c4e9bc
- Child:
- 7:78cdcb637927
diff -r 5dea450cbc80 -r d01614d14cd1 PID.h --- 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