Update align to goal with offset

Update align to goal with offset.
Improve goal align and stall detect

Improve goal align by slowing down when close to alignment.
Improve stall detect.by using further sensors.