+ // ------Gyro------//
+ public double getAngle() {
+ if (!this.imu.isInitialized())
+ return -1;
+ return this.imu.getHeading() - this.gyroZero;
+ }
+
+ public void resetGyro() {
+ this.gyroZero = this.getAngle();
+
+ }
+
+ public double getZeroAngle() {
+ return this.gyroZero;
+ }
+