Merge remote-tracking branch 'origin/main' into swerve

src/main/java/frc/robot/Constants.java

@@ -98,6 +98,11 @@ public static final class DriveConstants {
     public static final double kPHeadingCorrectionController = 5;
   }
 
+  public static final class ShooterConstants {
+    public static final int kTopShooterMotorPort = 35;
+    public static final int kBottomShooterMotorPort = 20;
+  }
+
   public static final class VisionConstants {
     // TODO: Update cam pose relative to center of bot
     public static final Pose3d kCamPose = new Pose3d(0, 0, 0, new Rotation3d(0, 0, 0));

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

@@ -15,6 +15,7 @@
 import frc.robot.Constants.DriveConstants;
 import frc.robot.Constants.IOConstants;
 import frc.robot.subsystems.DriveSubsystem;
+import frc.robot.subsystems.ShooterSubsystem;
 
 /*
  * This class is where the bulk of the robot should be declared.  Since Command-based is a
@@ -25,6 +26,7 @@
 public class RobotContainer {
   // The robot's subsystems and commands are defined here
   private final DriveSubsystem m_robotDrive = new DriveSubsystem();
+  public final ShooterSubsystem m_shooterSubsystem = new ShooterSubsystem();
 
   private final XboxController m_driverController = new XboxController(IOConstants.kDriverControllerPort);
 
@@ -72,6 +74,14 @@ public RobotContainer() {
   private void configureBindings() {
     new JoystickButton(m_driverController, Button.kStart.value)
         .onTrue(new InstantCommand(m_robotDrive::zeroHeading, m_robotDrive));
+
+    // TODO: Move shoot commands to operator controller
+    new JoystickButton(m_driverController, Button.kB.value)
+        .onTrue(new InstantCommand(() -> m_shooterSubsystem.spin(0.75), m_shooterSubsystem))
+        .onFalse(new InstantCommand(() -> m_shooterSubsystem.spin(0), m_shooterSubsystem));
+    new JoystickButton(m_driverController, Button.kA.value)
+        .onTrue(new InstantCommand(() -> m_shooterSubsystem.spin(-0.75), m_shooterSubsystem))
+        .onFalse(new InstantCommand(() -> m_shooterSubsystem.spin(0), m_shooterSubsystem));
   }
 
   /**

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

@@ -0,0 +1,32 @@
+// 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.revrobotics.CANSparkFlex;
+import com.revrobotics.CANSparkLowLevel.MotorType;
+
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants.ShooterConstants;
+
+public class ShooterSubsystem extends SubsystemBase {
+  /** Creates a new ShooterSubsystem. */
+  CANSparkFlex m_bottom = new CANSparkFlex(ShooterConstants.kBottomShooterMotorPort, MotorType.kBrushless);
+  CANSparkFlex m_top = new CANSparkFlex(ShooterConstants.kTopShooterMotorPort, MotorType.kBrushless);
+
+  public ShooterSubsystem() {
+
+  }
+
+  public void spin(double speed) {
+    m_bottom.set(speed);
+    m_top.set(speed);
+  }
+
+  @Override
+  public void periodic() {
+    // This method will be called once per scheduler run
+    // SmartDashboard.putNumber("Speed", m_bottom.);
+  }
+}

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

@@ -112,7 +112,7 @@ public void setDesiredState(SwerveModuleState desiredState) {
    * @param encoder The encoder to get the absolute angle of.
    * @return A Rotation2d of the absolute angle.
    */
-  public Rotation2d getEncoderAngle(CANcoder encoder) {
+  public static Rotation2d getEncoderAngle(CANcoder encoder) {
     return new Rotation2d(encoder.getAbsolutePosition().getValueAsDouble() * 2 * Math.PI);
   }
 }