Motion Control library for 2, 3 inputs Full Bridge + Quadrature Encoder, motor system (also called a 2 wheels robot)
Dependencies: Encoder_Nucleo_16_bits
Dependents: FRC_2018 FRC2018_Bis 0hackton_08_06_18 lib_FRC_2019 ... more
Diff: PID.h
- Revision:
- 9:e7c306f7e579
- Parent:
- 8:4553677e8b99
- Child:
- 10:a852385c6a34
--- a/PID.h Tue Jun 05 13:33:28 2018 +0000 +++ b/PID.h Mon Oct 14 15:19:54 2019 +0000 @@ -59,6 +59,10 @@ * - 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 The odometric position is computed by the trapezoidal integration method (approximation of a part of a circle by a trapezoid) after each program loop. Hence, odometric error increase with time. + * + * @note The odometric X coordinate is the direction in front of the robot at startup. Y coordinate is the orthogonal vector in trigonometric direction (counterclockwise) refered to X, and THETA is the angle of the robot refered to X at startup */ class PID { @@ -140,36 +144,77 @@ void getSpeed (double *vG, double *vD); /** - * Global Variable of corrective values + * Global Variable of corrective proportionnal value + * @note works well with Kp = 2.0, Ki = 0.4 and Kd = 1 + */ + double _Kp; + /** + * Global Variable of corrective integral value + * @note works well with Kp = 2.0, Ki = 0.4 and Kd = 1 + */ + double _Ki; + /** + * Global Variable of corrective derivative value * @note works well with Kp = 2.0, Ki = 0.4 and Kd = 1 */ - double _Kp, _Ki, _Kd; + double _Kd; /** - * Global Variable of speed + * Global Variable of Left speed */ - double _SpeedG, _SpeedD; + double _SpeedG, /** - * Global Variable of measured speed + * Global Variable of Right speed + */ + double _SpeedD, + /** + * Global Variable of measured left speed */ - double _measSpeedG, _measSpeedD; + double _measSpeedG; + /** + * Global Variable of measured right speed + */ + double _measSpeedD; /** - * Global Variable of measured displacement + * Global Variable of measured Left wheel displacement */ - double _measDistG, _measDistD; + double _measDistG; /** - * Global Variable to indicate that required wheel speed is unreachable (set if speed is unreachable) + * Global Variable of measured Right wheel displacement + */ + double _measDistD; + /** + * Global Variable to indicate that required Left wheel speed is unreachable (set if speed is unreachable) * @note Must be cleared by user */ - int _WheelStuckG, _WheelStuckD; + int _WheelStuckG; /** - * Global Variable of wheel PWM value + * Global Variable to indicate that required Right wheel speed is unreachable (set if speed is unreachable) + * @note Must be cleared by user + */ + int _WheelStuckD; + /** + * Global Variable of Left wheel PWM value + */ + double _PwmValueG; + /** + * Global Variable of Right wheel PWM value */ - double _PwmValueG, _PwmValueD; + double _PwmValueD; + /** + * Global Variable of odometric measurement of X position of gravity center of robot. + * @note odometric error increase with time + */ + double _X; /** - * Global Variable of gravity center of robot position (odometric, error increase with time) + * Global Variable of odometric measurement of Y position of gravity center of robot. + * @note odometric error increase with time */ - double _X, _Y, _THETA; - + double _Y; + /** + * Global Variable of odometric measurement of THETA angle of gravity center of robot. + * @note odometric error increase with time + */ + double _THETA; /** * Global Variable to indicate that required speed is unreachable (=1 if speed is unreachable) * @note Must be cleared by user