New motor control algorithm, u.s. sensor filtering

Changed motor control algorithm:
> Now uses base percentages which we determined to be close to stable
hovering
> Instead of incrementing the motor speed, applies an offset that is
calculated based on distance from target and sensitivity. This should
improve response
Added ultrasonic sensor filtering:
>If new reading is too far away from last reading, discard.