diff --git a/README.md b/README.md index bc87e00..b39eec5 100644 --- a/README.md +++ b/README.md @@ -26,7 +26,7 @@ control-systems-torbox = "*" The crate contains a representation for transfer functions: -```rust,no_run +```rust use control_systems_torbox::*; let tf_lp = Tf::::new(&[10.0], &[10.0, 1.0]); // 10/(s + 10) @@ -34,22 +34,56 @@ let sys = 1.0 / Tf::s(); let pi_c = 1.0 + 1.0 / Tf::s(); let openloop = sys * pi_c * tf_lp; -let tracking = openloop.clone() / (1.0 + openloop); +let tracking = openloop.feedback(&Tf::new_from_scalar(1.0)); ``` State-Space representations is also implemented and you can convert between transfer function and state-space -```rust,no_run +```rust use control_systems_torbox::*; -use nalgebra::DMatrix; +use nalgebra::dmatrix; -let a = DMatrix::from_row_slice(2, 2, &[0., 0., 1., 0.]); -let b = DMatrix::from_row_slice(2, 1, &[1., 0.]); -let c = DMatrix::from_row_slice(1, 2, &[0., 1.]); -let d = DMatrix::from_row_slice(1, 1, &[0.]); +let a = dmatrix![0., 1.; + 0., 0.]; +let b = dmatrix![0.; + 1.]; +let c = dmatrix![1., 0.]; +let d = dmatrix![0.]; let sys_ss = Ss::::new(a, b, c, d).unwrap(); -let sys_tf = ss2tf(&sys_ss).unwrap(); +let sys_tf = sys_ss.to_tf().unwrap(); +``` + +System norms such as H2-norm and H-inf-norm are also possible to calculte + +```rust +use control_systems_torbox::*; + +let low_pass = 1.0/(Tf::s().powi(2) + 2.0*0.2*Tf::s() + 0.1); +let low_pass = low_pass.to_ss().unwrap(); +let h2_norm = low_pass.norm_h2().unwrap(); +let hinf_norm = low_pass.norm_hinf().unwrap(); +println!("H2 norm is: {}, H-inf norm is: {}", h2_norm, hinf_norm); +``` + +It is also possible to calculte the poles and zeros of a system + +```rust +use control_systems_torbox::*; + +let tf = (Tf::s() -1.0)*(Tf::s() + 3.0)/((Tf::s() - 4.0)*(Tf::s() + 5.0)); +let ss = tf.to_ss().unwrap(); +let poles = ss.poles(); +let zeros = ss.zeros().unwrap(); +println!("Tf:\n{}", tf); +println!("Poles:"); +for pole in poles { + println!("{}", pole); +} +println!("\nZeros:"); +for zero in zeros { + println!("{}", zero); +} ``` Both Bodeplot and Nyquistplot is implemented and can be displayed natively: @@ -61,10 +95,10 @@ let sys = (1.0/Tf::s()) * (1.0 + 1.0/Tf::s()); let sys_clp = sys.clone() / (1.0 + sys.clone()); let mut bode_plot = BodePlot::new(BodePlotOptions::default()); -bode_plot.add_system(BodePlotData::new(bode(sys_clp, 0.1, 10.))); +bode_plot.add_system(BodePlotData::new(sys_clp.bode(0.1, 10.))); bode_plot.show(800, 600, "Bode plot demo").unwrap(); let mut nyq_plot = NyquistPlot::new(NyquistPlotOptions::default()); -nyq_plot.add_system(NyquistPlotData::new(nyquist(sys, 0.1, 100.))); +nyq_plot.add_system(NyquistPlotData::new(sys.nyquist(0.1, 100.))); nyq_plot.show(600, 600, "Nyquist plot demo").unwrap(); -``` +``` \ No newline at end of file diff --git a/examples/motion_controller.rs b/examples/motion_controller.rs new file mode 100644 index 0000000..ddd056e --- /dev/null +++ b/examples/motion_controller.rs @@ -0,0 +1,93 @@ +use control_systems_torbox::{ + BodePlot, BodePlotOptions, Continuous, FrequencyResponse, NyquistPlot, + NyquistPlotOptions, Plot, Ss, Tf, analysis::frequency_response::lin_space, +}; + +fn main() { + // This example shows how we can create a plant, design a controller for the + // plant and analyse the resulting closed loop system. + + // Plant is integrator, 1/s, and a power converter with a bandwith of 10 + // rad/s, and a time delay of 0.5s approximated with a second order pade + // approximation + let s = Tf::s(); + let plant = 1.0 / &s * 50.0 / (&s + 50.0) * Tf::pade(0.12, 3); + // let plant = plant.to_ss().unwrap(); + + // We will use a PI controller: Kp*(1 + 1/T_i*1/s) + // We set the integration time to 1 second and find the largest Kp such that + // Ms = ||S(s)||_inf = peak of sensitity function is less than two. + let ms_limit = 2.0; + let t_i = 10.0; + let kps = lin_space(100.0, 0.001, 100); // Kp to test + let mut controller = None; + for kp in kps { + let controller_i = kp * (1.0 + 1.0 / t_i * 1.0 / &s); //.to_ss().unwrap(); + let open_loop = controller_i.series(&plant); + // S = 1/(1 + L) + let sensitivity_func = + Ss::new_from_scalar(1.0).feedback(&open_loop.to_ss().unwrap()); + if !sensitivity_func.is_stable() { + continue; + } + + let max_sensitivity = sensitivity_func.norm_hinf().unwrap(); + + // println!("Ms = {}", max_sensitivity); + // println!("Stable: {}", sensitivity_func.is_stable()); + let mut bode_plot = BodePlot::new(BodePlotOptions::default()); + bode_plot.add_system(sensitivity_func.bode(0.1, 100.).into()); + bode_plot.add_system( + Tf::::new_from_scalar(ms_limit) + .bode(0.1, 100.) + .into(), + ); + // uncomment if you want to see the progression of the search + // bode_plot.show(600, 400, "Candidate Sensitivity Function").unwrap(); + if max_sensitivity < ms_limit { + println!("Found Kp = {}, which gives Ms = {}", kp, max_sensitivity); + controller = Some(controller_i); + break; + } + } + let controller = controller.unwrap(); + + // Lets look at the properties of the controller and closed loop system + + println!("Open Loop transfer function"); + let open_loop = controller.series(&plant); + let mut bode_plot = BodePlot::new(BodePlotOptions::default()); + bode_plot.add_system(open_loop.bode(0.1, 100.0).into()); + bode_plot.show(600, 400, "Open Loop").unwrap(); + + println!("Sensitivity Function bode plot"); + let sensitivity_func = Tf::new_from_scalar(1.0).feedback(&open_loop); + bode_plot = BodePlot::new(BodePlotOptions::default()); + bode_plot.add_system(sensitivity_func.bode(0.1, 100.).into()); + bode_plot.add_system( + Tf::::new_from_scalar(ms_limit) + .bode(0.1, 100.) + .into(), + ); + bode_plot.show(600, 400, "Sensitivity Function").unwrap(); + + println!("Nyquist plot of open loop"); + let mut nyq_plot = NyquistPlot::new(NyquistPlotOptions::default()); + nyq_plot.add_system(open_loop.nyquist(0.1, 100.).into()); + nyq_plot.show(400, 400, "Nyquist Open Loop").unwrap(); + + println!("Tracking Function bode plot"); + let tracking_func = open_loop.feedback(&Tf::new_from_scalar(1.0)); + let mut bode_plot = BodePlot::new(BodePlotOptions::default()); + bode_plot.add_system(tracking_func.bode(0.1, 100.).into()); + bode_plot + .show(600, 400, "Tracking Function Bode Plot") + .unwrap(); + + println!("Location of poles: [re, im]"); + let poles = tracking_func.to_ss().unwrap().poles(); + for pole in poles { + print!("[{:.2}, {:.2}] ", pole.re, pole.im); + } + println!(); +} diff --git a/src/analysis/frequency_response.rs b/src/analysis/frequency_response.rs index d765123..5e6b845 100644 --- a/src/analysis/frequency_response.rs +++ b/src/analysis/frequency_response.rs @@ -195,6 +195,11 @@ fn freq_response_ss_mat( assert_eq!(m, 1, "Function is only tested for SISO systems"); assert_eq!(p, 1, "Function is only tested for SISO systems"); + if n == 0 { + let res = DMatrix::from_iterator(p, m, d.iter().map(|&x| c64(x, 0.0))); + return Ok(res); + } + let baleig = CString::new("A").unwrap(); // balance A and compute condition number let inita = CString::new("G").unwrap(); // general A matrix @@ -272,7 +277,7 @@ fn freq_response_ss_mat( #[cfg(test)] mod tests { - use crate::{FrequencyResponse, Tf}; + use crate::{Continuous, FrequencyResponse, Ss, Tf, utils::traits::Mag2Db}; use super::log_space; use approx::assert_abs_diff_eq; @@ -317,4 +322,15 @@ mod tests { assert_abs_diff_eq!(res_tf.im, res_tf.im, epsilon = 1e-3); } } + + #[test] + fn ss_freq_resp_edge_cases() { + let gain = 3.0; + let ss = Ss::::new_from_scalar(gain); + let mag_phase_freq = ss.bode(0.1, 10.); + for [mag, phase, _] in mag_phase_freq { + assert_abs_diff_eq!(mag, gain.mag2db(), epsilon = 1e-6); + assert_abs_diff_eq!(phase, 0.); + } + } } diff --git a/src/lib.rs b/src/lib.rs index 21fa75a..3ddd5fa 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -24,19 +24,21 @@ let sys = 1.0 / Tf::s(); let pi_c = 1.0 + 1.0 / Tf::s(); let openloop = sys * pi_c * tf_lp; -let tracking = openloop.clone() / (1.0 + openloop); +let tracking = openloop.feedback(&Tf::new_from_scalar(1.0)); ``` State-Space representations is also implemented and you can convert between transfer function and state-space ```rust use control_systems_torbox::*; -use nalgebra::DMatrix; - -let a = DMatrix::from_row_slice(2, 2, &[0., 0., 1., 0.]); -let b = DMatrix::from_row_slice(2, 1, &[1., 0.]); -let c = DMatrix::from_row_slice(1, 2, &[0., 1.]); -let d = DMatrix::from_row_slice(1, 1, &[0.]); +use nalgebra::dmatrix; + +let a = dmatrix![0., 1.; + 0., 0.]; +let b = dmatrix![0.; + 1.]; +let c = dmatrix![1., 0.]; +let d = dmatrix![0.]; let sys_ss = Ss::::new(a, b, c, d).unwrap(); let sys_tf = sys_ss.to_tf().unwrap(); diff --git a/src/systems/state_space.rs b/src/systems/state_space.rs index cf9bffe..705bb60 100644 --- a/src/systems/state_space.rs +++ b/src/systems/state_space.rs @@ -394,7 +394,7 @@ impl Ss { /// # Panics /// This function will panic if the input-output dimensions of `self` and /// `sys2` do not match. - pub fn feedback(self, sys2: Self) -> Self { + pub fn feedback(self, sys2: &Self) -> Self { assert_eq!(self.ninputs(), sys2.noutputs()); assert_eq!(self.noutputs(), sys2.ninputs()); self.assert_valid(); @@ -960,12 +960,12 @@ mod tests { let ss1 = (Tf::s() / (Tf::s() + 1.0)).to_ss().unwrap(); let ss2 = (1.0 / Tf::s()).to_ss().unwrap(); - let ss_fb = ss1.clone().feedback(ss2.clone()); + let ss_fb = ss1.clone().feedback(&ss2); let tf_fb = ss_fb.to_tf().unwrap(); assert_abs_diff_eq!(tf_fb, Tf::s() / (Tf::s() + 2.0)); let ss2 = Ss::::new_from_scalar(1.0); - let ss_fb = ss1.clone().feedback(ss2.clone()); + let ss_fb = ss1.clone().feedback(&ss2); let tf_fb = ss_fb.to_tf().unwrap(); assert_abs_diff_eq!(tf_fb, 0.5 * Tf::s() / (Tf::s() + 0.5)); @@ -989,7 +989,7 @@ mod tests { let ss1 = tf1.to_ss_method(ControllableCF).unwrap(); let ss2 = tf2.to_ss_method(ObservableCF).unwrap(); - let ss_fb = ss1.feedback(ss2); + let ss_fb = ss1.feedback(&ss2); let ss_fb = ss_fb.to_tf().unwrap(); let tf_fb = tf1.feedback(&tf2);