Если у Вас явный дискомфорт при торможении, с учетом что у пунта тормоза вообще замечательные, то может дело не в алгоритме робота, а конкретно в исполнительных механизмах тормозной и сопутствующих систем?
И последуйте советам materillo, может все вопросы сразу и решатся.