Pathplanning debuged and enhanced

Signed-off-by: eldr4d <nikofinas@gmail.com>