-
Notifications
You must be signed in to change notification settings - Fork 0
/
pid.cpp
85 lines (73 loc) · 2.39 KB
/
pid.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
/**
* @file pid.cpp
* @author Fern Lane
* @brief PID-controller functions
*
* @copyright Copyright (c) 2024 Fern Lane
*
* This file is part of the ilet-103-motor-controller distribution.
* See <https://github.com/F33RNI/ilet-103-motor-controller> for more info.
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU Affero General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU Affero General Public License for more details.
*
* You should have received a copy of the GNU Affero General Public License
* long with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include <Arduino.h>
#include "include/config.h"
#include "include/pid.h"
/**
* @brief Constructs a new PID::PID object
* (wrapper for reset() that initializes class variables)
*/
PID::PID (void) { reset (); }
/**
* @brief Calculate one cycle of PID-controller
*
* @param error setpoint - input
* @param time_delta current time - previous time (in seconds)
* @param pid_output existing variable to write into (will be in [-PID_MIN_MAX_OUT, PID_MIN_MAX_OUT] range)
*/
float
PID::calculate (float error, float time_delta)
{
// Calculate P term
float p_output = PID_P_GAIN * error;
// Calculate I term
integral_accumulator += error * time_delta;
// Prevent integral windup
if (integral_accumulator > PID_MAX_INTEGRAL)
integral_accumulator = PID_MAX_INTEGRAL;
else if (integral_accumulator < PID_MIN_INTEGRAL)
integral_accumulator = PID_MIN_INTEGRAL;
// Calculate D term
float d_output = 0.f;
if (time_delta != 0.f)
d_output = PID_D_GAIN * ((error - error_prev) / time_delta);
error_prev = error;
// Calculate total output
float pid_output = p_output + PID_I_GAIN * integral_accumulator + d_output;
// Clamp output
if (pid_output > PID_MAX_OUT)
pid_output = PID_MAX_OUT;
else if (pid_output < PID_MIN_OUT)
pid_output = PID_MIN_OUT;
return pid_output;
}
/**
* @brief Resets PID's I and D variables
*/
void
PID::reset (void)
{
integral_accumulator = 0.f;
error_prev = 0.f;
}