fix: Use absolute target velocity to calculate end phase distance

[PaulVerhoeckx]

Solves #145

[cesar-lopez-mar]

I think we should check how d_end_phase is used instead of changing is calculation. Negative d_end_phase is allowed, indicating the robot will end the current section at a distance behind the current position

[PaulVerhoeckx]

I'm pretty sure the implementation is wrong. d_end_phase is set equal to the current theoretical stopping distance plus some factor which should compensate for delays. The sign of this theoretical stopping distance does not depend on the sign of current_x_vel while the compensation part does.

For example, take: target_end_x_vel = 0 current_x_vel = target_x_vel = 1 target_x_decc = 1 dt=1/20

This yields:

t_end_phase_current = (target_end_x_vel - current_x_vel) / (-target_x_decc) = 1

d_end_phase = current_x_vel * t_end_phase_current -
                  0.5 * (target_x_decc) * t_end_phase_current * t_end_phase_current +
                  target_x_vel * 2.0 * dt = 1 - 0.5 + 0.1 = 0.6

While for when driving backwards, with: current_x_vel = target_x_vel = -1

This yields:

t_end_phase_current = (target_end_x_vel - current_x_vel) / (-target_x_decc) = -1

d_end_phase = current_x_vel * t_end_phase_current -
                  0.5 * (target_x_decc) * t_end_phase_current * t_end_phase_current +
                  target_x_vel * 2.0 * dt = 1 - 0.5 - 0.1 = 0.4

[cesar-lopez-mar]

t_end_phase_current = (target_end_x_vel - current_x_vel) / (-target_x_decc) = -1

Time should not be negative. The bug is in this equation. During braking when going backwards we are actually accelerating! so the minus sign we added here should depend on the sign of the target velocity

[PaulVerhoeckx]

I proposed a different fix to the problem in a new commit