diff --git a/src/main/java/org/jmhsrobotics/frc2024/RobotContainer.java b/src/main/java/org/jmhsrobotics/frc2024/RobotContainer.java index f587c41d..72b7e97f 100644 --- a/src/main/java/org/jmhsrobotics/frc2024/RobotContainer.java +++ b/src/main/java/org/jmhsrobotics/frc2024/RobotContainer.java @@ -10,6 +10,8 @@ import org.jmhsrobotics.frc2024.subsystems.drive.DriveSubsystem; import org.jmhsrobotics.frc2024.subsystems.drive.commands.DriveCommand; import org.jmhsrobotics.frc2024.subsystems.drive.commands.IntakeCommand; +import org.jmhsrobotics.frc2024.subsystems.shooter.ShooterSubsystem; +import org.jmhsrobotics.frc2024.subsystems.shooter.shooterCommands.ShooterCommand; import org.jmhsrobotics.frc2024.subsystems.vision.VisionSubsystem; import com.pathplanner.lib.auto.AutoBuilder; @@ -35,6 +37,8 @@ public class RobotContainer { private final VisionSubsystem vision = new VisionSubsystem(this.driveSubsystem); + private final ShooterSubsystem shooterSubsystem = new ShooterSubsystem(); + private final SendableChooser autoChooser; public RobotContainer() { @@ -47,6 +51,8 @@ public RobotContainer() { configurePathPlanner(); autoChooser = AutoBuilder.buildAutoChooser(); SmartDashboard.putData("Auto Chooser", autoChooser); + ShooterCommand shooterCommand = new ShooterCommand(2000, shooterSubsystem); + SmartDashboard.putData("Shooter Command", shooterCommand); } private void configurePathPlanner() { diff --git a/src/main/java/org/jmhsrobotics/frc2024/subsystems/shooter/ShooterSubsystem.java b/src/main/java/org/jmhsrobotics/frc2024/subsystems/shooter/ShooterSubsystem.java index c2534919..de878eaf 100644 --- a/src/main/java/org/jmhsrobotics/frc2024/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/org/jmhsrobotics/frc2024/subsystems/shooter/ShooterSubsystem.java @@ -1,12 +1,16 @@ package org.jmhsrobotics.frc2024.subsystems.shooter; import com.revrobotics.CANSparkMax; +import com.revrobotics.RelativeEncoder; import com.revrobotics.CANSparkBase.IdleMode; +import com.revrobotics.CANSparkLowLevel.MotorType; + import edu.wpi.first.wpilibj2.command.SubsystemBase; public class ShooterSubsystem extends SubsystemBase { - private CANSparkMax motor1; - private CANSparkMax motor2; + private CANSparkMax motor1 = new CANSparkMax(7, MotorType.kBrushless); + private CANSparkMax motor2 = new CANSparkMax(8, MotorType.kBrushless);; + private RelativeEncoder encoder; // private double speed; @@ -26,6 +30,11 @@ private void initializeMotors() { motor2.follow(motor1); } + public double getRPM() { + encoder = motor1.getEncoder(); + return encoder.getVelocity(); + } + public void setSpeed(double speed) { motor1.set(speed); } diff --git a/src/main/java/org/jmhsrobotics/frc2024/subsystems/shooter/shooterCommands/ShooterCommand.java b/src/main/java/org/jmhsrobotics/frc2024/subsystems/shooter/shooterCommands/ShooterCommand.java index 6ae4b777..eefd2021 100644 --- a/src/main/java/org/jmhsrobotics/frc2024/subsystems/shooter/shooterCommands/ShooterCommand.java +++ b/src/main/java/org/jmhsrobotics/frc2024/subsystems/shooter/shooterCommands/ShooterCommand.java @@ -1,23 +1,42 @@ package org.jmhsrobotics.frc2024.subsystems.shooter.shooterCommands; import org.jmhsrobotics.frc2024.subsystems.shooter.ShooterSubsystem; -import org.jmhsrobotics.frc2024.controlBoard.ControlBoard; + +import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.wpilibj2.command.Command; public class ShooterCommand extends Command { - private ShooterSubsystem shooterSubsystem; - private ControlBoard control; + private ShooterSubsystem shooter; + private double targetRPM; + private PIDController pid; + + public ShooterCommand(double targetRPM, ShooterSubsystem shooterSubsystem) { + this.shooter = shooterSubsystem; + this.targetRPM = targetRPM; + pid = new PIDController(.1, 0, 0); + + addRequirements(this.shooter); + } + + @Override + public void initialize() { + pid.reset(); + pid.setSetpoint(targetRPM); + } - public ShooterCommand(ShooterSubsystem shooterSubsystem, ControlBoard control) { - this.shooterSubsystem = shooterSubsystem; - this.control = control; + @Override + public boolean isFinished() { + return false; + } - addRequirements(this.shooterSubsystem); + @Override + public void end(boolean interrupted) { + shooter.setSpeed(0); } @Override public void execute() { - double motorSpeed = control.shooterInput(); - shooterSubsystem.setSpeed(motorSpeed); + double motorSpeed = pid.calculate(shooter.getRPM(), targetRPM); + shooter.setSpeed(motorSpeed); } }