worky

src/main/java/frc4388/utility/Gains.java

@@ -0,0 +1,83 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc4388.utility;
+
+/** Add your docs here. */
+public class Gains {
+    public double kP;
+    public double kI;
+    public double kD;
+    public double kF;
+    public int kIZone;
+    public double kPeakOutput;
+    public double kMaxOutput;
+    public double kMinOutput;
+
+    /**
+     * Creates Gains object for PIDs
+     * @param kP The P value.
+     * @param kI The I value.
+     * @param kD The D value.
+     * @param kF The F value.
+     * @param kIZone The zone of the I value.
+     * @param kPeakOutput The peak output setting the motors to run the gains at, in both forward and reverse directions. By default 1.0.
+     */
+    public Gains(double kP, double kI, double kD, double kF, int kIZone, double kPeakOutput) {
+        this.kP = kP;
+        this.kI = kI;
+        this.kD = kD;
+        this.kF = kF;
+        this.kIZone = kIZone;
+        this.kPeakOutput = kPeakOutput;
+        this.kMaxOutput = kPeakOutput;
+        this.kMinOutput = -kPeakOutput;
+    }
+
+    /**
+     * Creates Gains object for PIDs
+     * @param kP The P value.
+     * @param kI The I value.
+     * @param kD The D value.
+     * @param kF The F value.
+     * @param kIZone The zone of the I value.
+     */
+    public Gains(double kP, double kI, double kD, double kF, int kIZone) {
+        this.kP = kP;
+        this.kI = kI;
+        this.kD = kD;
+        this.kF = kF;
+        this.kIZone = kIZone;
+        this.kPeakOutput = 1.0;
+        this.kMaxOutput = 1.0;
+        this.kMinOutput = -1.0;
+    }
+
+    public Gains(double kP, double kI, double kD) {
+        this.kP = kP;
+        this.kI = kI;
+        this.kD = kD;
+    }
+
+    /**
+     * Creates Gains object for PIDs
+     * @param kP The P value.
+     * @param kI The I value.
+     * @param kD The D value.
+     * @param kF The F value.
+     * @param kIZone The zone of the I value.
+     * @param kMinOutput The lowest output setting to run the gains at, usually in the reverse direction. By default -1.0.
+     * @param kMaxOutput The highest output setting to run the gains at, usually in the forward direction. By default 1.0.
+     */
+    public Gains(double kP, double kI, double kD, double kF, int kIZone, double kMaxOutput, double kMinOutput) {
+        this.kP = kP;
+        this.kI = kI;
+        this.kD = kD;
+        this.kF = kF;
+        this.kIZone = kIZone;
+        this.kMaxOutput = kMaxOutput;
+        this.kMinOutput = kMinOutput;
+        this.kPeakOutput = (Math.abs(kMinOutput) > Math.abs(kMaxOutput)) ? Math.abs(kMinOutput) : Math.abs(kMaxOutput);
+    }    
+}

src/main/java/frc4388/utility/RobotGyro.java

@@ -7,31 +7,34 @@
 
 package frc4388.utility;
 
-import com.ctre.phoenix.sensors.PigeonIMU;
-import com.ctre.phoenix.sensors.PigeonIMU.CalibrationMode;
+import com.ctre.phoenix.sensors.WPI_Pigeon2;
 import com.kauailabs.navx.frc.AHRS;
 
-import edu.wpi.first.wpilibj.GyroBase;
-import edu.wpi.first.wpiutil.math.MathUtil;
+// import edu.wpi.first.wpilibj.GyroBase;
+import edu.wpi.first.wpilibj.interfaces.Gyro;
+import edu.wpi.first.math.MathUtil;
 
 /**
  * Gyro class that allows for interchangeable use between a pigeon and a navX
  */
-public class RobotGyro extends GyroBase {
+public class RobotGyro implements Gyro {
     private RobotTime m_robotTime = RobotTime.getInstance();
 
-    private PigeonIMU m_pigeon = null;
+    private WPI_Pigeon2 m_pigeon = null;
     private AHRS m_navX = null;
     public boolean m_isGyroAPigeon; //true if pigeon, false if navX
 
     private double m_lastPigeonAngle;
     private double m_deltaPigeonAngle;
 
+    private double pitchZero = 0;
+    private double rollZero = 0;
+
     /**
      * Creates a Gyro based on a pigeon
      * @param gyro the gyroscope to use for Gyro
      */
-    public RobotGyro(PigeonIMU gyro) {
+    public RobotGyro(WPI_Pigeon2 gyro) {
         m_pigeon = gyro;
         m_isGyroAPigeon = true;
     }
@@ -45,6 +48,16 @@ public class RobotGyro extends GyroBase {
         m_isGyroAPigeon = false;
     }
 
+    /**
+     * Resets yaw, pitch, and roll.
+     */
+    public void resetZeroValues() {
+        if (!m_isGyroAPigeon) return;
+
+        pitchZero = m_pigeon.getPitch();
+        rollZero =  m_pigeon.getRoll();
+    }
+
     /**
      * Run in periodic if you are using a pigeon. Updates a delta angle so that it can calculate getRate(). Note
      * that the getRate() method for a navX will likely be much more accurate than for a pigeon.
@@ -74,7 +87,7 @@ public class RobotGyro extends GyroBase {
     @Override
     public void calibrate() {
         if (m_isGyroAPigeon) {
-            m_pigeon.enterCalibrationMode(CalibrationMode.Temperature);
+            m_pigeon.calibrate();
         } else {
             m_navX.calibrate();
         }
@@ -82,6 +95,8 @@ public class RobotGyro extends GyroBase {
 
     @Override
     public void reset() {
+        resetZeroValues();
+
         if (m_isGyroAPigeon) {
             m_pigeon.setYaw(0);
         } else {
@@ -98,9 +113,10 @@ public class RobotGyro extends GyroBase {
 	 *					Roll is within [-90,+90] degrees.
      */
     private double[] getPigeonAngles() {
-        double[] angles = new double[3];
-        m_pigeon.getYawPitchRoll(angles);
-        return angles;
+        double[] ypr = new double[3];
+        m_pigeon.getYawPitchRoll(ypr);
+
+        return new double[] {ypr[0], (ypr[1] - pitchZero), (ypr[2] - rollZero)};
     }
 
     @Override
@@ -112,6 +128,10 @@ public class RobotGyro extends GyroBase {
         }
     }
 
+    public double getYaw() {
+       return this.getAngle();
+    }
+
     /**
      * Gets an absolute heading of the robot
      * @return heading from -180 to 180 degrees
@@ -165,7 +185,7 @@ public class RobotGyro extends GyroBase {
         }
     }
 
-    public PigeonIMU getPigeon(){
+    public WPI_Pigeon2 getPigeon(){
         return m_pigeon;
     }
 

src/main/java/frc4388/utility/controller/XboxTriggerButton.java

@@ -1,7 +1,6 @@
 package frc4388.utility.controller;
 
 import edu.wpi.first.wpilibj2.command.button.Button;
-import frc4388.utility.controller.XboxController;
 
 /**
  * Mapping for the Xbox controller triggers to allow triggers to be defined as
@@ -32,7 +31,7 @@ public class XboxTriggerButton extends Button {
 	}
 
 	/** {@inheritDoc} */
-	@Override
+	// @Override
 	public boolean get() {
 		if (m_trigger == RIGHT_TRIGGER) {
 			return m_controller.getRightTrigger();