change gyro from old imu to new adx imu

diff --git a/src/org/usfirst/frc/team3501/robot/Constants.java b/src/org/usfirst/frc/team3501/robot/Constants.java
index 0c3fb74cc339ae3c44dc77c0fca61aad77e2770f..d8af3dbe5fb0d573242c861ae0a809568eb28f8a 100644 (file)
--- 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 c445f630109f1d4d30067f6735fb65e8c5a6395c..eb9a4420a62024423e7a176f6d7bbbbaea6d4d95 100644 (file)
--- 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() {