diff --git a/Robot2024/src/main/java/com/team2813/RobotContainer.java b/Robot2024/src/main/java/com/team2813/RobotContainer.java index a4179e0..2fae95c 100644 --- a/Robot2024/src/main/java/com/team2813/RobotContainer.java +++ b/Robot2024/src/main/java/com/team2813/RobotContainer.java @@ -14,14 +14,20 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import com.team2813.commands.DefaultDriveCommand; +import com.team2813.commands.DefaultShooterCommand; import com.team2813.subsystems.Drive; +import com.team2813.subsystems.Shooter; + import static com.team2813.Constants.*; import static com.team2813.Constants.DriverConstants.*; +import static com.team2813.Constants.OperatorConstants.*; public class RobotContainer { private final SendableChooser autoChooser; private final Drive drive = new Drive(); + private final Shooter shooter = new Shooter(); private final XboxController driverController = new XboxController(driverControllerPort); + private final XboxController operatorController = new XboxController(operatorControllerPort); public RobotContainer() { drive.setDefaultCommand(new DefaultDriveCommand( () -> -modifyAxis(driverController.getLeftY()) * Drive.MAX_VELOCITY, @@ -29,6 +35,7 @@ public RobotContainer() { () -> -modifyAxis(driverController.getRightX()) * Drive.MAX_ANGULAR_VELOCITY, drive )); + shooter.setDefaultCommand(new DefaultShooterCommand(shooter, operatorController::getRightY)); configureBindings(); autoChooser = AutoBuilder.buildAutoChooser(); SmartDashboard.putData("Auto", autoChooser); diff --git a/Robot2024/src/main/java/com/team2813/commands/DefaultShooterCommand.java b/Robot2024/src/main/java/com/team2813/commands/DefaultShooterCommand.java new file mode 100644 index 0000000..dd7c3e2 --- /dev/null +++ b/Robot2024/src/main/java/com/team2813/commands/DefaultShooterCommand.java @@ -0,0 +1,34 @@ +package com.team2813.commands; + +import java.util.function.DoubleSupplier; + +import com.team2813.lib2813.control.ControlMode; +import com.team2813.subsystems.Shooter; + +import edu.wpi.first.wpilibj2.command.Command; + +public class DefaultShooterCommand extends Command { + private Shooter shooter; + private DoubleSupplier controll; + public DefaultShooterCommand(Shooter shooter, DoubleSupplier controll) { + this.shooter = shooter; + this.controll = controll; + } + /** + * The number to multiply results from {@link #controll}. Multiplies by {@value #MULTIPLIER} + */ + private static final double MULTIPLIER = 1; + private static final double DEADZONE = 0.01; + /** + * Sets the motor to the value from the {@link DoubleSupplier} {@link #controll}. + * Multiplies the result by {@value #MULTIPLIER}. The return from {@link #controll} MUST be less than or equal to 1. + */ + public void excecute() { + double val = controll.getAsDouble(); + if (Math.abs(val) > DEADZONE) { + shooter.set(ControlMode.DUTY_CYCLE, val * MULTIPLIER); + } else { + shooter.set(ControlMode.DUTY_CYCLE, 0); + } + } +} diff --git a/Robot2024/src/main/java/com/team2813/subsystems/Shooter.java b/Robot2024/src/main/java/com/team2813/subsystems/Shooter.java index f790f9c..46ff2e1 100644 --- a/Robot2024/src/main/java/com/team2813/subsystems/Shooter.java +++ b/Robot2024/src/main/java/com/team2813/subsystems/Shooter.java @@ -1,6 +1,8 @@ package com.team2813.subsystems; +import com.team2813.lib2813.control.ControlMode; import com.team2813.lib2813.control.Motor; +import com.team2813.lib2813.control.encoders.CancoderWrapper; import com.team2813.lib2813.control.motors.TalonFXWrapper; import com.team2813.lib2813.subsystems.MotorSubsystem; import static com.team2813.Constants.*; @@ -13,13 +15,21 @@ public Shooter() { // TODO: fix invert type super(new MotorSubsystemConfiguration( new TalonFXWrapper(SHOOTER_PIVOT, TalonFXInvertType.Clockwise), - null //TODO: write encoder class (embarassing wow) + new CancoderWrapper(SHOOTER_ENCODER) )); TalonFXWrapper m = new TalonFXWrapper(SHOOTER_1, TalonFXInvertType.Clockwise); m.addFollower(SHOOTER_2, TalonFXInvertType.OpposeMaster); shooterMotor = m; setSetpoint(Angle.TEST); } + public void stop() { + shooterMotor.set(ControlMode.DUTY_CYCLE, 0); + } + + public void run(double demand) { + shooterMotor.set(ControlMode.DUTY_CYCLE, demand); + } + public static enum Angle implements MotorSubsystem.Position { TEST(0.0); private final double pos;