Encoder testing, fixed drivetrain, adjusted the way jag works, bug fixes
Fixed Drive function
Added enhanced jaguar testing
Added correct formula for the ultrasonic with r^2=0.999
fixed unsafe stuff