diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index de2c9d09..a53481b9 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,15 +4,29 @@ package frc.robot; +import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.commands.gizmo.GizmoCommand; +import frc.robot.subsystems.gizmo.GizmoSimple; public class RobotContainer { + private static final XboxController controller = new XboxController(0); + private static final GizmoSimple gizmo = new GizmoSimple(); + public RobotContainer() { configureBindings(); } - private void configureBindings() {} + private void configureBindings() { + gizmo.setDefaultCommand( + new GizmoCommand( + gizmo, + () -> controller.getPOV() == 270, + () -> controller.getPOV() == 90, + () -> controller.getLeftBumper(), + () -> controller.getRightBumper())); + } public Command getAutonomousCommand() { return Commands.print("No autonomous command configured"); diff --git a/src/main/java/frc/robot/commands/gizmo/GizmoCommand.java b/src/main/java/frc/robot/commands/gizmo/GizmoCommand.java new file mode 100644 index 00000000..d08aed4d --- /dev/null +++ b/src/main/java/frc/robot/commands/gizmo/GizmoCommand.java @@ -0,0 +1,64 @@ +package frc.robot.commands.gizmo; + +import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.gizmo.GizmoSimple; +import java.util.function.BooleanSupplier; + +public class GizmoCommand extends Command { + private GizmoSimple gizmo; + private BooleanSupplier decreaseSpeed; + private BooleanSupplier increaseSpeed; + private BooleanSupplier runBackward; + private BooleanSupplier runForward; + private double currentSpeed = 0.0; + private double minSpeed = 0.0; + private double maxSpeed = 1.0; + private double speedIncrement = 0.01; + + public GizmoCommand( + GizmoSimple gizmo, + BooleanSupplier decreaseSpeed, + BooleanSupplier increaseSpeed, + BooleanSupplier runBackward, + BooleanSupplier runForward) { + this.gizmo = gizmo; + this.decreaseSpeed = decreaseSpeed; + this.increaseSpeed = increaseSpeed; + this.runBackward = runBackward; + this.runForward = runForward; + + addRequirements(gizmo); + } + + public void execute() { + + /* Check if speed needs to be changed */ + if (decreaseSpeed.getAsBoolean()) { + if (currentSpeed >= speedIncrement) { + currentSpeed -= speedIncrement; + } else { + currentSpeed = minSpeed; + } + } else if (increaseSpeed.getAsBoolean()) { + if (currentSpeed <= (maxSpeed - speedIncrement)) { + currentSpeed += speedIncrement; + } else { + currentSpeed = maxSpeed; + } + } + + /* Check if motor needs to be turned on */ + if (runForward.getAsBoolean()) { + gizmo.setVoltage(currentSpeed * RobotController.getBatteryVoltage()); + } else if (runBackward.getAsBoolean()) { + gizmo.setVoltage(-currentSpeed * RobotController.getBatteryVoltage()); + } else { + gizmo.setVoltage(0); + } + } + + public void end(boolean interrupted) { + gizmo.setVoltage(0); + } +} diff --git a/src/main/java/frc/robot/subsystems/gizmo/Gizmo.java b/src/main/java/frc/robot/subsystems/gizmo/Gizmo.java new file mode 100644 index 00000000..f16d23b3 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/gizmo/Gizmo.java @@ -0,0 +1,11 @@ +package frc.robot.subsystems.gizmo; + +public interface Gizmo { + /* Open Loop Controls */ + /** + * Sets open loop voltage to send to motor + * + * @param volts voltage to send to the motor Typically in the range [-12.0..12.0] + */ + public void setVoltage(double volts); +} diff --git a/src/main/java/frc/robot/subsystems/gizmo/GizmoSimple.java b/src/main/java/frc/robot/subsystems/gizmo/GizmoSimple.java index f6348ad8..00f6c7a8 100644 --- a/src/main/java/frc/robot/subsystems/gizmo/GizmoSimple.java +++ b/src/main/java/frc/robot/subsystems/gizmo/GizmoSimple.java @@ -1,8 +1,17 @@ package frc.robot.subsystems.gizmo; +import edu.wpi.first.wpilibj.motorcontrol.Spark; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import org.littletonrobotics.junction.AutoLogOutput; -public class GizmoSimple extends SubsystemBase { - - +public class GizmoSimple extends SubsystemBase implements Gizmo { + /** Spark PWM Motor is connected to RIO PWM Port 0 */ + private static final Spark motor = new Spark(0); + @AutoLogOutput private double volts = 0; + + /* Control */ + public void setVoltage(double volts) { + this.volts = volts; + motor.setVoltage(volts); + } }