diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 7d56e02..aa4bfd6 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/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)); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 1fa8833..c19c224 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/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)); } /** diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java new file mode 100644 index 0000000..afdbd19 --- /dev/null +++ b/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.); + } +} diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java index f4de950..9d98ea8 100644 --- a/src/main/java/frc/robot/subsystems/SwerveModule.java +++ b/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); } }