diff --git a/Chapter03/src/lib.rs b/Chapter03/src/lib.rs index 4ec5f97..db023fe 100644 --- a/Chapter03/src/lib.rs +++ b/Chapter03/src/lib.rs @@ -1,4 +1,5 @@ mod physics; +mod motor; extern crate floating_duration; use std::time::Instant; diff --git a/Chapter03/src/motor.rs b/Chapter03/src/motor.rs new file mode 100644 index 0000000..224bdd4 --- /dev/null +++ b/Chapter03/src/motor.rs @@ -0,0 +1,77 @@ +extern crate floating_duration; +use std::time::Instant; +use floating_duration::{TimeAsFloat, TimeFormat}; +use physics::{ElevatorSpecification, ElevatorState, MotorInput, MotorController}; + +pub struct SimpleMotorController +{ + esp: ElevatorSpecification +} + +impl MotorController for SimpleMotorController +{ + fn init(&mut self, esp: ElevatorSpecification, est: ElevatorState) + { + self.esp = esp; + } + + fn poll(&mut self, est: ElevatorState, dst: u64) -> MotorInput + { + //5.3. Adjust motor control to process next floor request + + //it will take t seconds to decelerate from velocity v at -1 m/s^2 + let t = est.velocity.abs() / 1.0; + + //during which time, the carriage will travel d=t * v/2 meters + //at an average velocity of v/2 before stopping + let d = t * (est.velocity/2.0); + + //l = distance to next floor + let l = (est.location - (dst as f64)*self.esp.floor_height).abs(); + + let target_acceleration = { + //are we going up? + let going_up = est.location < (dst as f64)*self.esp.floor_height; + + //Do not exceed maximum velocity + if est.velocity.abs() >= 5.0 { + if going_up==(est.velocity>0.0) { + 0.0 + //decelerate if going in wrong direction + } else if going_up { + 1.0 + } else { + -1.0 + } + + //if within comfortable deceleration range and moving in right direction, decelerate + } else if l < d && going_up==(est.velocity>0.0) { + if going_up { + -1.0 + } else { + 1.0 + } + + //else if not at peak velocity, accelerate + } else { + if going_up { + 1.0 + } else { + -1.0 + } + } + }; + + let gravity_adjusted_acceleration = target_acceleration + 9.8; + let target_force = gravity_adjusted_acceleration * self.esp.carriage_weight; + let target_voltage = target_force / 8.0; + if target_voltage > 0.0 { + MotorInput::Up { voltage: target_voltage } + } else { + MotorInput::Down { voltage: target_voltage.abs() } + } + } +} + +q +q diff --git a/Chapter03/src/physics.rs b/Chapter03/src/physics.rs index 23b34fd..d80bd41 100644 --- a/Chapter03/src/physics.rs +++ b/Chapter03/src/physics.rs @@ -13,19 +13,19 @@ pub enum MotorInput #[derive(Clone)] pub struct ElevatorSpecification { - floor_count: u64, - floor_height: f64, - carriage_weight: f64 + pub floor_count: u64, + pub floor_height: f64, + pub carriage_weight: f64 } #[derive(Clone)] pub struct ElevatorState { - timestamp: Instant, - location: f64, - velocity: f64, - acceleration: f64, - motor_input: MotorInput + pub timestamp: Instant, + pub location: f64, + pub velocity: f64, + pub acceleration: f64, + pub motor_input: MotorInput } pub type FloorRequests = Vec;