Added acceleration & deceleration

-Still doesn't work perfectly
-moved setting max_speed  & new_accel to before  stop_accel_pos &
decel_pos are calculated, to avoid using incorrect data