smooth motor controller

Chapter05/src/buildings.rs

@@ -1,5 +1,5 @@
 use elevator_drivers::{ElevatorDriver, ElevatorDriver1, ElevatorDriver2, ElevatorDriver3};
-use motor_controllers::{MotorInput, MotorController, newMotorController1, newMotorController2, newMotorController3};
+use motor_controllers::{MotorController, newMotorController1, newMotorController2, newMotorController3};
 
 pub trait Building
 {
@@ -7,6 +7,7 @@ pub trait Building
    fn get_motor_controller(&self) -> Box<MotorController>;
    fn get_floor_heights(&self) -> Vec<f64>;
    fn get_carriage_weight(&self) -> f64;
+   fn clone(&self) -> Box<Building>;
 }
 
 pub struct Building1;
@@ -27,6 +28,9 @@ impl Building for Building1 {
    {
       1200.0
    }
+   fn clone(&self) -> Box<Building> {
+      Box::new(Building1)
+   }
 }
 
 pub struct Building2;
@@ -47,6 +51,9 @@ impl Building for Building2 {
    {
       1350.0
    }
+   fn clone(&self) -> Box<Building> {
+      Box::new(Building2)
+   }
 }
 
 pub struct Building3;
@@ -67,4 +74,7 @@ impl Building for Building3 {
    {
       1400.0
    }
+   fn clone(&self) -> Box<Building> {
+      Box::new(Building3)
+   }
 }

Chapter05/src/data_recorders.rs

@@ -8,6 +8,13 @@ pub trait DataRecorder
    fn summary(&mut self);
 }
 
