logic_mesh/blocks/control/
pid.rs

1// Copyright (c) 2022-2023, Radu Racariu.
2
3use std::time::Duration;
4
5use uuid::Uuid;
6
7use crate::base::output::props::OutputProps;
8use crate::blocks::utils::input_as_number;
9use crate::blocks::utils::input_to_millis_or_default;
10use crate::{
11    base::{
12        block::{Block, BlockDesc, BlockProps, BlockState},
13        input::{input_reader::InputReader, Input, InputProps},
14        output::Output,
15    },
16    blocks::utils::input_as_float_or_default,
17};
18use libhaystack::val::kind::HaystackKind;
19
20use crate::{blocks::InputImpl, blocks::OutputImpl};
21
22/// Outputs the PID loop result based on input, sp, KP, KI, and KD inputs.
23/// If input is not connected, the output value is used for the input.
24#[block]
25#[derive(BlockProps, Debug)]
26#[category = "control"]
27pub struct Pid {
28    #[input(kind = "Number")]
29    pub input: InputImpl,
30
31    #[input(kind = "Number")]
32    pub sp: InputImpl,
33
34    #[input(kind = "Number")]
35    pub kp: InputImpl,
36
37    #[input(kind = "Number")]
38    pub ki: InputImpl,
39
40    #[input(kind = "Number")]
41    pub kd: InputImpl,
42
43    #[input(kind = "Number")]
44    pub interval: InputImpl,
45
46    #[input(kind = "Number")]
47    pub min: InputImpl,
48
49    #[input(kind = "Number")]
50    pub max: InputImpl,
51
52    #[input(kind = "Number")]
53    pub bias: InputImpl,
54
55    #[output(kind = "Number")]
56    pub out: OutputImpl,
57
58    integral: f64,
59    derivative: f64,
60    last_error: f64,
61    last_value: f64,
62}
63
64impl Block for Pid {
65    async fn execute(&mut self) {
66        let millis = input_to_millis_or_default(&self.interval.val);
67
68        self.wait_on_inputs(Duration::from_millis(millis)).await;
69
70        if !self.out.is_connected() {
71            return;
72        }
73
74        let kp = input_as_number(&self.kp).map(|v| v.value).unwrap_or(0.98);
75        let ki = input_as_number(&self.ki).map(|v| v.value).unwrap_or(0.002);
76        let kd = input_as_number(&self.kd).map(|v| v.value).unwrap_or(0.25);
77        let bias = input_as_number(&self.kd).map(|v| v.value).unwrap_or(100.0);
78
79        if kp <= 0.0 || ki <= 0.0 || kd <= 0.0 {
80            return;
81        }
82
83        let sp = input_as_float_or_default(&self.sp);
84        let min = input_as_number(&self.min).map(|v| v.value).unwrap_or(0.0);
85        let max = input_as_number(&self.max).map(|v| v.value).unwrap_or(100.0);
86        let time = millis as f64;
87
88        let cur_value = if self.input.is_connected() {
89            input_as_number(&self.input).map(|v| v.value).unwrap_or(0.0)
90        } else {
91            TryInto::<f64>::try_into(self.out.value()).unwrap_or(0.0)
92        };
93
94        // Error
95        let error = sp - cur_value;
96
97        // Proportional term
98        let proportional = kp * error;
99
100        // Integral term
101        self.integral += ki * time * (error + self.last_error);
102        self.integral = self.integral.clamp(min, max);
103
104        // Derivative term
105        self.derivative = -(bias * kd * (cur_value - self.last_value)
106            + (bias - time) * self.derivative)
107            / (bias + time);
108
109        let mut output = proportional + self.integral + self.derivative;
110
111        if output > max {
112            output = max;
113        } else if output < min {
114            output = min;
115        }
116
117        self.last_value = output;
118        self.last_error = error;
119
120        self.out.set(output.into());
121    }
122}
123
124#[cfg(test)]
125mod test {
126
127    use crate::{
128        base::block::test_utils::write_block_inputs,
129        base::{block::Block, input::input_reader::InputReader, link::BaseLink},
130        blocks::control::Pid,
131    };
132
133    #[tokio::test]
134    async fn test_pid_block() {
135        let mut block = Pid::new();
136
137        for _ in write_block_inputs(&mut [
138            (&mut block.interval, (1).into()),
139            (&mut block.sp, (100).into()),
140            (&mut block.kp, (1.0).into()),
141            (&mut block.ki, (1.0).into()),
142            (&mut block.kd, (1.0).into()),
143        ])
144        .await
145        {
146            block.read_inputs().await;
147        }
148
149        block
150            .out
151            .links
152            .push(BaseLink::new(uuid::Uuid::new_v4(), "invalid".to_string()));
153
154        block.execute().await;
155        block.execute().await;
156        block.execute().await;
157        block.execute().await;
158
159        assert_eq!(block.out.value, 100.into());
160    }
161}
logic_mesh/blocks/control/pid.rs

logic_mesh/blocks/control/
pid.rs
