IMU Subsystem

src/main/java/frc/robot/AutoChooser.java

@@ -6,25 +6,25 @@
 /*----------------------------------------------------------------------------*/
 package frc.robot;
 
+//import edu.wpi.first.networktables.NetworkTableEntry;
+import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
+import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.WaitCommand;
 import frc.robot.commands.Autonomous.CrossTheLineSequence;
 import frc.robot.commands.Autonomous.DoNothingSequence;
-import frc.robot.commands.Autonomous.TwoShotSequenceLeft;
 import frc.robot.commands.Autonomous.OneShotSequenceMiddle;
 import frc.robot.commands.Autonomous.ThreeShotSequenceRight;
+import frc.robot.commands.Autonomous.TwoShotSequenceLeft;
 import frc.robot.commands.Autonomous.TwoShotSequenceRight;
-import frc.robot.commands.DriveCommands.Drive;
-import frc.robot.commands.IntakeCommand.IntakeSequence;
 import frc.robot.subsystems.DriveTrain;
 import frc.robot.subsystems.Hood;
+import frc.robot.subsystems.IMUSubsystem;
 import frc.robot.subsystems.IntakeSubsystem;
 import frc.robot.subsystems.ShooterSubsystem;
 import frc.robot.subsystems.TurretSubsystem;
 import frc.robot.utils.limelight.LimeLightVision;
-//import edu.wpi.first.networktables.NetworkTableEntry;
-import edu.wpi.first.wpilibj.shuffleboard.*;
 
 /**
  * Add your docs here.
@@ -38,21 +38,23 @@ public class AutoChooser {
     private TurretSubsystem turretSubsystem;
     private LimeLightVision limeLightVision;
     private Hood hood;
+    private IMUSubsystem IMUSubsystem;
 
     // all actions driver choose at beginning of match
     enum Action {
         THREE_SHOT, TWO_SHOT_RIGHT, TWO_SHOT_LEFT, ONE_SHOT, CROSS_LINE, DO_NOTHING;
     }
 
 
-    public AutoChooser(IntakeSubsystem intakeSubsystem, DriveTrain driveTrain, ShooterSubsystem shooterSubsystem, TurretSubsystem turretSubsystem, LimeLightVision limeLightVision, Hood hood) {
+    public AutoChooser(IntakeSubsystem intakeSubsystem, DriveTrain driveTrain, ShooterSubsystem shooterSubsystem, TurretSubsystem turretSubsystem, LimeLightVision limeLightVision, Hood hood, IMUSubsystem IMUSubsystem) {
         actionChooser = new SendableChooser<Action>();
         this.intakeSubsystem = intakeSubsystem;
         this.driveTrain = driveTrain;
         this.shooterSubsystem = shooterSubsystem;
         this.turretSubsystem = turretSubsystem;
         this.limeLightVision = limeLightVision;
         this.hood = hood;
+        this.IMUSubsystem = IMUSubsystem;
     }
 
     public void addOptions() {
@@ -88,7 +90,7 @@ public Command getAutonomousCommand(Action a) {
             Constants.AUTO_DISTANCE_INCHES, shooterSubsystem, limeLightVision, hood);}
         else if (a == Action.THREE_SHOT) {return new ThreeShotSequenceRight(turretSubsystem, 
             Constants.AUTO_TURRET_SPEED,  intakeSubsystem, driveTrain, Constants.AUTO_MOVE_SPEED, 
-            Constants.AUTO_DISTANCE_INCHES, shooterSubsystem, limeLightVision, hood);}
+            Constants.AUTO_DISTANCE_INCHES, shooterSubsystem, limeLightVision, hood, IMUSubsystem);}
         else if (a == Action.TWO_SHOT_LEFT) {return new TwoShotSequenceLeft(turretSubsystem, 
             Constants.AUTO_TURRET_SPEED,  intakeSubsystem, driveTrain, Constants.AUTO_MOVE_SPEED, 
             Constants.AUTO_DISTANCE_INCHES, shooterSubsystem, limeLightVision, hood);}

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

@@ -73,6 +73,7 @@
 import frc.robot.commands.TurretCommands.TurretSweepSequence;
 import frc.robot.subsystems.DriveTrain;
 import frc.robot.subsystems.Hood;
+import frc.robot.subsystems.IMUSubsystem;
 import frc.robot.subsystems.IntakeSubsystem;
 import frc.robot.subsystems.LimelightSubsystem;
 import frc.robot.subsystems.ShooterSubsystem;
@@ -130,12 +131,13 @@ public class RobotContainer {
   private final ShooterSubsystem shooterSubsystem = new ShooterSubsystem();
   private final PowerDistribution m_PowerDistPanel = new PowerDistribution();
   private final ClimberArmSubsystem climberArmSubsystem = new ClimberArmSubsystem(m_PowerDistPanel);
+  private final IMUSubsystem IMUSubsystem = new IMUSubsystem();
   private final ClimberWinchSubsystem climberWinchSubsystem = new ClimberWinchSubsystem(m_PowerDistPanel);
 
   private final Hood hood = new Hood();
   private final TurretSubsystem turretSubsystem= new TurretSubsystem(); 
 
-  public AutoChooser autoChooser = new AutoChooser(intakeSubsystem, driveTrain, shooterSubsystem, turretSubsystem, limeLightVision.getLimeLightVision(), hood);
+  public AutoChooser autoChooser = new AutoChooser(intakeSubsystem, driveTrain, shooterSubsystem, turretSubsystem, limeLightVision.getLimeLightVision(), hood, IMUSubsystem);
 
   private final Drive driveCommand = new Drive(driveTrain, () -> joyLeft.getY(), () -> joyRight.getY());
   private final TurretContinousTarget turretCommand= new TurretContinousTarget(turretSubsystem, limeLightVision.getLimeLightVision(), () -> -xboxController.getLeftX());
@@ -286,13 +288,13 @@ public void installCommandsOnShuffleboard() {
       SmartShuffleboard.putCommand("Miscellaneous", "Set LED On", new SetLEDOn());
       SmartShuffleboard.putCommand("Miscellaneous", "Set Pipeline to 0", new SetPipeline(0));
       SmartShuffleboard.putCommand("Miscellaneous", "Set Pipeline to 1", new SetPipeline(1));
-      SmartShuffleboard.putCommand("Turn", "Turn Degrees", new TurnDegrees(driveTrain, 90));
-      SmartShuffleboard.putCommand("Drive", "Turn 30 Degrees", new AutoTurnDegrees(driveTrain, 30));
-      SmartShuffleboard.putCommand("Drive", "Turn -30 Degrees", new AutoTurnDegrees(driveTrain, -30));
-      SmartShuffleboard.putCommand("Drive", "Turn 90 Degrees", new AutoTurnDegrees(driveTrain, 90));
-      SmartShuffleboard.putCommand("Drive", "Turn 120 Degrees", new AutoTurnDegrees(driveTrain, 120));
-      SmartShuffleboard.putCommand("Drive", "Turn 150 Degrees", new AutoTurnDegrees(driveTrain, 150));
-      SmartShuffleboard.putCommand("Drive", "Turn 180 Degrees", new AutoTurnDegrees(driveTrain, 180));
+      SmartShuffleboard.putCommand("Turn", "Turn Degrees", new TurnDegrees(driveTrain, 90, IMUSubsystem));
+      SmartShuffleboard.putCommand("Drive", "Turn 30 Degrees", new AutoTurnDegrees(driveTrain, 30, IMUSubsystem));
+      SmartShuffleboard.putCommand("Drive", "Turn -30 Degrees", new AutoTurnDegrees(driveTrain, -30, IMUSubsystem));
+      SmartShuffleboard.putCommand("Drive", "Turn 90 Degrees", new AutoTurnDegrees(driveTrain, 90, IMUSubsystem));
+      SmartShuffleboard.putCommand("Drive", "Turn 120 Degrees", new AutoTurnDegrees(driveTrain, 120, IMUSubsystem));
+      SmartShuffleboard.putCommand("Drive", "Turn 150 Degrees", new AutoTurnDegrees(driveTrain, 150, IMUSubsystem));
+      SmartShuffleboard.putCommand("Drive", "Turn 180 Degrees", new AutoTurnDegrees(driveTrain, 180, IMUSubsystem));
 
 
       SmartShuffleboard.putCommand("Hood", "Move Hood Down", new MoveHoodDown(hood));

src/main/java/frc/robot/commands/Autonomous/ThreeShotSequenceRight.java

@@ -12,6 +12,7 @@
 import frc.robot.commands.ShooterCommands.ShooterSequeunce;
 import frc.robot.subsystems.DriveTrain;
 import frc.robot.subsystems.Hood;
+import frc.robot.subsystems.IMUSubsystem;
 import frc.robot.subsystems.IntakeSubsystem;
 import frc.robot.subsystems.ShooterSubsystem;
 import frc.robot.subsystems.TurretSubsystem;
@@ -24,7 +25,7 @@ public class ThreeShotSequenceRight extends SequentialCommandGroup {
   /** Creates a new TwoShotSequenceRight. 
    * @param vision 
    * @param hoodSubsystem */
-  public ThreeShotSequenceRight(TurretSubsystem turretSubsystem, double turretSpeed, IntakeSubsystem intakeSubsystem, DriveTrain driveTrain, double speed, double distanceInches, ShooterSubsystem shooterSubsystem, LimeLightVision limeLightVision, Hood hood) {
+  public ThreeShotSequenceRight(TurretSubsystem turretSubsystem, double turretSpeed, IntakeSubsystem intakeSubsystem, DriveTrain driveTrain, double speed, double distanceInches, ShooterSubsystem shooterSubsystem, LimeLightVision limeLightVision, Hood hood, IMUSubsystem IMUSubsystem) {
     // Add your commands in the addCommands() call, e.g.
     // addCommands(new FooCommand(), new BarCommand());
     addCommands(
@@ -34,7 +35,7 @@ public ThreeShotSequenceRight(TurretSubsystem turretSubsystem, double turretSpee
       new ShooterSequeunce(shooterSubsystem, limeLightVision),
       new WaitCommand(0.8),
       new ShooterSequeunce(shooterSubsystem, limeLightVision),
-      new TurnDegrees(driveTrain, 105),
+      new TurnDegrees(driveTrain, 105, IMUSubsystem),
       new ParralelMoveAndIntake(driveTrain, speed, 60, turretSubsystem, turretSpeed, intakeSubsystem, hood, turretSubsystem),
 
       new AutoTargetSequence(turretSubsystem, limeLightVision, hood),

src/main/java/frc/robot/commands/DriveCommands/AutoTurnDegrees.java

@@ -4,7 +4,7 @@
 import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc.robot.Constants;
 import frc.robot.subsystems.DriveTrain;
-import frc.robot.utils.SmartShuffleboard;
+import frc.robot.subsystems.IMUSubsystem;
 
 public class AutoTurnDegrees extends CommandBase{
 
@@ -13,22 +13,25 @@ public class AutoTurnDegrees extends CommandBase{
     private double startTime;
     private double startDegrees;
     private double turnSpeed;
+    private IMUSubsystem IMUSubsystem;
 
-    public AutoTurnDegrees(DriveTrain driveTrain, double angleToTurn) {
+    public AutoTurnDegrees(DriveTrain driveTrain, double angleToTurn, IMUSubsystem IMUSubsystem) {
         this.driveTrain = driveTrain;
         this.turnDegrees = angleToTurn;
+        this.IMUSubsystem = IMUSubsystem;
         addRequirements(driveTrain);
+        addRequirements(IMUSubsystem);
     }
 
     @Override
     public void initialize() {
         this.startTime = Timer.getFPGATimestamp();
-        this.startDegrees = driveTrain.getAngle();
+        this.startDegrees = IMUSubsystem.getAngle();
     }
 
     @Override
     public void execute() {
-        double rawError = (startDegrees + turnDegrees - driveTrain.getAngle());
+        double rawError = (startDegrees + turnDegrees - IMUSubsystem.getAngle());
         double error = Math.abs(rawError);
         double direction = Math.signum(rawError);
         if (error > Constants.AUTO_MOVE_TURN_SLOWDOWN_ERROR) {
@@ -50,7 +53,7 @@ public boolean isFinished() {
         if (Timer.getFPGATimestamp() - startTime >= Constants.AUTO_MOVE_TURN_TIMEOUT) {
             return true;
         }
-        double error = Math.abs(startDegrees + turnDegrees - driveTrain.getAngle());
+        double error = Math.abs(startDegrees + turnDegrees - IMUSubsystem.getAngle());
         return (error <= Constants.AUTO_MOVE_TURN_THRESHOLD);
     }
 

src/main/java/frc/robot/commands/DriveCommands/TurnDegrees.java

@@ -4,6 +4,7 @@
 import frc.robot.Constants;
 import frc.robot.commands.LoggedCommandBase;
 import frc.robot.subsystems.DriveTrain;
+import frc.robot.subsystems.IMUSubsystem;
 import frc.robot.utils.SmartShuffleboard;
 
 
@@ -14,16 +15,19 @@ public class TurnDegrees extends LoggedCommandBase{
     private final int SLOWDOWN_ANGLE = 45;
     private final double MAXIMUM_TIME_S = 10;
     private DriveTrain driveTrain;
+    private IMUSubsystem IMUSubsystem;
     private double requiredAngle;
     private double currAngle;
     private double speed = 0.0;
     private double startTime;
 
 
-    public TurnDegrees(DriveTrain driveTrain, int requiredAngle){
+    public TurnDegrees(DriveTrain driveTrain, int requiredAngle, IMUSubsystem IMUSubsystem){
         this.driveTrain = driveTrain;
         this.requiredAngle = requiredAngle;
+        this.IMUSubsystem = IMUSubsystem;
         addRequirements(driveTrain);
+        addRequirements(IMUSubsystem);
         addLog(requiredAngle);
     }
 
@@ -32,7 +36,7 @@ public void initialize(){
     }
 
     public void execute(){
-        currAngle = driveTrain.getAngle();
+        currAngle = IMUSubsystem.getAngle();
         double angleError = requiredAngle-currAngle;
 
         if (Math.abs(angleError) <= ANGLE_THRESHOLD) {

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

@@ -5,7 +5,6 @@
 import com.revrobotics.CANSparkMaxLowLevel.MotorType;
 import com.revrobotics.RelativeEncoder;
 
-import edu.wpi.first.wpilibj.ADIS16470_IMU;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants;
 import frc.robot.Robot;
@@ -19,16 +18,14 @@ public class DriveTrain extends SubsystemBase {
     private CANSparkMax right2;
     private RelativeEncoder leftEncoder;
     private RelativeEncoder rightEncoder;
-
-    private final ADIS16470_IMU imu;
+    private IMUSubsystem IMUSubsystem;
 
     public DriveTrain(){
         left1 = new CANSparkMax(Constants.DRIVE_LEFT1_ID, MotorType.kBrushless);
         left2 = new CANSparkMax(Constants.DRIVE_LEFT2_ID, MotorType.kBrushless);
         right1 = new CANSparkMax(Constants.DRIVE_RIGHT1_ID, MotorType.kBrushless);
         right2 = new CANSparkMax(Constants.DRIVE_RIGHT2_ID, MotorType.kBrushless);
 
-        imu = new ADIS16470_IMU();
 
         left1.restoreFactoryDefaults();
         left2.restoreFactoryDefaults();
@@ -53,7 +50,7 @@ public DriveTrain(){
         right1.setIdleMode(IdleMode.kBrake);
         right2.setIdleMode(IdleMode.kBrake);
 
-        resetGyro();
+        IMUSubsystem.resetGyro();
 
 
         Robot.getDiagnostics().addDiagnosable(new DiagSparkMaxEncoder("Left Drive Encoder", 10, left1));
@@ -72,35 +69,12 @@ public void drive(double speedLeft, double speedRight, boolean isSquared) {
           right1.set(speedRight);
     }
 
-      /**
-   * Resets the Gyro
-   */
-    public void resetGyro() {
-        imu.reset();
-        imu.calibrate();
-    }
-
-      /**
-   * Gets the angle of the robot
-   * 
-   * @return angle of robot between -180-180
-   */
-    public double getAngle() {
-        return imu.getAngle();
-    }
-
     @Override
     public void periodic() {
         // This method will be called once per scheduler run
         if (Constants.ENABLE_DEBUG) {
             SmartShuffleboard.put("Drive", "Encoders", "L", getLeftEncoder());
             SmartShuffleboard.put("Drive", "Encoders", "R", getRightEncoder());
-            SmartShuffleboard.put("Drive", "Gyro", "Angle", getAngle());
-            SmartShuffleboard.put("Drive", "Gyro", "Raw Angle", imu.getAngle());
-            SmartShuffleboard.put("Drive", "Gyro", "X Comp Angle", imu.getXComplementaryAngle());
-            SmartShuffleboard.put("Drive", "Gyro", "Y Comp Angle", imu.getYComplementaryAngle());
-            SmartShuffleboard.put("Drive", "Gyro", "X filtered acceleration angle", imu.getXFilteredAccelAngle());
-            SmartShuffleboard.put("Drive", "Gyro", "Y filtered acceleration angle", imu.getYFilteredAccelAngle());
          }
     }
 

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

@@ -0,0 +1,61 @@
+// 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 edu.wpi.first.wpilibj.ADIS16470_IMU;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants;
+import frc.robot.utils.SmartShuffleboard;
+
+public class IMUSubsystem extends SubsystemBase {
+  /** Creates a new IMUSubststem. */
+  private ADIS16470_IMU gyro;
+
+  public IMUSubsystem() {
+    gyro = new ADIS16470_IMU();
+  }
+
+  // add @params Please do this before a competition or while the robot is
+  // perfectly still
+  public void resetGyro() {
+    gyro.reset();
+    gyro.calibrate();
+  }
+
+  public double getAccelY() {
+    return gyro.getAccelY();
+  }
+
+  public double getAccelX() {
+    return gyro.getAccelX();
+  }
+
+  public double getAccelZ() {
+    return gyro.getAccelZ();
+  }
+
+  public double getAngle() {
+    return gyro.getAngle();
+  }
+
+  public ADIS16470_IMU getGyro() {
+    return gyro;
+  }
+
+  public void name() {
+
+  }
+
+  @Override
+  public void periodic() {
+    // This method will be called once per scheduler run
+    if (Constants.ENABLE_DEBUG) {
+      SmartShuffleboard.put("Gyro", "Angle", getAngle());
+      SmartShuffleboard.put("Gyro", "Z accelerated angle", getAccelZ());
+      SmartShuffleboard.put("Gyro", "Y accelerated angle", getAccelY());
+      SmartShuffleboard.put("Gyro", "X accelerated angle", getAccelX());
+   }
+  }
+}