Final solution. Including velocity ramping and min/max speeds.