diff --git a/utilities/Cargo.toml b/utilities/Cargo.toml index 0b26319..53b8c3f 100644 --- a/utilities/Cargo.toml +++ b/utilities/Cargo.toml @@ -6,4 +6,7 @@ edition = "2024" [dependencies] serde = { workspace = true } eframe = { workspace = true } -egui_plot = { workspace = true } \ No newline at end of file +egui_plot = { workspace = true } +nexosim = "1.0.0" +rand_xoshiro = { version = "0.7.0", features = ["serde"] } +rand_distr = "0.5.1" diff --git a/utilities/examples/servo.rs b/utilities/examples/servo.rs new file mode 100644 index 0000000..1c2c838 --- /dev/null +++ b/utilities/examples/servo.rs @@ -0,0 +1,569 @@ +//! Example: Servo motor +//! +//! This example demonstrates in particular: +//! +//! * closed loop control +//! * periodic scheduling +//! * model prototypes and submodels +//! * adding randomness to models +//! +//! This example displays plot showing the motor position following the setpoint +//! changes. +//! +//! ```text +//! ┌─────────────────────────────────────────────────────────────────┐ +//! │ServoAssembly │ +//! │ ┌────────────┐ Voltage │ +//! │ │ │◄────────────────────────────────────────────┐ │ +//! │ │ Servo │ ┌───────────────┐│ │ +//! degrees │ │ Controller │ Voltage ┌─────────┐ ┌►│ Potentiometer ├┘ │ +//! Setpoint ●───────────┼──►│ ├─────────────►│ │ │ └───────────────┘ │ +//! (0:180) │ │ │ │ DC │ │ position │ +//! │ └────────────┘ │ Motor ├─┴─────────────────────┼──────────► +//! torque, inertia │ │ │ (0:180) │ +//! Load ●────────────────┼──────────────────────────────►│ │ │ +//! │ └─────────┘ │ +//! └─────────────────────────────────────────────────────────────────┘ +//! ``` + +use std::iter; +use std::time::Duration; + +use serde::{Deserialize, Serialize}; + +use nexosim::model::{BuildContext, Context, Model, ProtoModel, schedulable}; +use nexosim::ports::{EventSinkReader, EventSource, Output, SinkState, event_queue}; +use nexosim::simulation::{Mailbox, SimInit}; +use nexosim::time::MonotonicTime; + +use rand_distr::{Distribution, Normal}; +use rand_xoshiro::Xoshiro256PlusPlus; +use rand_xoshiro::rand_core::SeedableRng; +use utilities::{pid::PidController, plot}; + +use std::f64::consts::PI; + +/// Direct current motor. +#[derive(Serialize, Deserialize)] +pub struct DCMotor { + /// Position [degrees] -- output port. + pub position: Output, + + /// Position [degrees] -- internal state. + pos: f64, + /// Previous velocity [rad/s] -- internal state. + prev_vel: f64, + /// Previous acceleration [rad/s^2] -- internal state. + prev_acc: f64, + /// Time of last position update -- internal state. + last_position_update: Option, + /// Load on the motor -- internal state. + load: Load, + /// Torque generated by the motor with maximal voltage -- constant. + max_torque: f64, + /// Voltage with which motor has maximal torque [V] -- constant. + nominal_voltage: f64, +} + +#[Model] +impl DCMotor { + /// Create a new DC motor. + pub fn new( + max_torque: f64, + nominal_voltage: f64, + initial_load: Load, + initial_position: f64, + ) -> Self { + Self { + position: Default::default(), + pos: initial_position, + prev_vel: 0.0, + prev_acc: 0.0, + last_position_update: None, + load: initial_load, + max_torque, + nominal_voltage, + } + } + + /// Broadcasts the initial position of the motor. + #[nexosim(init)] + async fn init(&mut self) { + self.position.send(self.pos).await; + } + + /// Motor voltage supplied from controller [V]. + /// + /// Calculates motors position based on simplified model in which motor + /// torque is proportional to voltage. + pub async fn voltage_in(&mut self, voltage: f64, cx: &Context) { + let now = cx.time(); + if let Some(prev_time) = self.last_position_update { + let elapsed_time = now.duration_since(prev_time).as_secs_f64(); + // Current velocity calculated based on acceleration during last period. + let vel = self.prev_vel + self.prev_acc * elapsed_time; + // Integration of velocity over time to get position change, converted to + // degrees. + self.pos += (self.prev_vel + vel) / 2.0 * elapsed_time / PI * 180.0; + // Saves velocity for next iteration. + self.prev_vel = vel; + } + // Saves time for next iteration. + self.last_position_update = Some(now); + // Calculates current acceleration to use at next iteration. + let torque = voltage / self.nominal_voltage * self.max_torque + self.load.torque; + let acceleration = torque / self.load.inertia; + self.prev_acc = acceleration; + // Sends position. + self.position.send(self.pos).await; + } + + /// Load -- input port. + pub fn load_in(&mut self, load: Load) { + self.load = load; + } +} + +/// Represents motor load. +#[derive(Serialize, Deserialize, Clone)] +pub struct Load { + /// Torque applied by the load [N·m]. + /// Positive values push motor towards higher position values and negative + /// towards lower. + pub torque: f64, + /// Rotational inertia of the load [kg·m^2]. + pub inertia: f64, +} + +/// Potentiometer measuring the motor's position. +#[derive(Serialize, Deserialize)] +pub struct Potentiometer { + /// Voltage corresponding to the position of servo [V] -- output port. + pub voltage_out: Output, + + /// Voltage supplied to the servo [V] -- constant. + supply_voltage: f64, + /// Maximal position of the servo [degrees] -- constant. + max_position: f64, + /// Noise standard deviation as fraction of potentiometer range [-] -- + /// constant. + noise_sigma: f64, + /// Random number generator for adding noise. + rng: Xoshiro256PlusPlus, +} + +#[Model] +impl Potentiometer { + /// Creates a new potentiometer. + pub fn new(supply_voltage: f64, max_position: f64, noise_sigma: f64) -> Self { + let rng_seed = 12345; + Self { + voltage_out: Default::default(), + supply_voltage, + max_position, + noise_sigma, + rng: Xoshiro256PlusPlus::seed_from_u64(rng_seed), + } + } + + /// Measures the position of motor and broadcasts output voltage. + /// Adds noise with a configured standard deviation. + /// For output uses middle 80% of voltage for encoding position withing + /// servo's operating limits while the remaining portion allows for control + /// when the motor moves beyond that range. + pub async fn measure_position(&mut self, position: f64) { + let norm_pos = position / self.max_position; + let normal = Normal::new(0.0, self.noise_sigma).unwrap(); + let noise = normal.sample(&mut self.rng); + let norm_pos_noise = norm_pos + noise; + let voltage = (0.1 + 0.8 * norm_pos_noise).clamp(0.0, 1.0) * self.supply_voltage; + self.voltage_out.send(voltage).await; + } +} + +/// Servo motor controller utilizing PID algorithm. +#[derive(Serialize, Deserialize)] +pub struct ServoController { + /// Voltage controlling the motor [V] -- output port. + pub voltage_out: Output, + + /// Position of the motor read from potentiometer [degrees] -- internal + /// state. + pos: f64, + /// Setpoint for regulator [degrees] -- internal state. + setpoint: Option, + /// Voltage supplied to the servo [V] -- constant. + supply_voltage: f64, + /// Maximal position of the servo [degrees] -- constant. + max_position: f64, + /// Period of the control loop [s] -- constant. + period: f64, + /// Implementation of PID controller. + pid: PidController, +} + +#[Model] +impl ServoController { + /// Creates a new servo controller. + pub fn new( + supply_voltage: f64, + max_position: f64, + controller_config: ControllerConfig, + ) -> Self { + assert!(controller_config.period > 0.0); + let pid = PidController::new_with_limits( + controller_config.proportional_gain, + controller_config.integral_gain, + controller_config.derivative_gain, + Some(-1.0), + Some(1.0), + ); + + Self { + voltage_out: Default::default(), + pos: 0.0, + setpoint: None, + supply_voltage, + max_position, + period: controller_config.period, + pid, + } + } + + /// Sets the position based on voltage-encoded position from potentiometer. + pub async fn position_in(&mut self, potentiometer_voltage: f64) { + let potentiometer_voltage = potentiometer_voltage.clamp(0.0, self.supply_voltage); + self.pos = ((potentiometer_voltage / self.supply_voltage) - 0.1) / 0.8 * self.max_position; + } + + /// Sets the setpoint and schedules first iteration if necessary. + pub async fn setpoint_in(&mut self, angle: f64, cx: &Context) { + let is_idle = self.setpoint.is_none(); + self.setpoint = Some(angle); + if is_idle { + let period = Duration::from_secs_f64(self.period); + cx.schedule_periodic_event(period, period, schedulable!(Self::set_output), ()) + .unwrap(); + } + } + + /// Sends voltage. + #[nexosim(schedulable)] + async fn set_output(&mut self, _: ()) { + // Normalized error. + let error = (self.setpoint.unwrap() - self.pos) / self.max_position; + // Calculating control value using PID regulator and denormalizing it. + let voltage = self.pid.update(error, self.period) * self.supply_voltage; + self.voltage_out.send(voltage).await; + } +} + +/// Contains parameters for controller configuration. +pub struct ControllerConfig { + pub period: f64, + pub proportional_gain: f64, + pub integral_gain: f64, + pub derivative_gain: f64, +} + +/// The parent model which submodels are DCMotor, Potentiometer +/// and ServoController. +#[derive(Serialize, Deserialize)] +pub struct ServoAssembly { + /// Private output for submodel connection. + setpoint: Output, + /// Private output for submodel connection. + load: Output, +} + +#[Model] +impl ServoAssembly { + /// The parent model has a module-private constructor. + fn new() -> Self { + Self { + setpoint: Default::default(), + load: Default::default(), + } + } + + /// Setpoint of the motor position [degrees] -- input port. + pub async fn setpoint_in(&mut self, setpoint: f64) { + self.setpoint.send(setpoint).await; + } + + /// Load on the motor -- input port. + pub async fn load_in(&mut self, load: Load) { + self.load.send(load).await; + } +} + +/// Prototype for 'ServoAssembly'. +pub struct ProtoServoAssembly { + pub position: Output, + initial_position: f64, + max_torque: f64, + max_position: f64, + supply_voltage: f64, + noise_sigma: f64, + controller_config: ControllerConfig, + initial_load: Load, +} + +impl ProtoServoAssembly { + /// The prototype has a public constructor. + pub fn new( + initial_position: f64, + max_torque: f64, + max_position: f64, + supply_voltage: f64, + noise_sigma: f64, + controller_config: ControllerConfig, + initial_load: Load, + ) -> Self { + Self { + position: Default::default(), + initial_position, + max_torque, + max_position, + supply_voltage, + noise_sigma, + controller_config, + initial_load, + } + } + // Input methods are in the model itself. +} + +impl ProtoModel for ProtoServoAssembly { + type Model = ServoAssembly; + + fn build(self, cx: &mut BuildContext) -> (ServoAssembly, ()) { + // Creates parent model. + let mut assembly = ServoAssembly::new(); + // Creates child models. + let mut motor = DCMotor::new( + self.max_torque, + self.supply_voltage, // Assumes supply voltage nominal voltage for the motor. + self.initial_load, + self.initial_position, + ); + let mut potentiometer = + Potentiometer::new(self.supply_voltage, self.max_position, self.noise_sigma); + let mut controller = ServoController::new( + self.supply_voltage, + self.max_position, + self.controller_config, + ); + + // Mailboxes + let motor_mbox = Mailbox::new(); + let potentiometer_mbox = Mailbox::new(); + let controller_mbox = Mailbox::new(); + + // Connections + assembly + .setpoint + .connect(ServoController::setpoint_in, &controller_mbox); + assembly.load.connect(DCMotor::load_in, &motor_mbox); + controller + .voltage_out + .connect(DCMotor::voltage_in, &motor_mbox); + potentiometer + .voltage_out + .connect(ServoController::position_in, &controller_mbox); + // Move the prototype's output to the submodel. The `self.position` + // output can be cloned if necessary if several submodels need access to + // it. + motor.position = self.position; + // Internal connection made after move. + motor + .position + .connect(Potentiometer::measure_position, &potentiometer_mbox); + + cx.add_submodel(motor, motor_mbox, "motor"); + cx.add_submodel(potentiometer, potentiometer_mbox, "potentiometer"); + cx.add_submodel(controller, controller_mbox, "controller"); + + (assembly, ()) + } +} + +fn main() -> Result<(), nexosim::simulation::SimulationError> { + // Parameters. + let initial_position: f64 = 0.0; + let period = 0.01; + let max_torque = 0.2; + let max_position = 180.0; + let supply_voltage = 6.0; + let noise_sigma = 0.005; + let initial_load = Load { + torque: 0.0, + // Inertia of 100g disc with 10cm radius. + inertia: 0.0005, + }; + let controller_config = ControllerConfig { + period: 0.01, + proportional_gain: 3.6, + integral_gain: 1.8, + derivative_gain: 0.36, + }; + + // Model + let mut servo = ProtoServoAssembly::new( + initial_position, + max_torque, + max_position, + supply_voltage, + noise_sigma, + controller_config, + initial_load.clone(), + ); + + let servo_mbox = Mailbox::new(); + let servo_addr = servo_mbox.address(); + + // Endpoints + let mut bench = SimInit::new(); + + let setpoint = EventSource::new() + .connect(ServoAssembly::setpoint_in, &servo_addr) + .register(&mut bench); + let motor_load = EventSource::new() + .connect(ServoAssembly::load_in, &servo_addr) + .register(&mut bench); + + let (sink, mut position) = event_queue(SinkState::Enabled); + servo.position.connect_sink(sink); + + // Bench assembly and initialization. + let t0 = MonotonicTime::EPOCH; // arbitrary since models do not depend on absolute time + let mut simu = bench.add_model(servo, servo_mbox, "servo").init(t0)?; + + // ---------- + // Simulation. + // ---------- + + let scheduler = simu.scheduler(); + + // Check initial conditions. + let mut t = t0; + assert_eq!(simu.time(), t); + assert_eq!(position.try_read(), Some(0.0)); + assert!(position.try_read().is_none()); + + let mut setpoint_value = 90.0; + let epsilon = 5.0; + + // Start the servo in 1s with a setpoint of 90 degrees. + scheduler.schedule_event(Duration::from_secs(1), &setpoint, setpoint_value)?; + + // Advance simulation to one second after servo starts. + simu.step_until(Duration::new(2, 0))?; + + // Check if servo is around setpoint 1 second after start. + t += Duration::new(2, 0); + assert_eq!(simu.time(), t); + let mut current_pos = iter::from_fn(|| position.try_read()).last().unwrap(); + assert!((current_pos - setpoint_value).abs() < epsilon); + + // Change the setpoint to 150 degrees. + setpoint_value = 150.0; + simu.process_event(&setpoint, setpoint_value)?; + + // Advance the simulation time to the moment immediately after the change + // and check whether the servo has not yet reached the setpoint. + simu.step_until(Duration::new(0, 50_000_000))?; + t += Duration::new(0, 50_000_000); + assert_eq!(simu.time(), t); + current_pos = iter::from_fn(|| position.try_read()).last().unwrap(); + assert!(current_pos < setpoint_value - epsilon); + + // Advance simulation time and check if servo is around setpoint 1 second after + // setpoint change. + simu.step_until(Duration::new(0, 950_000_000))?; + t += Duration::new(0, 950_000_000); + assert_eq!(simu.time(), t); + current_pos = iter::from_fn(|| position.try_read()).last().unwrap(); + assert!((current_pos - setpoint_value).abs() < epsilon); + + // Apply torque on the motor. + let load = Load { + torque: -0.05, + inertia: 0.0005, + }; + simu.process_event(&motor_load, load)?; + + // Advance the simulation time and check whether the position changed due to + // the load applied. + simu.step_until(Duration::new(0, 200_000_000))?; + t += Duration::new(0, 200_000_000); + assert_eq!(simu.time(), t); + current_pos = iter::from_fn(|| position.try_read()).last().unwrap(); + assert!(current_pos < setpoint_value - epsilon); + + // Advance the simulation time and check whether the position stabilized + // around setpoint 3 seconds after the load were applied. + simu.step_until(Duration::new(2, 800_000_000))?; + t += Duration::new(2, 800_000_000); + assert_eq!(simu.time(), t); + current_pos = iter::from_fn(|| position.try_read()).last().unwrap(); + assert!((current_pos - setpoint_value).abs() < epsilon); + + // Plot creation. + // Plot shows several setpoint changes and their impact on the servo position. + + // Zero torque. + simu.process_event(&motor_load, initial_load)?; + + // Advance simulation time to stabilize the servo without collecting position + // values. + position.disable(); + simu.step_until(Duration::new(10, 0))?; + position.enable(); + + // Setpoints for the trajectory, vector: [(time, value)]. + // First value is current setpoint. + let trajectory: Vec<(u64, f64)> = vec![(0, 150.0), (2, 60.0), (6, 0.0), (8, 90.0)]; + + // Scheduling setpoint changes. + for (time, value) in &trajectory[1..] { + scheduler.schedule_event(Duration::from_secs(*time), &setpoint, *value)?; + } + + // Length of the simulation for the plot [s]. + let sim_len = 14; + + // Simulating + simu.step_until(Duration::new(sim_len, 0))?; + + // Formatting the setpoint vector for the plot drawing function. + let mut trajectory_for_drawing: Vec<[f64; 2]> = trajectory + .windows(2) + .flat_map(|pair| { + let (t0, val0) = pair[0]; + let (t1, _val1) = pair[1]; + [[t0 as f64, val0], [t1 as f64, val0]] + }) + .collect(); + + if let Some(&(last_t, last_val)) = trajectory.last() { + trajectory_for_drawing.push([last_t as f64, last_val]); + trajectory_for_drawing.push([sim_len as f64, last_val]); + } + + // Collecting all positions to a vector. + let positions: Vec = iter::from_fn(|| position.try_read()).collect(); + + // Formatting the positions vector for the plot drawing function. + let positions_with_time: Vec<[f64; 2]> = positions + .iter() + .enumerate() + .map(|(id, pos)| [id as f64 * period, *pos]) + .collect(); + + plot::plot_series(&[ + ("position", &positions_with_time), + ("setpoint", &trajectory_for_drawing), + ]); + Ok(()) +}