Merge moment

src/main/java/frc4388/robot/Robot.java

@@ -113,6 +113,8 @@ public class Robot extends TimedRobot {
       m_autonomousCommand.cancel();
     }
     m_robotTime.startMatchTime();
+
+    m_robotContainer.gyroRef.reset();
   }
 
   /**
@@ -123,6 +125,9 @@ public class Robot extends TimedRobot {
     SmartDashboard.putNumber("Gyro Yaw", gyro.getYaw());
     SmartDashboard.putNumber("Gyro Pitch", gyro.getPitch());
     SmartDashboard.putNumber("Gyro Roll", gyro.getRoll());
+    SmartDashboard.putNumber("yaw", m_robotContainer.gyroRef.getAngle());
+    SmartDashboard.putNumber("pitch", m_robotContainer.gyroRef.getPitch());
+    SmartDashboard.putNumber("roll", m_robotContainer.gyroRef.getRoll());
   }
 
   /**

src/main/java/frc4388/robot/RobotContainer.java

@@ -15,6 +15,7 @@ import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 import frc4388.robot.Constants.*;
 import frc4388.robot.subsystems.LED;
 import frc4388.utility.LEDPatterns;
+import frc4388.utility.RobotGyro;
 import frc4388.utility.controller.IHandController;
 import frc4388.utility.controller.XboxController;
 
@@ -36,6 +37,8 @@ public class RobotContainer {
     private final XboxController m_driverXbox = new XboxController(OIConstants.XBOX_DRIVER_ID);
     private final XboxController m_operatorXbox = new XboxController(OIConstants.XBOX_OPERATOR_ID);
 
+    public RobotGyro gyroRef = m_robotMap.gyro;
+
     /**
      * The container for the robot. Contains subsystems, OI devices, and commands.
      */

src/main/java/frc4388/robot/RobotMap.java

@@ -11,6 +11,7 @@ import com.ctre.phoenix.motorcontrol.InvertType;
 import com.ctre.phoenix.motorcontrol.NeutralMode;
 import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
 import com.ctre.phoenix.sensors.PigeonIMU;
+import com.ctre.phoenix.sensors.WPI_Pigeon2;
 
 import edu.wpi.first.wpilibj.motorcontrol.Spark;
 import edu.wpi.first.wpilibj.drive.DifferentialDrive;
@@ -22,6 +23,8 @@ import frc4388.utility.RobotGyro;
  * testing and modularization.
  */
 public class RobotMap {
+    private WPI_Pigeon2 m_pigeon2 = new WPI_Pigeon2(14);
+    public RobotGyro gyro = new RobotGyro(m_pigeon2);
 
     public RobotMap() {
         configureLEDMotorControllers();

src/main/java/frc4388/robot/commands/AutoBalanceTF2.java

@@ -0,0 +1,36 @@
+// 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.robot.commands;
+
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.wpilibj2.command.PIDCommand;
+
+// NOTE:  Consider using this command inline, rather than writing a subclass.  For more
+// information, see:
+// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
+public class AutoBalanceTF2 extends PIDCommand {
+  /** Creates a new AutoBalanceTF2. */
+  public AutoBalanceTF2() {
+    super(
+        // The controller that the command will use
+        new PIDController(0, 0, 0),
+        // This should return the measurement
+        () -> 0,
+        // This should return the setpoint (can also be a constant)
+        () -> 0,
+        // This uses the output
+        output -> {
+          // Use the output here
+        });
+    // Use addRequirements() here to declare subsystem dependencies.
+    // Configure additional PID options by calling `getController` here.
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

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

@@ -8,6 +8,9 @@
 package frc4388.utility;
 
 import com.ctre.phoenix.sensors.WPI_Pigeon2;
+import com.ctre.phoenix.sensors.PigeonIMU;
+import com.ctre.phoenix.sensors.WPI_Pigeon2;
+import com.ctre.phoenix.sensors.PigeonIMU.CalibrationMode;
 import com.kauailabs.navx.frc.AHRS;
 
 // import edu.wpi.first.wpilibj.GyroBase;
@@ -27,7 +30,6 @@ public class RobotGyro implements Gyro {
     private double m_lastPigeonAngle;
     private double m_deltaPigeonAngle;
 
-    private double yawZero = 0;
     private double pitchZero = 0;
     private double rollZero = 0;
 
@@ -52,10 +54,9 @@ public class RobotGyro implements Gyro {
     /**
      * Resets yaw, pitch, and roll.
      */
-    public void resetAllAngles() {
+    public void resetZeroValues() {
         if (!m_isGyroAPigeon) return;
 
-        yawZero = m_pigeon.getYaw();
         pitchZero = m_pigeon.getPitch();
         rollZero =  m_pigeon.getRoll();
     }
@@ -97,6 +98,8 @@ public class RobotGyro implements Gyro {
 
     @Override
     public void reset() {
+        resetZeroValues();
+
         if (m_isGyroAPigeon) {
             m_pigeon.setYaw(0);
         } else {
@@ -116,7 +119,7 @@ public class RobotGyro implements Gyro {
         double[] ypr = new double[3];
         m_pigeon.getYawPitchRoll(ypr);
 
-        return new double[] {(ypr[0] - yawZero), (ypr[1] - pitchZero), (ypr[2] - rollZero)};
+        return new double[] {ypr[0], (ypr[1] - pitchZero), (ypr[2] - rollZero)};
     }
 
     @Override