Updated driving functions

They still need some fine tuning
Gyro is still calibrated correctly