You cannot select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
72 lines
2.2 KiB
Rust
72 lines
2.2 KiB
Rust
use buildings::{Building,getCumulativeFloorHeight};
|
|
use trip_planning::{RequestQueue};
|
|
use motion_controllers::{MotionController};
|
|
use data_recorders::{DataRecorder};
|
|
use std::time::Instant;
|
|
use floating_duration::{TimeAsFloat, TimeFormat};
|
|
use std::{thread, time};
|
|
|
|
#[derive(Clone,Debug,Serialize,Deserialize)]
|
|
pub struct ElevatorState {
|
|
pub timestamp: f64,
|
|
pub location: f64,
|
|
pub velocity: f64,
|
|
pub acceleration: f64,
|
|
pub motor_input: f64
|
|
}
|
|
|
|
pub const MAX_JERK: f64 = 20.0;
|
|
pub const MAX_ACCELERATION: f64 = 2.0;
|
|
pub const MAX_VELOCITY: f64 = 5.0;
|
|
|
|
pub fn simulate_elevator(esp: Box<Building>, est: ElevatorState, floor_requests: &mut Box<RequestQueue>,
|
|
mc: &mut Box<MotionController>, dr: &mut Box<DataRecorder>)
|
|
{
|
|
//immutable input becomes mutable local state
|
|
let mut esp = esp.clone();
|
|
let mut est = est.clone();
|
|
|
|
//initialize MotorController and DataController
|
|
mc.init(esp.clone(), est.clone());
|
|
dr.init(esp.clone(), est.clone());
|
|
|
|
//5. Loop while there are remaining floor requests
|
|
let original_ts = Instant::now();
|
|
thread::sleep(time::Duration::from_millis(1));
|
|
let mut next_floor = floor_requests.pop_request();
|
|
while let Some(dst) = next_floor
|
|
{
|
|
//5.1. Update location, velocity, and acceleration
|
|
let now = Instant::now();
|
|
let ts = now.duration_since(original_ts)
|
|
.as_fractional_secs();
|
|
let dt = ts - est.timestamp;
|
|
est.timestamp = ts;
|
|
|
|
est.location = est.location + est.velocity * dt;
|
|
est.velocity = est.velocity + est.acceleration * dt;
|
|
est.acceleration = {
|
|
let F = est.motor_input;
|
|
let m = esp.get_carriage_weight();
|
|
-9.8 + F/m
|
|
};
|
|
|
|
//5.2. If next floor request in queue is satisfied, then remove from queue
|
|
if (est.location - getCumulativeFloorHeight(esp.get_floor_heights(), dst)).abs() < 0.01 &&
|
|
est.velocity.abs() < 0.01
|
|
{
|
|
est.velocity = 0.0;
|
|
next_floor = floor_requests.pop_request();
|
|
}
|
|
|
|
//5.4. Print realtime statistics
|
|
dr.record(est.clone(), dst);
|
|
|
|
//5.3. Adjust motor control to process next floor request
|
|
est.motor_input = mc.adjust(&est, dst);
|
|
|
|
thread::sleep(time::Duration::from_millis(1));
|
|
}
|
|
|
|
}
|