ma working code, new gyro subsystem

src/main/java/frc/robot/RobotContainer.java

@@ -22,6 +22,7 @@
 import frc.robot.commands.DefaultDrive;
 import frc.robot.commands.StraightCommand;
 import frc.robot.subsystems.DriveSubsystem;
+import frc.robot.subsystems.GyroSubsystem;
 import frc.robot.subsystems.UltrasonicSubsystem;
 import java.util.HashMap;
 import java.util.List;
@@ -45,16 +46,20 @@ public class RobotContainer {
   private final UltrasonicSubsystem m_ultrasonic1 =
       new UltrasonicSubsystem(Constants.ULTRASONIC1PORT);
 
+  private final GyroSubsystem m_gyroSubsystem = new GyroSubsystem();
   private final DriveSubsystem m_driveSubsystem = new DriveSubsystem();
+
   // The robots commands are defined here..
   // private final ExampleCommand m_autoCommand = new ExampleCommand(m_exampleSubsystem);
 
-  private final AimCommand m_aimCommand = new AimCommand(m_driveSubsystem);
-  private final BalanceCommand m_balanceCommand = new BalanceCommand(m_driveSubsystem);
+  private final AimCommand m_aimCommand = new AimCommand(m_driveSubsystem, m_gyroSubsystem);
+  private final BalanceCommand m_balanceCommand =
+      new BalanceCommand(m_driveSubsystem, m_gyroSubsystem);
   private final DefaultDrive m_defaultDrive =
       new DefaultDrive(m_driveSubsystem, this::getControllerLeftY, this::getControllerRightY);
   private final StraightCommand m_straightCommand =
-      new StraightCommand(m_driveSubsystem, this::getControllerLeftY, this::getControllerRightY);
+      new StraightCommand(
+          m_driveSubsystem, m_gyroSubsystem, this::getControllerLeftY, this::getControllerRightY);
   // misc init
   private Trigger m_switchCameraButton;
   private Trigger m_balanceButton;

src/main/java/frc/robot/commands/AimCommand.java

@@ -8,13 +8,15 @@
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc.robot.subsystems.DriveSubsystem;
+import frc.robot.subsystems.GyroSubsystem;
 import org.photonvision.PhotonCamera;
 import org.photonvision.PhotonUtils;
 import org.photonvision.targeting.PhotonPipelineResult;
 
 /** The Aim command that uses the camera + gyro to control the robot. */
 public class AimCommand extends CommandBase {
   private final DriveSubsystem m_driveSubsystem;
+  private final GyroSubsystem m_gyroSubsystem;
   private final PhotonCamera m_camera;
   private final String CAMERANAME = "OV5647";
   // Constants such as camera and target height stored. Change per robot and goal!
@@ -29,16 +31,18 @@ public class AimCommand extends CommandBase {
   /**
    * Creates a new AimCommand.
    *
-   * @param d_subsystem The drive subsystem used by this command.
+   * @param d_driveSubsystem The drive subsystem used by this command.
+   * @param d_gyroSubsystem The gyro subsystem used by this command.
    */
-  public AimCommand(DriveSubsystem d_subsystem) {
-    m_driveSubsystem = d_subsystem;
+  public AimCommand(DriveSubsystem d_driveSubsystem, GyroSubsystem d_gyroSubsystem) {
+    m_driveSubsystem = d_driveSubsystem;
+    m_gyroSubsystem = d_gyroSubsystem;
 
     // Change this to match the name of your camera
     m_camera = new PhotonCamera(CAMERANAME);
 
     // Use addRequirements() here to declare subsystem dependencies.
-    addRequirements(d_subsystem);
+    addRequirements(d_driveSubsystem, d_gyroSubsystem);
   }
 
   // Called when the command is initially scheduled.
@@ -52,7 +56,7 @@ public void execute() {
     // will not work if cam is defined incorrectly, but will not tell you
     if (CamResult.hasTargets()) {
       SmartDashboard.putBoolean("CameraTargetDetected", true);
-      double angleGoal = m_driveSubsystem.getYaw() + CamResult.getBestTarget().getYaw();
+      double angleGoal = m_gyroSubsystem.getYaw() + CamResult.getBestTarget().getYaw();
       SmartDashboard.putNumber("CameraTargetPitch", angleGoal);
       double distanceFromTarget =
           PhotonUtils.calculateDistanceToTargetMeters(
@@ -62,7 +66,7 @@ public void execute() {
                   Units.degreesToRadians(CamResult.getBestTarget().getPitch()))
               - GOAL_RANGE_METERS;
       // turn and move towards target.
-      m_driveSubsystem.driveAndTurn(m_driveSubsystem.getYaw(), angleGoal, distanceFromTarget);
+      m_driveSubsystem.driveAndTurn(m_gyroSubsystem.getYaw(), angleGoal, distanceFromTarget);
       // we reset the angle everytime as the target could change / move.
       m_driveSubsystem.turnSetGoal(angleGoal);
       m_driveSubsystem.distanceSetGoal(distanceFromTarget);

src/main/java/frc/robot/commands/BalanceCommand.java

@@ -6,20 +6,24 @@
 
 import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc.robot.subsystems.DriveSubsystem;
+import frc.robot.subsystems.GyroSubsystem;
 
 /** A Balancing command that uses the gyro subsystem. */
 public class BalanceCommand extends CommandBase {
   private final DriveSubsystem m_driveSubsystem;
+  private final GyroSubsystem m_gyroSubsystem;
 
   /**
    * Creates a new BalanceCommand.
    *
-   * @param d_subsystem The drive subsystem used by this command.
+   * @param d_driveSubsystem The drive subsystem used by this command.
+   * @param d_gyroSubsystem The gyro subsystem used by this command.
    */
-  public BalanceCommand(DriveSubsystem d_subsystem) {
-    m_driveSubsystem = d_subsystem;
+  public BalanceCommand(DriveSubsystem d_driveSubsystem, GyroSubsystem d_gyroSubsystem) {
+    m_driveSubsystem = d_driveSubsystem;
+    m_gyroSubsystem = d_gyroSubsystem;
     // Use addRequirements() here to declare subsystem dependencies.
-    addRequirements(m_driveSubsystem);
+    addRequirements(m_driveSubsystem, m_gyroSubsystem);
   }
 
   // Called when the command is initially scheduled.
@@ -31,7 +35,7 @@ public void initialize() {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    m_driveSubsystem.balanceCorrection(m_driveSubsystem.getPitch());
+    m_driveSubsystem.balanceCorrection(m_gyroSubsystem.getPitch());
   }
 
   // Called once the command ends or is interrupted.

src/main/java/frc/robot/commands/StraightCommand.java

@@ -6,29 +6,37 @@
 
 import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc.robot.subsystems.DriveSubsystem;
+import frc.robot.subsystems.GyroSubsystem;
 import java.util.function.DoubleSupplier;
 
 /** An example command that uses an example subsystem. */
 public class StraightCommand extends CommandBase {
   private final DriveSubsystem m_driveSubsystem;
+  private final GyroSubsystem m_gyroSubsystem;
   private final DoubleSupplier m_left_y; // this gives us the left y axis for current controller
   private final DoubleSupplier m_right_y; // this gives us the right y axis for current controller
   /**
    * Creates a new StraightCommand.
    *
-   * @param d_subsystem The drive subsystem used by this command.
+   * @param d_driveSubsystem The drive subsystem used by this command.
+   * @param d_gyroSubsystem The gyro subsystem used by this command.
    * @param xbox_left_y A function that returns the value of the left y axis for the joystick.
    * @param xbox_right_y A function that returns the value of the right Y axis for the joystick.
    */
   public StraightCommand(
-      DriveSubsystem d_subsystem, DoubleSupplier xbox_left_y, DoubleSupplier xbox_right_y) {
-    m_driveSubsystem = d_subsystem;
+      DriveSubsystem d_driveSubsystem,
+      GyroSubsystem d_gyroSubsystem,
+      DoubleSupplier xbox_left_y,
+      DoubleSupplier xbox_right_y) {
+    m_driveSubsystem = d_driveSubsystem;
+    m_gyroSubsystem = d_gyroSubsystem;
+
     m_left_y = xbox_left_y;
     m_right_y = xbox_right_y;
 
     // Change this to match the name of your camera
     // Use addRequirements() here to declare subsystem dependencies.
-    addRequirements(d_subsystem);
+    addRequirements(d_driveSubsystem, m_gyroSubsystem);
   }
 
   // Called when the command is initially scheduled.
@@ -41,8 +49,8 @@ public void initialize() {
   @Override
   public void execute() {
     m_driveSubsystem.driveStraight(
-        m_driveSubsystem.getYaw(),
-        m_driveSubsystem.getAccumYaw(),
+        m_gyroSubsystem.getYaw(),
+        m_gyroSubsystem.getAccumYaw(),
         (m_left_y.getAsDouble() + m_right_y.getAsDouble()) / 2);
   }
 

src/main/java/frc/robot/subsystems/DriveSubsystem.java

@@ -5,16 +5,13 @@
 import com.ctre.phoenix.motorcontrol.NeutralMode;
 // import motor & frc dependencies
 import com.ctre.phoenix.motorcontrol.can.WPI_VictorSPX;
-import com.kauailabs.navx.frc.AHRS;
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.controller.ProfiledPIDController;
 import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.kinematics.DifferentialDriveOdometry;
 import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds;
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import edu.wpi.first.wpilibj.Encoder;
-import edu.wpi.first.wpilibj.SPI;
 import edu.wpi.first.wpilibj.drive.DifferentialDrive;
 import edu.wpi.first.wpilibj.motorcontrol.MotorControllerGroup;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
@@ -26,7 +23,6 @@
 /** This Subsystem is what allows the code to interact with the drivetrain of the robot. */
 public class DriveSubsystem extends SubsystemBase {
   // Gyro
-  private final AHRS m_Gyro;
 
   // motors
   private final WPI_VictorSPX m_backLeft;
@@ -86,8 +82,7 @@ public class DriveSubsystem extends SubsystemBase {
 
   /** Creates a new DriveSubsystem. */
   public DriveSubsystem() {
-    // Init gyro
-    m_Gyro = new AHRS(SPI.Port.kMXP);
+
     // init motors
     // rio means built into the roboRIO
     m_backLeft = new WPI_VictorSPX(CANConstants.MOTORBACKLEFTID);
@@ -117,12 +112,13 @@ public DriveSubsystem() {
     m_encoderRight.setMinRate(0.1); // min rate to be determined moving
     resetEncoders();
     // Every time this function is called, A dollar is taken out of Jack's savings. Aka do it more.
-    resetGyro();
 
     // configure Odemetry
     m_driveOdometry =
         new DifferentialDriveOdometry(
-            getRotation2d(), m_encoderLeft.getDistance(), m_encoderRight.getDistance());
+            frc.robot.subsystems.GyroSubsystem.Rotation2d,
+            m_encoderLeft.getDistance(),
+            m_encoderRight.getDistance());
 
     // config turn pid controller.
     m_turnController.enableContinuousInput(-180.0f, 180.0f);
@@ -312,55 +308,31 @@ public void SetCoastmode() {
   public void resetPose(Pose2d pose) {
     resetEncoders();
     m_driveOdometry.resetPosition(
-        getRotation2d(), m_encoderLeft.getDistance(), m_encoderRight.getDistance(), pose);
+        frc.robot.subsystems.GyroSubsystem.Rotation2d,
+        m_encoderLeft.getDistance(),
+        m_encoderRight.getDistance(),
+        pose);
   }
 
   public void stop() {
     this.tankDrive(0, 0);
   }
 
-  public void calibrate() {
-    m_Gyro.calibrate();
-  }
-
-  public Rotation2d getRotation2d() {
-    return m_Gyro.getRotation2d();
-  }
-
-  // for balance correction
-  public double getPitch() {
-    return m_Gyro.getPitch(); // get pitch in degrees
-  }
-
-  // for PID control (turn by degrees)
-  public double getAccumYaw() {
-    return m_Gyro.getAngle(); // get angle in degrees
-  }
-
-  public double getYaw() {
-    return m_Gyro.getYaw();
-  }
-
-  public void resetGyro() {
-    m_Gyro.reset();
-  }
-
   @Override
   public void periodic() {
-    // This method will be called once per scheduler run
+    // Update the odometry in the periodic block
     SmartDashboard.putNumber("Left Encoder Speed (M/s)", this.m_encoderLeft.getRate());
     SmartDashboard.putNumber("Right Encoder Speed (M/s)", this.m_encoderRight.getRate());
-    SmartDashboard.putNumber("Distance L", this.m_encoderLeft.getDistance());
+    SmartDashboard.putNumber("Distance L", m_encoderLeft.getDistance());
     SmartDashboard.putNumber("Distance R", this.m_encoderRight.getDistance());
     SmartDashboard.putNumber("Current Robot Location X axis", getPose().getX());
     SmartDashboard.putNumber("Current Robot Location Y axis", getPose().getY());
     SmartDashboard.putNumber("Current Robot Rotation", getPose().getRotation().getDegrees());
     SmartDashboard.putNumber("Average Distance Traveled", AverageDistance());
-    SmartDashboard.putNumber("Current Gyro Pitch", getPitch());
-    SmartDashboard.putNumber("Current Gyro Yaw", getYaw());
-    // Update the odometry in the periodic block
     m_driveOdometry.update(
-        getRotation2d(), m_encoderLeft.getDistance(), m_encoderRight.getDistance());
+        frc.robot.subsystems.GyroSubsystem.Rotation2d,
+        m_encoderLeft.getDistance(),
+        m_encoderRight.getDistance());
   }
 
   @Override

src/main/java/frc/robot/subsystems/GyroSubsystem.java

@@ -0,0 +1,63 @@
+// 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 frc.robot.subsystems;
+
+import com.kauailabs.navx.frc.AHRS;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.SPI;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+public class GyroSubsystem extends SubsystemBase {
+
+  final AHRS m_Gyro;
+  public static Rotation2d Rotation2d;
+  /** Creates a new ExampleSubsystem. */
+  public GyroSubsystem() {
+    // Init gyro
+    m_Gyro = new AHRS(SPI.Port.kMXP);
+    Rotation2d = this.getRotation2d();
+    resetGyro();
+  }
+
+  public void calibrate() {
+    m_Gyro.calibrate();
+  }
+
+  public Rotation2d getRotation2d() {
+    return m_Gyro.getRotation2d();
+  }
+
+  // for balance correction
+  public double getPitch() {
+    return m_Gyro.getPitch(); // get pitch in degrees
+  }
+
+  // for PID control (turn by degrees)
+  public double getAccumYaw() {
+    return m_Gyro.getAngle(); // get angle in degrees
+  }
+
+  public double getYaw() {
+    return m_Gyro.getYaw();
+  }
+
+  public void resetGyro() {
+    m_Gyro.reset();
+  }
+
+  @Override
+  public void periodic() {
+    // This method will be called once per scheduler run
+    SmartDashboard.putNumber("Current Gyro Pitch", getPitch());
+    SmartDashboard.putNumber("Current Gyro Yaw", getYaw());
+    Rotation2d = this.getRotation2d();
+  }
+
+  @Override
+  public void simulationPeriodic() {
+    // This method will be called once per scheduler run during simulation
+  }
+}