use control_systems_torbox::{
BodePlot, BodePlotData, BodePlotOptions, Continuous, FrequencyResponse,
NyquistPlot, NyquistPlotOptions, Plot, Tf, utils::traits::Mag2Db,
};
fn get_nominal_model() -> Tf<f64, Continuous> {
let j_el = 0.38;
let a1 = 0.167;
let damp1 = 0.025;
let freq1 = 754.0;
let a2 = 0.615;
let damp2 = 0.032;
let freq2 = 1073.0;
let time_delay = 2.5e-3;
let res1 = a1 * Tf::s().powi(2)
/ (Tf::s().powi(2) + 2. * damp1 * freq1 * Tf::s() + freq1.powi(2));
let res2 = a2 * Tf::s().powi(2)
/ (Tf::s().powi(2) + 2. * damp2 * freq2 * Tf::s() + freq2.powi(2));
let mut model = (1.0 / j_el + res1 + res2) * 1.0 / Tf::s();
model *= Tf::pade(time_delay, 3);
model
}
fn get_pid(kd: f64, kp: f64, ki: f64) -> Tf<f64, Continuous> {
Tf::new(&[ki, kp, kd], &[0.0, 0.0, 1.0])
}
fn main() {
println!("Motion Control Example:");
println!(
"Model is a system with two resonaces and a time delay. The time delay is approximated using a pade approximation."
);
let model_tf = get_nominal_model();
println!("Model: \n{model_tf}");
println!("The bode plot looks like this: ");
let mut bodeplot = BodePlot::new(BodePlotOptions::default());
bodeplot.add_system(model_tf.bode(6.0, 500. * 6.0).into());
bodeplot.show(600, 400, "Bodeplot Nominal Model").unwrap();
println!(
"We want to make a position controller for the plant. This can be done using a pid controller."
);
let pid_tf = get_pid(20.0, 20.0 * 30.0, 20.0 * 30.0 * 30.0);
println!("PID: \n{pid_tf}");
println!(
"To analyse stability we can look at the open loop transfer function: "
);
let openloop = pid_tf.clone().series(model_tf.clone());
println!("{openloop}");
println!("We can look at the bode plot: ");
let mut bode_plot = BodePlot::new(BodePlotOptions::default());
bode_plot.add_system(openloop.bode(6.0 * 0.01, 6.0 * 1000.0).into());
bode_plot.show(600, 400, "Bodeplot Open Loop").unwrap();
println!("Or the nyquist plot: ");
let opts = NyquistPlotOptions::default()
.set_x_limits([-3., 1.])
.set_y_limits([-2., 2.]);
let mut nyquist_plot = NyquistPlot::new(opts);
nyquist_plot.add_system(openloop.nyquist(6.0 * 0.01, 6.0 * 1000.0).into());
nyquist_plot
.show(400, 400, "Nyquist Plot Open Loop")
.unwrap();
println!();
println!(
"To get a quantitative measure of the robustness of the system we can look at the peak of the sensitivity function (see Multivariable Feedback Control ch. 2.4.3): "
);
let sensitivity = Tf::new_from_scalar(1.0).feedback(openloop.clone());
println!(
"The peak is in the SISO case equal to the H-infinty norm of the system"
);
let max_sensitivity = sensitivity.to_ss().unwrap().norm_hinf().unwrap();
println!("The peak is: {max_sensitivity}");
println!("This can also be seen from the bodeplot");
let mut bode_plot = BodePlot::new(BodePlotOptions::default());
let mut sens_data: BodePlotData =
sensitivity.bode(6.0 * 1.0, 6.0 * 500.0).into();
sens_data = sens_data.set_name("S");
bode_plot.add_system(sens_data);
let mut max_sens_data: BodePlotData =
Tf::<f64, Continuous>::new_from_scalar(max_sensitivity)
.bode(6.0 * 1.0, 6.0 * 500.0)
.into();
max_sens_data = max_sens_data.set_name("max S");
bode_plot.add_system(max_sens_data);
bode_plot
.show(600, 400, "Bode Plot Sensitivity Function")
.unwrap();
println!();
println!(
"The same can be done with the complementary sensitivity function (tracking function): "
);
let t_func = openloop.clone().feedback(Tf::new_from_scalar(1.0));
let max_t_func = t_func.to_ss().unwrap().norm_hinf().unwrap();
println!("Peak T is: {max_t_func}");
println!("Which can also be plotted as a bode plot");
let mut bode_plot = BodePlot::new(BodePlotOptions::default());
let t_data: BodePlotData = t_func.bode(6.0 * 1.0, 6.0 * 500.0).into();
let t_data = t_data.set_name("T");
bode_plot.add_system(t_data);
let t_max_data: BodePlotData =
Tf::<f64, Continuous>::new_from_scalar(max_t_func)
.bode(6.0 * 1.0, 6.0 * 500.0)
.into();
let t_max_data = t_max_data.set_name("max T");
bode_plot.add_system(t_max_data);
bode_plot.show(600, 400, "Bode Plot T Function").unwrap();
println!();
println!(
"To determine performance we will look at the H2 norm from an input disturbance to error."
);
println!(
"The H2 is equivalent to the L2 norm in timedomain. A good measure for performance of a motion control system is how fast it can compensate for disturbances."
);
println!(
"If we look at the transfer function from u to e and multiply with 1/s to represent step u disturbance and once more multipy with 1/s to go from velocity to position error."
);
let u_step_to_pos_error = model_tf
.clone()
.feedback(pid_tf.clone())
.series(1.0 / Tf::s().powi(2));
let h2_performance = u_step_to_pos_error
.to_ss()
.unwrap()
.minreal(None)
.unwrap()
.norm_h2()
.unwrap();
let hinf_performance = u_step_to_pos_error
.to_ss()
.unwrap()
.minreal(None)
.unwrap()
.norm_hinf()
.unwrap();
println!(
"The Perormance (lower is better) is: {} dB",
h2_performance.mag2db()
);
println!("Hinf db: {}", hinf_performance.mag2db());
let mut bode_plot = BodePlot::new(BodePlotOptions::default());
bode_plot
.add_system(u_step_to_pos_error.bode(6.0 * 1.0, 6.0 * 500.0).into());
bode_plot.add_system(
Tf::<f64, Continuous>::new_from_scalar(hinf_performance)
.bode(6.0 * 1.0, 6.0 * 500.0)
.into(),
);
bode_plot
.show(600, 400, "Bode Plot Disturbance Coupling")
.unwrap();
}