modify drive functions to scale down their speed as they get closer to their setpoints. need to test and tune