rrtk 0.6.1

Rust Robotics ToolKit
Documentation 
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
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179

// SPDX-License-Identifier: BSD-3-Clause
// Copyright 2024 UxuginPython
#[cfg(feature = "alloc")]
use rrtk::streams::converters::*;
#[cfg(feature = "alloc")]
use rrtk::streams::math::*;
#[cfg(feature = "alloc")]
use rrtk::*;
//Note that RRTK includes the rrtk::streams::control::PIDControllerStream type which should be a
//bit faster than this. This example is to show how streams can be chained for more complex data
//processing and control theory. Using the PID controller shown here in production is discouraged.
#[cfg(feature = "alloc")]
struct StreamPID {
    //It is possible to avoid dynamic dispatch by using the actual stream types instead of
    //`dyn Getter`, but doing that fully makes the types look like this:
    //Reference<IntegralStream<DifferenceStream<Quantity, ConstantGetter<Quantity, TimeGetterFromGetter<Quantity, dyn Getter<Quantity, ()>, ()>, ()>, dyn Getter<Quantity, ()>, ()>, ()>>
    //Here, to make this example legible, we don't do that. However, if you're actually making
    //something for production, expanding the type fully to avoid dynamic dispatch may be a good
    //idea. It really depends on how much readability you're willing to give up for a small
    //performance boost.
    int: Reference<dyn Getter<Quantity, ()>>,
    drv: Reference<dyn Getter<Quantity, ()>>,
    pro_float_maker: Reference<dyn Getter<f32, ()>>,
    int_float_maker: Reference<dyn Getter<f32, ()>>,
    drv_float_maker: Reference<dyn Getter<f32, ()>>,
    output: SumStream<f32, 3, ()>,
}
#[cfg(feature = "alloc")]
impl StreamPID {
    pub fn new(
        input: Reference<dyn Getter<Quantity, ()>>,
        setpoint: Quantity,
        kp: Quantity,
        ki: Quantity,
        kd: Quantity,
    ) -> Self {
        let time_getter = rc_ref_cell_reference(TimeGetterFromGetter::new(input.clone()));
        let setpoint = rc_ref_cell_reference(ConstantGetter::new(time_getter.clone(), setpoint));
        let kp = rc_ref_cell_reference(ConstantGetter::new(time_getter.clone(), kp));
        let ki = rc_ref_cell_reference(ConstantGetter::new(time_getter.clone(), ki));
        let kd = rc_ref_cell_reference(ConstantGetter::new(time_getter.clone(), kd));
        let error = rc_ref_cell_reference(DifferenceStream::new(setpoint.clone(), input.clone()));
        let int = rc_ref_cell_reference(IntegralStream::new(error.clone()));
        let drv = rc_ref_cell_reference(DerivativeStream::new(error.clone()));
        //`ProductStream`'s behavior is to treat all `None` values as 1.0 so that it's as if they
        //were not included. However, this is not what we want with the coefficient. `NoneToValue`
        //is used to convert all `None` values to `Some(0.0)` to effectively exlude them from the
        //final sum.
        let int_zeroer = rc_ref_cell_reference(NoneToValue::new(
            int.clone(),
            time_getter.clone(),
            Quantity::new(0.0, MILLIMETER),
        ));
        let drv_zeroer = rc_ref_cell_reference(NoneToValue::new(
            drv.clone(),
            time_getter.clone(),
            Quantity::new(0.0, MILLIMETER),
        ));
        let kp_mul = rc_ref_cell_reference(ProductStream::new([
            to_dyn!(Getter<Quantity, ()>, kp.clone()),
            to_dyn!(Getter<Quantity, ()>, error.clone()),
        ]));
        //The way a PID controller works necessitates that it adds quantities of different units.
        //Thus, QuantityToFloat streams are required to keep the dimensional analysis system from
        //stopping this.
        let pro_float_maker = rc_ref_cell_reference(QuantityToFloat::new(kp_mul));
        let ki_mul = rc_ref_cell_reference(ProductStream::new([
            to_dyn!(Getter<Quantity, ()>, ki.clone()),
            to_dyn!(Getter<Quantity, ()>, int_zeroer.clone()),
        ]));
        let int_float_maker = rc_ref_cell_reference(QuantityToFloat::new(ki_mul));
        let kd_mul = rc_ref_cell_reference(ProductStream::new([
            to_dyn!(Getter<Quantity, ()>, kd.clone()),
            to_dyn!(Getter<Quantity, ()>, drv_zeroer.clone()),
        ]));
        let drv_float_maker = rc_ref_cell_reference(QuantityToFloat::new(kd_mul));
        let output = SumStream::new([
            to_dyn!(Getter<f32, ()>, pro_float_maker.clone()),
            to_dyn!(Getter<f32, ()>, int_float_maker.clone()),
            to_dyn!(Getter<f32, ()>, drv_float_maker.clone()),
        ]);
        Self {
            int: to_dyn!(Getter<Quantity, ()>, int),
            drv: to_dyn!(Getter<Quantity, ()>, drv),
            pro_float_maker: to_dyn!(Getter<f32, ()>, pro_float_maker),
            int_float_maker: to_dyn!(Getter<f32, ()>, int_float_maker),
            drv_float_maker: to_dyn!(Getter<f32, ()>, drv_float_maker),
            output: output,
        }
    }
}
#[cfg(feature = "alloc")]
impl Getter<f32, ()> for StreamPID {
    fn get(&self) -> Output<f32, ()> {
        self.output.get()
    }
}
#[cfg(feature = "alloc")]
impl Updatable<()> for StreamPID {
    fn update(&mut self) -> NothingOrError<()> {
        //The other streams used that are not updated here do not need to be updated. Streams like
        //SumStream just calculate their output in the get method since they do not need to store
        //any data beyond the `Reference`s to their inputs. The non-math streams used here work in
        //a similar way.
        self.int.borrow_mut().update()?;
        self.drv.borrow_mut().update()?;
        self.pro_float_maker.borrow_mut().update()?;
        self.int_float_maker.borrow_mut().update()?;
        self.drv_float_maker.borrow_mut().update()?;
        Ok(())
    }
}
#[cfg(feature = "alloc")]
struct MyStream {
    time: Time,
}
#[cfg(feature = "alloc")]
impl MyStream {
    pub fn new() -> Self {
        Self { time: Time(0) }
    }
}
//In a real system, obviously, the process variable must be dependent on the command. This is a
//very rudimentary placeholder and a poor model of an actual system. All this example is
//intended to do is to show the PID controller's command values and not model a real system by
//assuming a constant velocity.
#[cfg(feature = "alloc")]
impl Getter<Quantity, ()> for MyStream {
    fn get(&self) -> Output<Quantity, ()> {
        Ok(Some(Datum::new(
            self.time,
            Quantity::from(self.time) * Quantity::new(0.5, MILLIMETER_PER_SECOND),
        )))
    }
}
#[cfg(feature = "alloc")]
impl Updatable<()> for MyStream {
    fn update(&mut self) -> NothingOrError<()> {
        self.time += Time(2_000_000_000);
        Ok(())
    }
}
#[cfg(feature = "alloc")]
fn main() {
    const SETPOINT: Quantity = Quantity::new(5.0, MILLIMETER);
    const KP: Quantity = Quantity::dimensionless(1.0);
    const KI: Quantity = Quantity::dimensionless(0.01);
    const KD: Quantity = Quantity::dimensionless(0.1);
    println!("PID Controller using RRTK Streams");
    println!(
        "kp = {:?}; ki = {:?}; kd = {:?}",
        KP.value, KI.value, KD.value
    );
    let input = to_dyn!(Getter<Quantity, ()>, rc_ref_cell_reference(MyStream::new()));
    let mut stream = StreamPID::new(input.clone(), SETPOINT, KP, KI, KD);
    stream.update().unwrap();
    println!(
        "time: {:?}; setpoint: {:?}; process: {:?}; command: {:?}",
        stream.get().unwrap().unwrap().time.0,
        SETPOINT.value,
        input.borrow().get().unwrap().unwrap().value.value,
        stream.get().unwrap().unwrap().value
    );
    for _ in 0..6 {
        input.borrow_mut().update().unwrap();
        stream.update().unwrap();
        println!(
            "time: {:?}; setpoint: {:?}; process: {:?}; command: {:?}",
            stream.get().unwrap().unwrap().time,
            SETPOINT,
            input.borrow().get().unwrap().unwrap().value,
            stream.get().unwrap().unwrap().value
        );
    }
}
#[cfg(not(feature = "alloc"))]
fn main() {
    println!("Enable the `alloc` feature to run this example.\nAssuming you're using Cargo, add the `--features alloc` flag to your command.");
}