From bf921ece957cae792bd5c52b2c7005bbf09ca469 Mon Sep 17 00:00:00 2001
From: Cindy Zhang <cindyzyx9@gmail.com>
Date: Mon, 30 Jan 2017 19:55:00 -0800
Subject: [PATCH] change gyro from old imu to new adx imu

---
 src/org/usfirst/frc/team3501/robot/Constants.java  |  4 ++++
 .../frc/team3501/robot/subsystems/DriveTrain.java  | 14 ++++----------
 2 files changed, 8 insertions(+), 10 deletions(-)

diff --git a/src/org/usfirst/frc/team3501/robot/Constants.java b/src/org/usfirst/frc/team3501/robot/Constants.java
index 0c3fb74..d8af3db 100644
--- a/src/org/usfirst/frc/team3501/robot/Constants.java
+++ b/src/org/usfirst/frc/team3501/robot/Constants.java
@@ -1,5 +1,7 @@
 package org.usfirst.frc.team3501.robot;
 
+import edu.wpi.first.wpilibj.SPI;
+
 /**
  * The Constants stores constant values for all subsystems. This includes the
  * port values for motors and sensors, as well as important operational
@@ -37,6 +39,8 @@ public class Constants {
     public static final int ENCODER_LEFT_B = 0;
     public static final int ENCODER_RIGHT_A = 2;
     public static final int ENCODER_RIGHT_B = 3;
+
+    public static final SPI.Port GYRO_PORT = SPI.Port.kOnboardCS0;
   }
 
   public static class Climber {
diff --git a/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java b/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java
index c445f63..eb9a442 100644
--- a/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java
+++ b/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java
@@ -2,12 +2,11 @@ package org.usfirst.frc.team3501.robot.subsystems;
 
 import org.usfirst.frc.team3501.robot.Constants;
 import org.usfirst.frc.team3501.robot.commands.driving.JoystickDrive;
-import org.usfirst.frc.team3501.robot.utils.BNO055;
 
 import com.ctre.CANTalon;
 
+import edu.wpi.first.wpilibj.ADXRS450_Gyro;
 import edu.wpi.first.wpilibj.Encoder;
-import edu.wpi.first.wpilibj.I2C.Port;
 import edu.wpi.first.wpilibj.RobotDrive;
 import edu.wpi.first.wpilibj.command.Subsystem;
 
@@ -27,7 +26,7 @@ public class DriveTrain extends Subsystem {
   private final RobotDrive robotDrive;
   private final Encoder leftEncoder, rightEncoder;
 
-  private BNO055 imu;
+  private ADXRS450_Gyro imu;
 
   private DriveTrain() {
     // MOTOR CONTROLLERS
@@ -48,9 +47,7 @@ public class DriveTrain extends Subsystem {
     // ROBOT DRIVE
     robotDrive = new RobotDrive(frontLeft, rearLeft, frontRight, rearRight);
 
-    this.imu = BNO055.getInstance(BNO055.opmode_t.OPERATION_MODE_IMUPLUS,
-        BNO055.vector_type_t.VECTOR_EULER, Port.kOnboard, (byte) 0x28);
-    gyroZero = imu.getHeading();
+    this.imu = new ADXRS450_Gyro(Constants.DriveTrain.GYRO_PORT);
   }
 
   public static DriveTrain getDriveTrain() {
@@ -124,14 +121,11 @@ public class DriveTrain extends Subsystem {
 
   // ------Gyro------//
   public double getAngle() {
-    if (!this.imu.isInitialized())
-      return -1;
-    return this.imu.getHeading() - this.gyroZero;
+    return this.imu.getAngle() - this.gyroZero;
   }
 
   public void resetGyro() {
     this.gyroZero = this.getAngle();
-
   }
 
   public double getZeroAngle() {
-- 
2.30.2