From 7e9f0f9a46e315bf8289e9aefaaa03013e691122 Mon Sep 17 00:00:00 2001
From: Programming SR <85456157+ProgrammingSR@users.noreply.github.com>
Date: Sun, 3 Mar 2024 17:13:31 -0800
Subject: [PATCH] Reset all subsystems on teleop init (#24)

---
 src/main/java/frc/robot/Robot.java                       | 2 ++
 src/main/java/frc/robot/RobotContainer.java              | 9 +++++++++
 src/main/java/frc/robot/subsystems/DriveSubsystem.java   | 4 ++++
 src/main/java/frc/robot/subsystems/IntakeSubsystem.java  | 8 ++++++++
 src/main/java/frc/robot/subsystems/ShooterSubsystem.java | 8 ++++++++
 5 files changed, 31 insertions(+)

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index bf49561..3dadca9 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -94,6 +94,8 @@ public void teleopInit() {
     if (m_autonomousCommand != null) {
       m_autonomousCommand.cancel();
     }
+
+    m_robotContainer.resetAllSubsystems();
   }
 
   /** This function is called periodically during operator control. */
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 44a7291..7500be3 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -147,6 +147,15 @@ private void configureBindings() {
         .onFalse(new IntakeArmPositionCommand(m_intakeSubsystem, ArmPosition.Retracted));
   }
 
+  /** 
+   * Reset all subsystems on teleop init
+   */
+  public void resetAllSubsystems() {
+    m_intakeSubsystem.reset();
+    m_shooterSubsystem.reset();
+    m_robotDrive.reset();
+  }
+
   /**
    * Use this to pass the autonomous command to the main {@link Robot} class.
    *
diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java
index dc822ef..ac3160c 100644
--- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java
@@ -110,6 +110,10 @@ public void periodic() {
     setModuleStates(swerveModuleStates);
   }
 
+  public void reset() {
+    swerveModuleStates = new SwerveModuleState[] { new SwerveModuleState(), new SwerveModuleState(), new SwerveModuleState(), new SwerveModuleState() };
+  }
+
   /**
    * Returns the currently-estimated pose of the robot.
    *
diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java
index 70600a7..8ed0a45 100644
--- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java
@@ -48,6 +48,14 @@ public IntakeSubsystem() {
     m_armSetpoint = m_armEncoder.getDistance();
   }
 
+  public void reset() {
+    m_intakeMotor.set(0);
+    m_armMotor.set(0);
+
+    m_intakeSpeed = 0;
+    m_armSetpoint = getDistanceSensor();
+  }
+
   public void setArmPosition(ArmPosition position) {
     switch (position) {
       case Amp:
diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java
index 788d265..251acbf 100644
--- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java
@@ -28,6 +28,14 @@ public ShooterSubsystem() {
     m_top.setInverted(true);
   }
 
+  public void reset() {
+    m_topSpeed = 0;
+    m_bottomSpeed = 0;
+
+    m_top.set(0);
+    m_bottom.set(0);
+  }
+
   public void setShootingSpeed(ShootSpeed speed) {
     switch (speed) {
       case Shooting: