├── package.yml ├── .gitignore ├── README.markdown ├── .travis.yml ├── pid.h ├── pid.c └── tests └── pid_test.cpp /package.yml: -------------------------------------------------------------------------------- 1 | depends: 2 | - test-runner 3 | 4 | source: 5 | - pid.c 6 | 7 | tests: 8 | - tests/pid_test.cpp 9 | 10 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # # Editor swap files 2 | *.swp 3 | 4 | # Object files 5 | *.o 6 | *.ko 7 | *.obj 8 | *.elf 9 | 10 | # Libraries 11 | *.lib 12 | *.a 13 | 14 | # Shared objects (inc. Windows DLLs) 15 | *.dll 16 | *.so 17 | *.so.* 18 | *.dylib 19 | 20 | # Executables 21 | *.exe 22 | *.out 23 | *.app 24 | *.i*86 25 | *.x86_64 26 | *.hex 27 | 28 | # Build scripts and directories 29 | CMakeLists.txt 30 | build/* 31 | dependencies/* 32 | -------------------------------------------------------------------------------- /README.markdown: -------------------------------------------------------------------------------- 1 | # PID Controller 2 | [![Build Status](https://travis-ci.org/cvra/pid.png)](https://travis-ci.org/cvra/pid) 3 | 4 | This modules implements a PID controller in parallel form for use in various control loops. 5 | It has the following features : 6 | 7 | * Discrete PID in parallel form. 8 | * Maximum integrator value (ARW). 9 | 10 | 11 | ## Usage 12 | ```cpp 13 | pid_ctrl_t pid; 14 | pid_init(&pid); 15 | 16 | /* PD controller. */ 17 | pid_set_gains(&pid, 10., 0, 4.); 18 | 19 | while (1) { 20 | error = motor_position - setpoint; 21 | motor_pwm = pid_process(&pid, error); 22 | } 23 | 24 | ``` 25 | 26 | ## Frequency compensation 27 | The user of this module can specify a frequency for the PID loop and the gains 28 | will then be adjusted to allow the same gains to be used for various frequencies. 29 | 30 | This is done using the function `pid_set_frequency`. 31 | By default there is no compensation for the frequency of the PID. 32 | 33 | ## Dependencies 34 | None 35 | -------------------------------------------------------------------------------- /.travis.yml: -------------------------------------------------------------------------------- 1 | sudo: false 2 | addons: 3 | apt: 4 | sources: 5 | - deadsnakes 6 | packages: 7 | - python3.5 8 | - python3.5-dev 9 | - python-pip 10 | - python-virtualenv 11 | 12 | language: cpp 13 | compiler: 14 | - gcc 15 | - clang 16 | 17 | before_install: 18 | - python3.5 -m venv env --without-pip 19 | - source env/bin/activate 20 | - python --version 21 | - wget https://bootstrap.pypa.io/get-pip.py 22 | - python get-pip.py 23 | - pip install cvra-packager~=1.0.0 24 | - pip install msgpack-python==0.4.8 PyYAML==3.11 25 | 26 | - pushd .. 27 | - wget "https://github.com/cpputest/cpputest/releases/download/v3.8/cpputest-3.8.tar.gz" -O cpputest.tar.gz 28 | - tar -xzf cpputest.tar.gz 29 | - cd cpputest-3.8/ 30 | - ./configure --prefix=$HOME/cpputest 31 | - make 32 | - make install 33 | - popd 34 | 35 | before_script: 36 | - export CFLAGS="$CFLAGS -I $HOME/cpputest/include/" 37 | - export CXXFLAGS="$CXXFLAGS -I $HOME/cpputest/include/" 38 | - export LDFLAGS="$CXXFLAGS -L $HOME/cpputest/lib/" 39 | 40 | - packager 41 | - mkdir build/ 42 | - cd build/ 43 | - cmake .. 44 | 45 | script: 46 | - make 47 | - ./tests 48 | -------------------------------------------------------------------------------- /pid.h: -------------------------------------------------------------------------------- 1 | #ifndef PID_H_ 2 | #define PID_H_ 3 | 4 | /** Instance of a PID controller. 5 | * 6 | * @note This structure is only public to be able to do static allocation of it. 7 | * Do not access its fields directly. 8 | */ 9 | typedef struct { 10 | float kp; 11 | float ki; 12 | float kd; 13 | float integrator; 14 | float previous_error; 15 | float integrator_limit; 16 | float frequency; 17 | } pid_ctrl_t; 18 | 19 | /** Initializes a PID controller. */ 20 | void pid_init(pid_ctrl_t *pid); 21 | 22 | /** Sets the gains of the given PID. */ 23 | void pid_set_gains(pid_ctrl_t *pid, float kp, float ki, float kd); 24 | 25 | /** Returns the proportional gains of the controller. */ 26 | void pid_get_gains(const pid_ctrl_t *pid, float *kp, float *ki, float *kd); 27 | 28 | /** Returns the limit of the PID integrator. */ 29 | float pid_get_integral_limit(const pid_ctrl_t *pid); 30 | 31 | /** Returns the value of the PID integrator. */ 32 | float pid_get_integral(const pid_ctrl_t *pid); 33 | 34 | /** Process one step if the PID algorithm. */ 35 | float pid_process(pid_ctrl_t *pid, float error); 36 | 37 | /** Sets a maximum value for the PID integrator. */ 38 | void pid_set_integral_limit(pid_ctrl_t *pid, float max); 39 | 40 | /** Resets the PID integrator to zero. */ 41 | void pid_reset_integral(pid_ctrl_t *pid); 42 | 43 | /** Sets the PID frequency for gain compensation. */ 44 | void pid_set_frequency(pid_ctrl_t *pid, float frequency); 45 | 46 | /** Gets the PID frequency for gain compensation. */ 47 | float pid_get_frequency(const pid_ctrl_t *pid); 48 | 49 | #endif 50 | -------------------------------------------------------------------------------- /pid.c: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include "pid.h" 4 | 5 | 6 | void pid_init(pid_ctrl_t *pid) 7 | { 8 | pid_set_gains(pid, 1., 0., 0.); 9 | pid->integrator = 0.; 10 | pid->previous_error = 0.; 11 | pid->integrator_limit = INFINITY; 12 | pid->frequency = 1.; 13 | } 14 | 15 | void pid_set_gains(pid_ctrl_t *pid, float kp, float ki, float kd) 16 | { 17 | pid->kp = kp; 18 | pid->ki = ki; 19 | pid->kd = kd; 20 | } 21 | 22 | void pid_get_gains(const pid_ctrl_t *pid, float *kp, float *ki, float *kd) 23 | { 24 | *kp = pid->kp; 25 | *ki = pid->ki; 26 | *kd = pid->kd; 27 | } 28 | 29 | float pid_get_integral_limit(const pid_ctrl_t *pid) 30 | { 31 | return pid->integrator_limit; 32 | } 33 | 34 | float pid_get_integral(const pid_ctrl_t *pid) 35 | { 36 | return pid->integrator; 37 | } 38 | 39 | float pid_process(pid_ctrl_t *pid, float error) 40 | { 41 | float output; 42 | pid->integrator += error; 43 | 44 | if (pid->integrator > pid->integrator_limit) { 45 | pid->integrator = pid->integrator_limit; 46 | } else if (pid->integrator < -pid->integrator_limit) { 47 | pid->integrator = -pid->integrator_limit; 48 | } 49 | 50 | output = - pid->kp * error; 51 | output += - pid->ki * pid->integrator / pid->frequency; 52 | output += - pid->kd * (error - pid->previous_error) * pid->frequency; 53 | 54 | pid->previous_error = error; 55 | return output; 56 | } 57 | 58 | void pid_set_integral_limit(pid_ctrl_t *pid, float max) 59 | { 60 | pid->integrator_limit = max; 61 | } 62 | 63 | void pid_reset_integral(pid_ctrl_t *pid) 64 | { 65 | pid->integrator = 0.; 66 | } 67 | 68 | void pid_set_frequency(pid_ctrl_t *pid, float frequency) 69 | { 70 | pid->frequency = frequency; 71 | } 72 | 73 | float pid_get_frequency(const pid_ctrl_t *pid) 74 | { 75 | return pid->frequency; 76 | } 77 | -------------------------------------------------------------------------------- /tests/pid_test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | extern "C" { 4 | #include "../pid.h" 5 | } 6 | 7 | TEST_GROUP(PIDTestGroup) 8 | { 9 | pid_ctrl_t pid; 10 | 11 | /** Processes a given input in the PID controller and compares it against 12 | * expected output. */ 13 | void process_and_expect(float input, float expected_output) 14 | { 15 | float output; 16 | output = pid_process(&pid, input); 17 | DOUBLES_EQUAL(expected_output, output, 1e-3); 18 | } 19 | 20 | void setup(void) 21 | { 22 | pid_init(&pid); 23 | } 24 | }; 25 | 26 | TEST(PIDTestGroup, IntegralValueIsZeroAtInit) 27 | { 28 | CHECK_EQUAL(0, pid_get_integral(&pid)); 29 | } 30 | 31 | TEST(PIDTestGroup, PControllerAtInit) 32 | { 33 | float kp,ki,kd; 34 | pid_get_gains(&pid,&kp,&ki,&kd); 35 | CHECK_EQUAL(1., kp); 36 | CHECK_EQUAL(0., ki); 37 | CHECK_EQUAL(0., kd); 38 | CHECK_EQUAL(1., pid_get_frequency(&pid)) 39 | } 40 | 41 | TEST(PIDTestGroup, CanSetGains) 42 | { 43 | float kp,ki,kd; 44 | pid_set_gains(&pid, 10., 20., 30.); 45 | pid_get_gains(&pid,&kp,&ki,&kd); 46 | CHECK_EQUAL(10., kp); 47 | CHECK_EQUAL(20., ki); 48 | CHECK_EQUAL(30., kd); 49 | } 50 | 51 | TEST(PIDTestGroup, CanSetLimit) 52 | { 53 | pid_set_integral_limit(&pid,100.); 54 | CHECK_EQUAL(100., pid_get_integral_limit(&pid)); 55 | } 56 | 57 | TEST(PIDTestGroup, CanSetFrequency) 58 | { 59 | pid_set_frequency(&pid,100.); 60 | CHECK_EQUAL(100., pid_get_frequency(&pid)); 61 | } 62 | 63 | TEST(PIDTestGroup, ZeroErrorMakesForZeroOutput) 64 | { 65 | process_and_expect(0., 0.); 66 | } 67 | 68 | TEST(PIDTestGroup, ByDefaultIsIdentity) 69 | { 70 | process_and_expect(42., -42.); 71 | } 72 | 73 | TEST(PIDTestGroup, KpHasInfluenceOnOutput) 74 | { 75 | pid_set_gains(&pid, 2., 0., 0.); 76 | process_and_expect(21., -42.); 77 | } 78 | 79 | TEST(PIDTestGroup, IntegratorIsChangedByProcess) 80 | { 81 | pid_process(&pid, 42.); 82 | CHECK_EQUAL(42., pid_get_integral(&pid)); 83 | 84 | pid_process(&pid, 42.); 85 | CHECK_EQUAL(84., pid_get_integral(&pid)); 86 | } 87 | 88 | TEST(PIDTestGroup, AddingKiMakesOutputIncrease) 89 | { 90 | pid_set_gains(&pid, 0., 2., 0.); 91 | process_and_expect(42., -84.); 92 | process_and_expect(42., -168.); 93 | } 94 | 95 | TEST(PIDTestGroup, IntegratorWorksInNegativeToo) 96 | { 97 | pid_set_gains(&pid, 0., 1., 0.); 98 | process_and_expect(-42., 42.); 99 | process_and_expect(-42., 84.); 100 | } 101 | 102 | TEST(PIDTestGroup, AddingKdCreatesDerivativeAction) 103 | { 104 | pid_set_gains(&pid, 0., 0., 2.); 105 | process_and_expect(42., -84.); 106 | process_and_expect(42., 0); 107 | } 108 | 109 | TEST(PIDTestGroup, IntegratorMaxValueIsRespected) 110 | { 111 | pid_set_integral_limit(&pid, 20.); 112 | pid_set_gains(&pid, 0., 1., 0.); 113 | process_and_expect(20., -20.); 114 | process_and_expect(20., -20.); 115 | } 116 | 117 | TEST(PIDTestGroup, IntegratorMaxValueWorksInNegativeToo) 118 | { 119 | pid_set_integral_limit(&pid, 20.); 120 | pid_set_gains(&pid, 0., 1., 0.); 121 | 122 | process_and_expect(-20., 20.); 123 | process_and_expect(-20., 20.); 124 | } 125 | 126 | TEST(PIDTestGroup, CanResetIntegrator) 127 | { 128 | pid_set_gains(&pid, 0., 1., 0.); 129 | process_and_expect(20., -20.); 130 | process_and_expect(20., -40.); 131 | 132 | pid_reset_integral(&pid); 133 | process_and_expect(20.,- 20.); 134 | } 135 | 136 | TEST(PIDTestGroup, FrequencyChangeIntegrator) 137 | { 138 | pid_set_frequency(&pid, 10.); 139 | pid_set_gains(&pid, 0., 1., 0.); 140 | process_and_expect(20., -2.); 141 | process_and_expect(20., -4.); 142 | } 143 | 144 | TEST(PIDTestGroup, FrequencyChangeDerivative) 145 | { 146 | pid_set_frequency(&pid, 10.); 147 | pid_set_gains(&pid, 0., 0., 1.); 148 | process_and_expect(20., -200.); 149 | process_and_expect(20., 0.); 150 | } 151 | 152 | --------------------------------------------------------------------------------