Cleaned up code
Fixed constants
Gyro and encoder code works
tested