|
|
|
@ -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;
|
|
|
|
|
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 {
|
|
|
|
|
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
|
|
|
|
|
//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 {
|
|
|
|
|