Reset all subsystems on teleop init (#24)

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. */

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.
    *

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.
    *

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:

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: