Merged Gyro and Drive subsystems (#12)

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

@@ -26,7 +26,6 @@
 import frc.robot.commands.arm.ArmUpCommand;
 import frc.robot.commands.arm.ClawCommand;
 import frc.robot.subsystems.DriveSubsystem;
-import frc.robot.subsystems.GyroSubsystem;
 import frc.robot.subsystems.UltrasonicSubsystem;
 import frc.robot.subsystems.arm.ClawSubsystem;
 import frc.robot.subsystems.arm.ElevatorSubsystem;
@@ -52,23 +51,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(m_gyroSubsystem);
+  private final DriveSubsystem m_driveSubsystem = new DriveSubsystem();
   private final ElevatorSubsystem m_elevatorSubsystem = new ElevatorSubsystem();
   private final ClawSubsystem m_clawSubsystem = new ClawSubsystem();
   // The robots commands are defined here..
   // private final ExampleCommand m_autoCommand = new ExampleCommand(m_exampleSubsystem);
 
-  private final AimCommand m_aimCommand = new AimCommand(m_driveSubsystem, m_gyroSubsystem);
-  private final BalanceCommand m_balanceCommand =
-      new BalanceCommand(m_driveSubsystem, m_gyroSubsystem);
+  private final AimCommand m_aimCommand = new AimCommand(m_driveSubsystem);
+  private final BalanceCommand m_balanceCommand = new BalanceCommand(m_driveSubsystem);
   private final PlaceCommand m_placeCommand =
       new PlaceCommand(m_clawSubsystem, m_elevatorSubsystem);
   private final DefaultDrive m_defaultDrive =
       new DefaultDrive(m_driveSubsystem, this::getControllerLeftY, this::getControllerRightY);
   private final StraightCommand m_straightCommand =
-      new StraightCommand(
-          m_driveSubsystem, m_gyroSubsystem, this::getControllerLeftY, this::getControllerRightY);
+      new StraightCommand(m_driveSubsystem, this::getControllerLeftY, this::getControllerRightY);
   private final ClawCommand m_clawCommand = new ClawCommand(m_clawSubsystem, m_flightStick::getY);
   private final ArmUpCommand m_armUpCommand = new ArmUpCommand(m_elevatorSubsystem);
   private final ArmDownCommand m_armDownCommand = new ArmDownCommand(m_elevatorSubsystem);

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

@@ -8,15 +8,13 @@
 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!
@@ -32,17 +30,15 @@ public class AimCommand extends CommandBase {
    * Creates a new AimCommand.
    *
    * @param d_subsystem The drive subsystem used by this command.
-   * @param g_subsystem The gyro subsystem used by this command.
    */
-  public AimCommand(DriveSubsystem d_subsystem, GyroSubsystem g_subsystem) {
+  public AimCommand(DriveSubsystem d_subsystem) {
     m_driveSubsystem = d_subsystem;
-    m_gyroSubsystem = g_subsystem;
 
     // Change this to match the name of your camera
     m_camera = new PhotonCamera(CAMERANAME);
 
     // Use addRequirements() here to declare subsystem dependencies.
-    addRequirements(d_subsystem, g_subsystem);
+    addRequirements(d_subsystem);
   }
 
   // Called when the command is initially scheduled.
@@ -56,7 +52,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_gyroSubsystem.getYaw() + CamResult.getBestTarget().getYaw();
+      double angleGoal = m_driveSubsystem.getYaw() + CamResult.getBestTarget().getYaw();
       SmartDashboard.putNumber("CameraTargetPitch", angleGoal);
       double distanceFromTarget =
           PhotonUtils.calculateDistanceToTargetMeters(
@@ -66,7 +62,7 @@ public void execute() {
                   Units.degreesToRadians(CamResult.getBestTarget().getPitch()))
               - GOAL_RANGE_METERS;
       // turn and move towards target.
-      m_driveSubsystem.driveAndTurn(m_gyroSubsystem.getYaw(), angleGoal, distanceFromTarget);
+      m_driveSubsystem.driveAndTurn(m_driveSubsystem.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,24 +6,20 @@
 
 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 GyroSubsystem m_gyroSubsystem;
   private final DriveSubsystem m_driveSubsystem;
 
   /**
    * Creates a new BalanceCommand.
    *
    * @param d_subsystem The drive subsystem used by this command.
-   * @param g_subsystem The gyro subsystem used by this command.
    */
-  public BalanceCommand(DriveSubsystem d_subsystem, GyroSubsystem g_subsystem) {
+  public BalanceCommand(DriveSubsystem d_subsystem) {
     m_driveSubsystem = d_subsystem;
-    m_gyroSubsystem = g_subsystem;
     // Use addRequirements() here to declare subsystem dependencies.
-    addRequirements(m_driveSubsystem, m_gyroSubsystem);
+    addRequirements(m_driveSubsystem);
   }
 
   // Called when the command is initially scheduled.
@@ -35,7 +31,7 @@ public void initialize() {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    m_driveSubsystem.balanceCorrection(m_gyroSubsystem.getPitch());
+    m_driveSubsystem.balanceCorrection(m_driveSubsystem.getPitch());
   }
 
   // Called once the command ends or is interrupted.

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

@@ -6,36 +6,29 @@
 
 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 g_subsystem 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,
-      GyroSubsystem g_subsystem,
-      DoubleSupplier xbox_left_y,
-      DoubleSupplier xbox_right_y) {
+      DriveSubsystem d_subsystem, DoubleSupplier xbox_left_y, DoubleSupplier xbox_right_y) {
     m_driveSubsystem = d_subsystem;
-    m_gyroSubsystem = g_subsystem;
     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, g_subsystem);
+    addRequirements(d_subsystem);
   }
 
   // Called when the command is initially scheduled.
@@ -48,8 +41,8 @@ public void initialize() {
   @Override
   public void execute() {
     m_driveSubsystem.driveStraight(
-        m_gyroSubsystem.getYaw(),
-        m_gyroSubsystem.getAccumYaw(),
+        m_driveSubsystem.getYaw(),
+        m_driveSubsystem.getAccumYaw(),
         (m_left_y.getAsDouble() + m_right_y.getAsDouble()) / 2);
   }
 

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

@@ -5,13 +5,16 @@
 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;
@@ -22,8 +25,8 @@
 
 /** This Subsystem is what allows the code to interact with the drivetrain of the robot. */
 public class DriveSubsystem extends SubsystemBase {
-  // Gyro Subsystem
-  private final GyroSubsystem m_gyroSubsystem;
+  // Gyro
+  private final AHRS m_Gyro;
 
   // motors
   private final WPI_VictorSPX m_backLeft;
@@ -101,8 +104,9 @@ public class DriveSubsystem extends SubsystemBase {
               MaxBalanceRateDegPerS, MaxBalanceAccelerationDegPerSSquared));
 
   /** Creates a new DriveSubsystem. */
-  public DriveSubsystem(final GyroSubsystem g_subsystem) {
-    m_gyroSubsystem = g_subsystem;
+  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);
@@ -131,14 +135,13 @@ public DriveSubsystem(final GyroSubsystem g_subsystem) {
     m_encoderLeft.setMinRate(0.1); // min rate to be determined moving
     m_encoderRight.setMinRate(0.1); // min rate to be determined moving
     resetEncoders();
-    m_gyroSubsystem.reset();
+    // 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(
-            m_gyroSubsystem.getRotation2d(),
-            m_encoderLeft.getDistance(),
-            m_encoderRight.getDistance());
+            getRotation2d(), m_encoderLeft.getDistance(), m_encoderRight.getDistance());
 
     // config turn pid controller.
     m_turnController.enableContinuousInput(-180.0f, 180.0f);
@@ -239,10 +242,10 @@ public void turnSetGoal(double targetAngleDegrees) {
     m_turnController.setGoal(targetAngleDegrees);
   }
 
-  private void calcuateAngleRate(double gyroYawAngle, double TargetAngleDegrees) {
+  private void calcuateAngleRate(double gyroYawAngle, double targetAngleDegrees) {
     if (!turnControllerEnabled) {
       m_turnController.reset(gyroYawAngle);
-      m_turnController.setGoal(TargetAngleDegrees);
+      m_turnController.setGoal(targetAngleDegrees);
       turnControllerEnabled = true;
     }
     turnRotateToAngleRate = MathUtil.clamp(m_turnController.calculate(gyroYawAngle), -1.0, 1.0);
@@ -323,16 +326,39 @@ public void SetCoastmode() {
   public void resetPose(Pose2d pose) {
     resetEncoders();
     m_driveOdometry.resetPosition(
-        m_gyroSubsystem.getRotation2d(),
-        m_encoderLeft.getDistance(),
-        m_encoderRight.getDistance(),
-        pose);
+        getRotation2d(), 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
@@ -344,9 +370,11 @@ public void periodic() {
     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(
-        m_gyroSubsystem.getRotation2d(), m_encoderLeft.getDistance(), m_encoderRight.getDistance());
+        getRotation2d(), m_encoderLeft.getDistance(), m_encoderRight.getDistance());
   }
 
   @Override