Archives/Hands-On-Functional-Programming-in-Rust
2
0
Fork 0
mirror of https://github.com/andrew-johnson-4/Hands-On-Functional-Programming-in-Rust synced 2024-11-18 03:25:33 +00:00
Code Issues Packages Projects Releases Wiki Activity

first working attempt at smooth motor control

Browse Source
This commit is contained in:
Andrew Johnson 2018-04-07 19:24:24 -06:00
parent cdcc3e10cf
commit 633666a07a
2 changed files with 27 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
Chapter03/src/lib.rs
View File

@ -222,15 +222,15 @@ pub fn run_simulation()
record_acceleration: Vec::new(), record_acceleration: Vec::new(),
record_voltage: Vec::new() record_voltage: Vec::new()
}; };
/*
let mut mc = SimpleMotorController { let mut mc = SimpleMotorController {
esp: esp.clone() esp: esp.clone()
}; };
/* */
let mut mc = SmoothMotorController { let mut mc = SmoothMotorController {
timestamp: 0.0, timestamp: 0.0,
esp: esp.clone() esp: esp.clone()
}; };
*/
simulate_elevator(esp, est, floor_requests, &mut mc, &mut dr); simulate_elevator(esp, est, floor_requests, &mut mc, &mut dr);
dr.summary(); dr.summary();

52
Chapter03/src/motor.rs
View File

@ -100,27 +100,17 @@ impl MotorController for SmoothMotorController
let t_veloc = MAX_VELOCITY / MAX_ACCELERATION; let t_veloc = MAX_VELOCITY / MAX_ACCELERATION;
//it may take up to d meters to decelerate from current //it may take up to d meters to decelerate from current
let d_accel = est.velocity.abs() * (est.acceleration.abs() / MAX_JERK); let decel_t = if (est.velocity>0.0) == (est.acceleration>0.0) {
let d_veloc = { //this case deliberately overestimates d to prevent "back up"
//excess acceleration (est.acceleration.abs() / MAX_JERK) +
let excess_t; (est.velocity.abs() / (MAX_ACCELERATION / 2.0)) +
let excess_d; 2.0 * (MAX_ACCELERATION / MAX_JERK)
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 { } else {
excess_t = 0.0; //without the MAX_JERK, this approaches infinity and decelerates way too soon
excess_d = 0.0; //MAX_JERK * 1s = acceleration in m/s^2
} est.velocity.abs() / (MAX_JERK + est.acceleration.abs())
//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 d = d_accel + d_veloc; let d = est.velocity.abs() * decel_t;
//l = distance to next floor //l = distance to next floor
let l = (est.location - (dst as f64)*self.esp.floor_height).abs(); 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; let dt = est.timestamp - self.timestamp;
self.timestamp = est.timestamp; self.timestamp = est.timestamp;
//Do not exceed maximum velocity //Do not exceed maximum acceleration
if est.velocity.abs() >= MAX_VELOCITY { if est.acceleration.abs() >= MAX_ACCELERATION {
if going_up==(est.velocity>0.0) { if est.acceleration > 0.0 {
0.0
//decelerate if going in wrong direction
} else if going_up {
est.acceleration + (dt * MAX_JERK)
} else {
est.acceleration - (dt * MAX_JERK) 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 //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 { if going_up {
est.acceleration - (dt * MAX_JERK) est.acceleration - (dt * MAX_JERK)
} else { } else {