second motor control

Chapter03/src/lib.rs

@@ -1,4 +1,5 @@
 mod physics;
+mod motor;
 
 extern crate floating_duration;
 use std::time::Instant;

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

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<u64>;