From 633666a07a1cbcd791cf423219b4792bb86abbfd Mon Sep 17 00:00:00 2001
From: Andrew Johnson <andrewjohnson@Andrews-MacBook-Pro.local>
Date: Sat, 7 Apr 2018 19:24:24 -0600
Subject: [PATCH] first working attempt at smooth motor control

---
 Chapter03/src/lib.rs   |  4 ++--
 Chapter03/src/motor.rs | 52 +++++++++++++++++++-----------------------
 2 files changed, 26 insertions(+), 30 deletions(-)

diff --git a/Chapter03/src/lib.rs b/Chapter03/src/lib.rs
index fb45ff6..a63197a 100644
--- a/Chapter03/src/lib.rs
+++ b/Chapter03/src/lib.rs
@@ -222,15 +222,15 @@ pub fn run_simulation()
       record_acceleration: Vec::new(),
       record_voltage: Vec::new()
    };
+   /*
    let mut mc = SimpleMotorController {
       esp: esp.clone()
    };
-   /*
+   */
    let mut mc = SmoothMotorController {
       timestamp: 0.0,
       esp: esp.clone()
    };
-   */
 
    simulate_elevator(esp, est, floor_requests, &mut mc, &mut dr);
    dr.summary();
diff --git a/Chapter03/src/motor.rs b/Chapter03/src/motor.rs
index aacd2a1..c7667a5 100644
--- a/Chapter03/src/motor.rs
+++ b/Chapter03/src/motor.rs
@@ -100,27 +100,17 @@ impl MotorController for SmoothMotorController
       let t_veloc = MAX_VELOCITY / MAX_ACCELERATION;
 
       //it may take up to d meters to decelerate from current
-      let d_accel = est.velocity.abs() * (est.acceleration.abs() / MAX_JERK);
-      let d_veloc = {
-         //excess acceleration
-         let excess_t;
-         let excess_d;
-         if (est.acceleration<0.0 && est.velocity<0.0) ||
-            (est.acceleration>0.0 && est.velocity>0.0) {
-            excess_t = est.acceleration.abs() / MAX_JERK;
-            excess_d = est.velocity.abs() * excess_t;
-         } else {
-            excess_t = 0.0;
-            excess_d = 0.0;
-         }
-
-         //ramping jerk down
-         let ramp_t = est.velocity.abs() / (t_accel + est.velocity.abs() / MAX_ACCELERATION);
-         let ramp_d = est.velocity.abs() * ramp_t;
-
-         excess_d + ramp_d
+      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 = d_accel + d_veloc;
+      let d = est.velocity.abs() * decel_t;
 
       //l = distance to next floor
       let l = (est.location - (dst as f64)*self.esp.floor_height).abs();
@@ -133,19 +123,25 @@ impl MotorController for SmoothMotorController
          let dt = est.timestamp - self.timestamp;
          self.timestamp = est.timestamp;
 
-         //Do not exceed maximum velocity
-         if est.velocity.abs() >= MAX_VELOCITY {
-            if going_up==(est.velocity>0.0) {
-               0.0
-            //decelerate if going in wrong direction
-            } else if going_up {
-               est.acceleration + (dt * MAX_JERK)
+         //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 && going_up==(est.velocity>0.0) {
+         } else if l < d && (est.velocity>0.0) == going_up {
             if going_up {
                est.acceleration - (dt * MAX_JERK)
             } else {