+pub struct SimpleDataRecorder;
+
+pub fn newSimpleDataRecorder() -> SimpleDataRecorder
+{
+   SimpleDataRecorder
+}
+
 /*
 use physics::{ElevatorSpecification, ElevatorState, MotorInput, SimpleMotorInput, simulate_elevator, DataRecorder, MotorController, MotorVoltage,
               ElevatorStateClone, ElevatorSpecificationClone, Motor, SimpleMotor};

Chapter05/src/lib.rs

@@ -8,4 +8,5 @@ pub mod elevator_drivers;
 pub mod buildings;
 pub mod physics;
 pub mod trip_planning;
-pub mod data_recorder;
+pub mod data_recorders;
+pub mod motion_controllers;

Chapter05/src/motion_controllers.rs

@@ -0,0 +1,118 @@
+use physics::{ElevatorState};
+use buildings::{Building};
+
+pub trait MotionController
+{
+   fn init(&mut self, esp: Box<Building>, est: ElevatorState);
+   fn poll(&mut self, est: ElevatorState, dst: u64) -> f64;
+}
+
+pub struct SmoothMotionController
+{
+   pub esp: Box<Building>,
+   pub timestamp: f64
+}
+
+fn getCumulativeFloorHeight(heights: Vec<f64>, floor: u64) -> f64
+{
+   let mut c = 0.0;
+   for fi in 0..floor
+   {
+      c += heights[fi as usize];
+   };
+   c
+}
+
+impl MotionController for SmoothMotionController
+{
+   fn init(&mut self, esp: Box<Building>, est: ElevatorState)
+   {
+      self.esp = esp;
+      self.timestamp = est.timestamp;
+   }
+
+   fn poll(&mut self, est: ElevatorState, dst: u64) -> f64
+   {
+      //5.3. Adjust motor control to process next floor request
+
+      let MAX_JERK = 0.2;
+      let MAX_ACCELERATION = 2.0;
+      let MAX_VELOCITY = 5.0;
+
+      //it will take t seconds to reach max from max
+      let t_accel = MAX_ACCELERATION / MAX_JERK;
+      let t_veloc = MAX_VELOCITY / MAX_ACCELERATION;
+
+      //it may take up to d meters to decelerate from current
+      let decel_t = if (est.velocity>0.0) == (est.acceleration>0.0) {
+         //this case deliberately overestimates d to prevent "back up"
+         (est.acceleration.abs() / MAX_JERK) +
+         (est.velocity.abs() / (MAX_ACCELERATION / 2.0)) +
+         2.0 * (MAX_ACCELERATION / MAX_JERK)
+      } else {
+         //without the MAX_JERK, this approaches infinity and decelerates way too soon
+         //MAX_JERK * 1s = acceleration in m/s^2
+         est.velocity.abs() / (MAX_JERK + est.acceleration.abs())
+      };
+      let d = est.velocity.abs() * decel_t;
+
+      let dst_height = getCumulativeFloorHeight(self.esp.get_floor_heights(), dst);
+
+      //l = distance to next floor
+      let l = (est.location - dst_height).abs();
+
+
+      let target_acceleration = {
+         //are we going up?
+         let going_up = est.location < dst_height;
+
+         //time elapsed since last poll
+         let dt = est.timestamp - self.timestamp;
+         self.timestamp = est.timestamp;
+
+         //Do not exceed maximum acceleration
+         if est.acceleration.abs() >= MAX_ACCELERATION {
+            if est.acceleration > 0.0 {
+               est.acceleration - (dt * MAX_JERK)
+            } else {
+               est.acceleration + (dt * MAX_JERK)
+            }
+
+         //Do not exceed maximum velocity
+         } else if est.velocity.abs() >= MAX_VELOCITY
+            || (est.velocity + est.acceleration * (est.acceleration.abs() / MAX_JERK)).abs() >= MAX_VELOCITY {
+            if est.velocity > 0.0 {
+               est.acceleration - (dt * MAX_JERK)
+            } else {
+               est.acceleration + (dt * MAX_JERK)
+            }
+
+         //if within comfortable deceleration range and moving in right direction, decelerate
+         } else if l < d && (est.velocity>0.0) == going_up {
+            if going_up {
+               est.acceleration - (dt * MAX_JERK)
+            } else {
+               est.acceleration + (dt * MAX_JERK)
+            }
+
+         //else if not at peak velocity, accelerate smoothly
+         } else {
+            if going_up {
+               est.acceleration + (dt * MAX_JERK)
+            } else {
+               est.acceleration - (dt * MAX_JERK)
+            }
+         }
+      };
+
+     let gravity_adjusted_acceleration = target_acceleration + 9.8;
+     let target_force = gravity_adjusted_acceleration * self.esp.get_carriage_weight();
+     if !target_force.is_finite() {
+         //divide by zero etc.
+         //may happen if time delta underflows
+         0.0
+     } else {
+         0.0
+     }
+   }
+}

Chapter05/src/motor_controllers.rs

@@ -1,5 +1,8 @@
 use libc::c_double;
 
+use buildings::{Building};
+use physics::{ElevatorState};
+
 #[link(name = "motor1")]
 extern {
    pub fn motor1_adjust_motor(target_force: c_double) -> c_double;
@@ -144,3 +147,4 @@ impl MotorController for MotorController3
       90000.0
    }
 }
+

Chapter05/src/simulate_trip.rs

@@ -4,6 +4,8 @@ extern crate floating_duration;
 use elevator::buildings::{Building, Building1, Building2, Building3};
 use elevator::trip_planning::{FloorRequests, RequestQueue};
 use elevator::physics::{ElevatorState};
+use elevator::motion_controllers::{SmoothMotionController};
+use elevator::data_recorders::{newSimpleDataRecorder};
 
 use std::time::Instant;
 use std::env;
@@ -106,29 +108,13 @@ pub fn run_simulation()
       }
    }
 
-   /*
-   let termsize = termion::terminal_size().ok();
-   let mut dr = SimpleDataRecorder {
-      esp: esp.clone(),
-      termwidth: termsize.map(|(w,_)| w-2).expect("termwidth") as u64,
-      termheight: termsize.map(|(_,h)| h-2).expect("termheight") as u64,
-      stdout: &mut io::stdout().into_raw_mode().unwrap(),
-      log: File::create("simulation.log").expect("log file"),
-      record_location: Vec::new(),
-      record_velocity: Vec::new(),
-      record_acceleration: Vec::new(),
-      record_voltage: Vec::new()
-   };
-   /*
-   let mut mc = SimpleMotorController {
-      esp: esp.clone()
-   };
-   */
-   let mut mc = SmoothMotorController {
+   let mut dr = newSimpleDataRecorder();
+   let mut mc = SmoothMotionController {
       timestamp: 0.0,
       esp: esp.clone()
    };
 
+   /*
    simulate_elevator(esp, est, floor_requests, &mut mc, &mut dr);
    dr.summary();
    */