Added basic P (of PID) controller to hover function. Tuned kP to be around 1/8.