step_response/
step_response.rs

1use control_sys::model;
2use control_sys::model::Pole;
3use control_sys::simulator;
4
5use plotters::prelude::*;
6
7use std::borrow::Borrow;
8use std::fs;
9use std::rc::Rc;
10
11extern crate nalgebra as na;
12
13pub mod two_spring_damper_mass {
14    extern crate nalgebra as na;
15
16    use std::default::Default;
17
18    use control_sys::model::DiscreteStateSpaceModel;
19
20    pub struct Parameters {
21        m1: f64,
22        m2: f64,
23        k1: f64,
24        k2: f64,
25        d1: f64,
26        d2: f64,
27    }
28
29    impl Default for Parameters {
30        fn default() -> Parameters {
31            Parameters {
32                m1: 2.0,
33                m2: 2.0,
34                k1: 100.0,
35                k2: 200.0,
36                d1: 1.0,
37                d2: 5.0,
38            }
39        }
40    }
41
42    pub fn build_model(params: Parameters, sampling_dt: f64) -> DiscreteStateSpaceModel {
43        // Define the continuous-time system matrices
44        let mat_ac = na::dmatrix![
45            0.0, 1.0, 0.0, 0.0;
46            -(params.k1 + params.k2) / params.m1, -(params.d1 + params.d2) / params.m1, params.k2 / params.m1, params.d2 / params.m1;
47            0.0, 0.0, 0.0, 1.0;
48            params.k2 / params.m2, params.d2 / params.m2, -params.k2 / params.m2, -params.d2 / params.m2
49        ];
50        let mat_bc = na::dmatrix![0.0; 0.0; 0.0; 1.0 / params.m2];
51        let mat_cc = na::dmatrix![1.0, 0.0, 0.0, 0.0];
52        let mat_dc = na::dmatrix![0.0];
53
54        // Model discretization
55        DiscreteStateSpaceModel::from_continuous_matrix_forward_euler(
56            &mat_ac,
57            &mat_bc,
58            &mat_cc,
59            &mat_dc,
60            sampling_dt,
61        )
62    }
63}
64
65pub mod dc_motor {
66    use control_sys::model::DiscreteStateSpaceModel;
67    use std::default::Default;
68
69    pub struct Parameters {
70        b: f64,
71        j: f64,
72        k: f64,
73        l: f64,
74        r: f64,
75    }
76
77    impl Default for Parameters {
78        fn default() -> Parameters {
79            Parameters {
80                b: 0.1,
81                j: 0.01,
82                k: 0.01,
83                l: 0.5,
84                r: 1.0,
85            }
86        }
87    }
88
89    pub fn build_model(params: Parameters, sampling_dt: f64) -> DiscreteStateSpaceModel {
90        // Define the continuous-time system matrices
91        let mat_ac = na::dmatrix![
92            -params.b / params.j, params.k / params.j;
93            -params.k / params.l, -params.r / params.l;
94        ];
95        let mat_bc = na::dmatrix![0.0; 1.0 / params.l];
96        let mat_cc = na::dmatrix![1.0, 0.0];
97
98        // Model discretization
99        DiscreteStateSpaceModel::from_continuous_matrix_forward_euler(
100            &mat_ac,
101            &mat_bc,
102            &mat_cc,
103            &na::dmatrix![0.0],
104            sampling_dt,
105        )
106    }
107}
108
109fn main() -> Result<(), Box<dyn std::error::Error>> {
110    let sampling_dt = 0.05;
111    let params = dc_motor::Parameters::default();
112    let model = Rc::new(dc_motor::build_model(params, sampling_dt));
113
114    model.poles();
115
116    let (step_response, step, _) = simulator::step_for_discrete_ss(
117        <Rc<model::DiscreteStateSpaceModel> as Borrow<model::DiscreteStateSpaceModel>>::borrow(
118            &model,
119        ),
120        10.0,
121    );
122
123    // Create a folder for results
124    fs::create_dir("img").unwrap_or_else(|_| {
125        println!("The folder img already exists, no need to create it.");
126    });
127
128    // Draw the step response
129    {
130        let root = BitMapBackend::new("img/step_response.png", (800, 600)).into_drawing_area();
131        root.fill(&WHITE)?;
132
133        let max_y = step_response
134            .iter()
135            .cloned()
136            .fold(f64::NEG_INFINITY, f64::max);
137        let min_y = step_response.iter().cloned().fold(f64::INFINITY, f64::min);
138        let mut chart = ChartBuilder::on(&root)
139            .caption("System Output Y", ("sans-serif", 20))
140            .margin(10)
141            .x_label_area_size(30)
142            .y_label_area_size(40)
143            .build_cartesian_2d(0..step_response.len() as i32, min_y..max_y)?;
144
145        chart.configure_mesh().draw()?;
146
147        // Plot input
148        let series_input: Vec<(i32, f64)> = step
149            .row(0)
150            .iter()
151            .enumerate()
152            .map(|(i, &val)| (i as i32, val as f64))
153            .collect();
154
155        chart
156            .draw_series(LineSeries::new(series_input, &Palette99::pick(0)))?
157            .label(format!("Output {}", 0))
158            .legend(move |(x, y)| PathElement::new([(x, y), (x + 20, y)], &Palette99::pick(0)));
159
160        // Plot system response
161        let series_y: Vec<(i32, f64)> = step_response
162            .iter()
163            .enumerate()
164            .map(|(i, &val)| (i as i32, val as f64))
165            .collect();
166
167        chart.draw_series(LineSeries::new(series_y, &Palette99::pick(1)))?;
168
169        chart
170            .configure_series_labels()
171            .background_style(&WHITE)
172            .border_style(&BLACK)
173            .draw()?;
174    }
175
176    Ok(())
177}
step_response/step_response.rs

step_response/
step_response.rs
