polish pathplanning
fix up joystick and pathplanner (one bug remaining), add trees for a possibly "sufficiently complex environment" :)