Skip to main content

second_order_position/
second_order_position.rs

1use ladrc_no_std::{LadrcSecondOrder, LadrcSecondOrderConfig, OutputLimit};
2
3fn main() -> Result<(), ladrc_no_std::ConfigError> {
4    let dt = 0.001;
5    let target_position = 1.0;
6
7    let config = LadrcSecondOrderConfig::from_bandwidth(
8        dt, 2.0, // one unit of command gives roughly +2 position-units/s^2
9        12.0, 60.0,
10    )
11    .with_output_limit(OutputLimit::new(-5.0, 5.0));
12
13    let mut controller = LadrcSecondOrder::new(config)?;
14    let mut position = 0.0;
15    let mut velocity = 0.0;
16    let mut control = 0.0;
17
18    for step in 0..5_000 {
19        let out = controller.update(target_position, position);
20        control = out.control;
21
22        let load_disturbance = if step > 2_000 { -0.8 } else { 0.0 };
23        let acceleration = -1.2 * velocity - 4.0 * position + 2.0 * control + load_disturbance;
24
25        velocity += dt * acceleration;
26        position += dt * velocity;
27    }
28
29    println!("target position: {target_position:.3}");
30    println!("final position:  {position:.3}");
31    println!("final velocity:  {velocity:.3}");
32    println!("final command:   {control:.3}");
33
34    Ok(())
35}