├── README.md ├── kharsansky-quadrotor-control-thesis.pdf ├── qPIDs.c ├── qPIDs.h └── qpid_diagram.png /README.md: -------------------------------------------------------------------------------- 1 | # qPID Digital PID controller library 2 | 3 | ## Introduction 4 | This library is a digital Proportional-Integrative-Derivative digital controller designed for embedded systems to interact with real systems. 5 | 6 | ## Principle of work and definitions 7 | ### Block Diagram 8 | ![Block diagram](qpid_diagram.png) 9 | 10 | ### Detailed information 11 | Full mathematical description and testing results of this libarary can be found (in spanish) in [kharsansky-quadrotor-control-thesis](kharsansky-quadrotor-control-thesis.pdf). 12 | 13 | ## Usage example 14 | 15 | ```C 16 | 17 | #include "qPIDs.h" 18 | 19 | float setPoint = 0.0; 20 | float sensor = 0.0; 21 | float output = 0.0; 22 | 23 | 24 | // Example usage for the qPID library for controlling the speed of a simle motor 25 | // The idea is to control the speed of the motor by varing the voltage applied via PWM 26 | 27 | float readSensor(void); // A function that reads the motor speed from a sensor 28 | float readSetPoint(void); // A function that reads the desired speed from a potentiometer 29 | void setActuator(float); // A function that sets the PWM output for controlling the motor 30 | 31 | void example(void){ 32 | 33 | // Declare de new object 34 | qPID controller; 35 | 36 | // Configure settings 37 | controller.AntiWindup = ENABLED; 38 | controller.Bumpless = ENABLED; 39 | 40 | // Set mode to auotmatic (otherwise it will be in manual mode) 41 | controller.Mode = AUTOMATIC; 42 | 43 | // Configure de output limits for clamping 44 | controller.OutputMax = 1.0; 45 | controller.OutputMin = -1.0; 46 | 47 | // Set the rate at the PID will run in seconds 48 | controller.Ts = 0.005; 49 | 50 | // More settings 51 | controller.b = 1.0; 52 | controller.c = 1.0; 53 | 54 | // Init de controller 55 | qPID_Init(&controller); 56 | 57 | // Set the tunning constants 58 | controller.K = 0.5; 59 | controller.Ti = 1/0.02; 60 | controller.Td = 0.0; 61 | controller.Nd = 4.0; 62 | 63 | while (1){ 64 | sensor = readSensor(); // update the process variable 65 | setPoint = readSetPoint(); // update the user desired value 66 | 67 | // Update the PID and get the new output 68 | output = qPID_Process(&controller, setPoint, sensor); 69 | 70 | setActuator(output); // update the actuator input 71 | } 72 | 73 | } 74 | ``` 75 | -------------------------------------------------------------------------------- /kharsansky-quadrotor-control-thesis.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/akharsa/qPID/8162bb62fa0cb65b8ae3ca14d866c9d3542535b1/kharsansky-quadrotor-control-thesis.pdf -------------------------------------------------------------------------------- /qPIDs.c: -------------------------------------------------------------------------------- 1 | /* 2 | * The MIT License (MIT) 3 | * Copyright (c) <2012> 4 | 5 | * Permission is hereby granted, free of charge, to any person obtaining a copy 6 | * of this software and associated documentation files (the "Software"), to deal 7 | * in the Software without restriction, including without limitation the rights 8 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | * copies of the Software, and to permit persons to whom the Software is furnished 10 | * to do so, subject to the following conditions: 11 | 12 | * The above copyright notice and this permission notice shall be included in all 13 | * copies or substantial portions of the Software. 14 | 15 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 18 | * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 21 | * THE SOFTWARE. 22 | */ 23 | 24 | #include 25 | #include 26 | #include "qPIDs.h" 27 | 28 | void qPID_Init(qPID *q) 29 | { 30 | q->ctx.Ui_old = 0.0; 31 | q->ctx.Ud_old = 0.0; 32 | q->ctx.PV_old = 0.0; 33 | q->ctx.SP_old = 0.0; 34 | q->Mode = OFF; 35 | } 36 | 37 | float qPID_Process_(qPID *q, float Input, float PV, float terms[]) 38 | { 39 | 40 | 41 | // ===================================== 42 | // MANUAL 43 | // ===================================== 44 | // Input = manual input -> CO 45 | 46 | // ===================================== 47 | // AUTOMATIC 48 | // ===================================== 49 | // Input = Setpoint 50 | 51 | // For local use 52 | float ControllerOutput; 53 | float Up, Ui, Ud; 54 | float Kp, Ki, Kd_a, Kd_b, Kd_c; 55 | 56 | 57 | if (fabsf(q->Ti) < EPSILON) { 58 | q->Ti = EPSILON; 59 | } 60 | if (fabsf(q->Nd) < EPSILON) { 61 | q->Nd = EPSILON; 62 | } 63 | Kp = q->K; 64 | Ki = ((q->K) * (q->Ts)) / (q->Ti); 65 | Kd_a = q->Td / (q->Td + q->Nd * q->Ts) ; 66 | Kd_b = (q->K * q->Td * q->Nd) / (q->Td + q->Nd * q->Ts); 67 | Kd_c = (q->c * q->K * q->Td * q->Nd) / (q->Td + q->Nd * q->Ts); 68 | 69 | 70 | // Proportional gain 71 | Up = Kp * (((q->b) * (Input)) - PV); 72 | 73 | // Deriative gain with filter 74 | Ud = Kd_a * (q->ctx.Ud_old) - Kd_b * (PV - q->ctx.PV_old) + Kd_c * (Input - q->ctx.SP_old); 75 | 76 | // Get last integral 77 | Ui = q->ctx.Ui_old; 78 | 79 | // Calculate controler output for Automatic or manual mode 80 | 81 | switch (q->Mode) { 82 | case MANUAL: 83 | ControllerOutput = Input; 84 | 85 | if (q->Bumpless == ENABLED) { 86 | q->ctx.Ui_old = PV; 87 | } 88 | 89 | break; 90 | 91 | case AUTOMATIC: 92 | ControllerOutput = Up + Ui + Ud; 93 | if (ControllerOutput > q->OutputMax) { 94 | ControllerOutput = q->OutputMax; 95 | } else if (ControllerOutput < q->OutputMin) { 96 | ControllerOutput = q->OutputMin; 97 | } 98 | break; 99 | 100 | case RELAY: 101 | if ((Input - PV) >= 0) { 102 | ControllerOutput = q->OutputMax; 103 | } else { 104 | ControllerOutput = q->OutputMin; 105 | } 106 | break; 107 | case OFF: 108 | ControllerOutput = 0; 109 | break; 110 | 111 | default: 112 | // ERROR 113 | ControllerOutput = NAN; 114 | break; 115 | } 116 | 117 | // Output parameters for debug 118 | if (terms != NULL) { 119 | terms[0] = Up; 120 | terms[1] = Ui; 121 | terms[2] = Ud; 122 | } 123 | 124 | // Anti Windup 125 | if ((q->AntiWindup == ENABLED) && (q->Mode == AUTOMATIC)) { 126 | //if (Ui >= q->OutputMax) { 127 | if (ControllerOutput >= q->OutputMax) { 128 | // do not integrate anymore 129 | } else if (ControllerOutput <= q->OutputMin) { 130 | // do not integrate anymore 131 | } else { 132 | Ui = q->ctx.Ui_old + Ki * ((Input) - PV); 133 | } 134 | } else { 135 | // Calc de integral for the next step in any other case 136 | Ui = q->ctx.Ui_old + Ki * ((Input) - PV); 137 | } 138 | 139 | // Save context for next step. 140 | q->ctx.Ui_old = Ui; 141 | q->ctx.Ud_old = Ud; 142 | q->ctx.PV_old = PV; 143 | q->ctx.SP_old = Input; 144 | 145 | return ControllerOutput; 146 | } 147 | -------------------------------------------------------------------------------- /qPIDs.h: -------------------------------------------------------------------------------- 1 | /* 2 | * The MIT License (MIT) 3 | * Copyright (c) <2012> 4 | 5 | * Permission is hereby granted, free of charge, to any person obtaining a copy 6 | * of this software and associated documentation files (the "Software"), to deal 7 | * in the Software without restriction, including without limitation the rights 8 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | * copies of the Software, and to permit persons to whom the Software is furnished 10 | * to do so, subject to the following conditions: 11 | 12 | * The above copyright notice and this permission notice shall be included in all 13 | * copies or substantial portions of the Software. 14 | 15 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 18 | * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 21 | * THE SOFTWARE. 22 | */ 23 | 24 | #ifndef __qPID_CONTROLLER_H__ 25 | #define __qPID_CONTROLLER_H__ 26 | 27 | #include 28 | 29 | //================================================================ 30 | // Types 31 | //================================================================ 32 | 33 | typedef enum { 34 | DISABLED = 0, 35 | ENABLED 36 | } qPID_Feature; 37 | 38 | typedef enum { 39 | MANUAL = 0, 40 | AUTOMATIC, 41 | RELAY, 42 | OFF 43 | } qPID_Mode; 44 | 45 | typedef struct { 46 | float PV_old; 47 | float Ui_old; 48 | float Ud_old; 49 | float SP_old; 50 | } qPID_Context; 51 | 52 | typedef struct { 53 | 54 | // Parameters: 55 | float K, Ti, Td; // For use in NON-INT or INT modes 56 | 57 | float OutputMax; // For windup 58 | float OutputMin; // For windup 59 | 60 | float Nd; // For derivator 61 | float b, c; // For setpoint Weighting 62 | 63 | float Ts; // General propoerty 64 | 65 | // Features: 66 | qPID_Mode Mode; 67 | qPID_Feature AntiWindup; 68 | qPID_Feature Bumpless; 69 | 70 | qPID_Context ctx; 71 | 72 | } qPID; 73 | 74 | //================================================================ 75 | // Defines 76 | //================================================================ 77 | #define EPSILON 0.0000001f 78 | 79 | //================================================================ 80 | // Prototypes 81 | //================================================================ 82 | void qPID_Init(qPID *q); 83 | float qPID_Process_(qPID *q, float Input, float ProcessVariable, float terms[]); 84 | #define qPID_Process(pPID,input,pv) qPID_Process_(pPID,input,pv, NULL); 85 | 86 | #endif 87 | -------------------------------------------------------------------------------- /qpid_diagram.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/akharsa/qPID/8162bb62fa0cb65b8ae3ca14d866c9d3542535b1/qpid_diagram.png --------------------------------------------------------------------------------