From 1d0509a34adb3cb67687d0dccb9a2593d6a90196 Mon Sep 17 00:00:00 2001 From: Jacob Hotz Date: Wed, 24 Jan 2024 13:22:12 -0800 Subject: [PATCH 1/4] remove elevator command and rename handoff to piece control --- .../frc/robot/commands/ElevatorCommand.java | 57 ------------------ .../{Handoff.java => PieceControl.java} | 58 ++++++++++--------- 2 files changed, 32 insertions(+), 83 deletions(-) delete mode 100644 src/main/java/frc/robot/commands/ElevatorCommand.java rename src/main/java/frc/robot/commands/handoff/{Handoff.java => PieceControl.java} (61%) diff --git a/src/main/java/frc/robot/commands/ElevatorCommand.java b/src/main/java/frc/robot/commands/ElevatorCommand.java deleted file mode 100644 index 97e2ea2b..00000000 --- a/src/main/java/frc/robot/commands/ElevatorCommand.java +++ /dev/null @@ -1,57 +0,0 @@ -// 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.commands; - -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.ElevatorSubs.Claw; -import frc.robot.subsystems.ElevatorSubs.Elevator; -import edu.wpi.first.wpilibj2.command.Commands; -import frc.robot.util.Constants.TrapConstants; - -// TODO: convert to placement? -public class ElevatorCommand { - /** Creates a new Elevator. */ - private final Elevator elevator; - private final Claw claw; - - public ElevatorCommand(Elevator elevator, Claw claw) { - - this.elevator = elevator; - this.claw = claw; - } - - public Command placeTrapCommand() { - return Commands.runOnce( - () -> this.elevator.setPositionCommand(TrapConstants.TRAP_PLACE_POS)).andThen( - Commands.waitUntil(elevator.isAtTargetPosition())) - .andThen(claw.placeCommand()); - } - - public Command intakeFromHandoff() { - return (claw.intakeFromHandoff()); - } - - public void setTargetPosition(double pos) { - this.elevator.setPosition(pos); - } - - public Command outtake() { - return this.claw.outtake(); - } - - public Command intake() { - return this.claw.intake(); - } - - public Command stopClaw() { - return this.claw.stop(); - } - - public Command stopMotors() { - return Commands.parallel( - elevator.stop(), - claw.stop()); - } -} diff --git a/src/main/java/frc/robot/commands/handoff/Handoff.java b/src/main/java/frc/robot/commands/handoff/PieceControl.java similarity index 61% rename from src/main/java/frc/robot/commands/handoff/Handoff.java rename to src/main/java/frc/robot/commands/handoff/PieceControl.java index b554be8f..4b5c2539 100644 --- a/src/main/java/frc/robot/commands/handoff/Handoff.java +++ b/src/main/java/frc/robot/commands/handoff/PieceControl.java @@ -7,31 +7,36 @@ import frc.robot.subsystems.Indexer; import frc.robot.subsystems.Intake; import frc.robot.subsystems.Swerve; -import frc.robot.commands.ElevatorCommand; +import frc.robot.subsystems.ElevatorSubs.Claw; +import frc.robot.subsystems.ElevatorSubs.Elevator; +import frc.robot.util.Constants.TrapConstants; import frc.robot.commands.ShooterCalc; -public class Handoff { +public class PieceControl { private final Command emptyCommand = null; private NotePosition notePosition = NotePosition.NONE; private Intake intake; private Indexer indexer; - - private ElevatorCommand elevatorCommand; + + private Elevator elevator; + private Claw claw; + private ShooterCalc shooterCalc; private Swerve swerve; - public Handoff( - Intake intake, + public PieceControl( + Intake intake, Indexer indexer, - ElevatorCommand elevatorCommand, + Elevator elevator, + Claw claw, ShooterCalc shooterCalc, - Swerve swerve) - { + Swerve swerve) { this.intake = intake; this.indexer = indexer; - this.elevatorCommand = elevatorCommand; + this.elevator = elevator; + this.claw = claw; this.shooterCalc = shooterCalc; this.swerve = swerve; } @@ -42,24 +47,23 @@ public Trigger readyToShoot() { public Command stopAllMotors() { return Commands.parallel( - intake.stop(), - indexer.stop(), - shooterCalc.stopMotors(), - elevatorCommand.stopMotors() - ); + intake.stop(), + indexer.stop(), + shooterCalc.stopMotors(), + elevator.stop(), + claw.stop()); } // TODO: We assume that the note is already in the claw when we want to shoot - public Command noteToShoot(){ + public Command noteToShoot() { // this.notePosition = NotePosition.CLAW; ^^^ Command shoot = emptyCommand; this.shooterCalc.prepareFireMovingCommand(() -> true, swerve); - if ( this.readyToShoot().getAsBoolean() ) { + if (this.readyToShoot().getAsBoolean()) { // run the indexer and intake to make sure the note gets to the shooter - shoot = - indexer.toShooter() + shoot = indexer.toShooter() .andThen(intake.inCommand()) - .andThen(elevatorCommand.outtake()) + .andThen(claw.outtake()) .andThen(() -> this.notePosition = NotePosition.SHOOTER) .andThen(Commands.waitSeconds(1)) // TODO: Change this to a wait until the note is in the shooter? .andThen(() -> this.notePosition = NotePosition.NONE) @@ -73,12 +77,14 @@ public Command noteToShoot(){ public Command noteToTrap() { Command shoot = emptyCommand; - if ( this.notePosition == NotePosition.CLAW ) { - // maybe make setPosition a command ORR Make the Elevator Command - shoot = - elevatorCommand.placeTrapCommand() - .andThen(new WaitCommand(1)) - .andThen(stopAllMotors()); + if (this.notePosition == NotePosition.CLAW) { + // maybe make setPosition a command ORR Make the Elevator Command + shoot = Commands.runOnce( + () -> this.elevator.setPositionCommand(TrapConstants.TRAP_PLACE_POS)).andThen( + Commands.waitUntil(elevator.isAtTargetPosition())) + .andThen(claw.placeCommand()) + .andThen(new WaitCommand(1)) + .andThen(stopAllMotors()); this.notePosition = NotePosition.CLAW; From 91cda6e32308d270d30b48de7905e48dd6fb57f8 Mon Sep 17 00:00:00 2001 From: PatribotsProgramming Date: Wed, 24 Jan 2024 14:04:01 -0800 Subject: [PATCH 2/4] rename Trigger to Indexer to prep for PR to #27 Co-authored-by: Alexander Hamilton --- src/main/java/frc/robot/RobotContainer.java | 4 ++-- .../frc/robot/subsystems/{TriggerWheel.java => Indexer.java} | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) rename src/main/java/frc/robot/subsystems/{TriggerWheel.java => Indexer.java} (96%) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index d807e4aa..60ba028f 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -31,7 +31,7 @@ public class RobotContainer implements Logged { private final Intake intake; @SuppressWarnings("unused") private final DriverUI driverUI; - private final TriggerWheel triggerWheel; + private final Indexer triggerWheel; private final Climb climb; @@ -44,7 +44,7 @@ public RobotContainer() { climb = new Climb(); swerve = new Swerve(); driverUI = new DriverUI(); - triggerWheel = new TriggerWheel(); + triggerWheel = new Indexer(); swerve.setDefaultCommand(new Drive( swerve, diff --git a/src/main/java/frc/robot/subsystems/TriggerWheel.java b/src/main/java/frc/robot/subsystems/Indexer.java similarity index 96% rename from src/main/java/frc/robot/subsystems/TriggerWheel.java rename to src/main/java/frc/robot/subsystems/Indexer.java index deb3b0b5..9b83fc9c 100644 --- a/src/main/java/frc/robot/subsystems/TriggerWheel.java +++ b/src/main/java/frc/robot/subsystems/Indexer.java @@ -8,12 +8,12 @@ import frc.robot.util.Neo; import java.util.Optional; -public class TriggerWheel extends SubsystemBase { +public class Indexer extends SubsystemBase { private final Neo triggerWheel; private Optional whichWay; private BooleanSupplier hasPiece; - public TriggerWheel() { + public Indexer() { triggerWheel = new Neo(IntakeConstants.TRIGGER_WHEEL_CAN_ID); whichWay = Optional.empty(); hasPiece = () -> false; From c56d916a930332a27c21f35b7657e8066c61b20a Mon Sep 17 00:00:00 2001 From: PatribotsProgramming Date: Wed, 24 Jan 2024 14:26:44 -0800 Subject: [PATCH 3/4] refactor the trigger wheel to have a desiredSpeed... rather than an enum #30 --- .../java/frc/robot/subsystems/Indexer.java | 44 ++++++++----------- 1 file changed, 18 insertions(+), 26 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Indexer.java b/src/main/java/frc/robot/subsystems/Indexer.java index 3d299343..a29c497d 100644 --- a/src/main/java/frc/robot/subsystems/Indexer.java +++ b/src/main/java/frc/robot/subsystems/Indexer.java @@ -8,20 +8,14 @@ public class Indexer extends SubsystemBase { private final Neo triggerWheel; - private MotorRotation whichWay; + private double desiredSpeed; public Indexer() { triggerWheel = new Neo(IntakeConstants.TRIGGER_WHEEL_CAN_ID); - whichWay = MotorRotation.STOP; + desiredSpeed = 0; configMotor(); } - enum MotorRotation { - CLOCKWISE, - COUNTER_CLOCKWISE, - STOP - } - public void configMotor() { // See https://docs.revrobotics.com/sparkmax/operating-modes/control-interfaces triggerWheel.setSmartCurrentLimit(IntakeConstants.TRIGGER_WHEEL_STALL_CURRENT_LIMIT_AMPS, IntakeConstants.TRIGGER_WHEEL_FREE_CURRENT_LIMIT_AMPS); @@ -32,31 +26,29 @@ public void configMotor() { //sets brake mode triggerWheel.setBrakeMode(); } - - public MotorRotation getWhichWay() { - return whichWay; + + public double getDesiredSpeed() { + return desiredSpeed; + } + + public void setDesiredSpeed(double speed) { + desiredSpeed = speed; + triggerWheel.set(speed); + } + + public Command setSpeedCommand(double speed) { + return runOnce(() -> setDesiredSpeed(speed)); } public Command toShooter() { - whichWay = MotorRotation.CLOCKWISE; - return runOnce(() -> triggerWheel.set(IntakeConstants.SHOOTER_TRIGGER_WHEEL_SPEED)); + return setSpeedCommand(IntakeConstants.SHOOTER_TRIGGER_WHEEL_SPEED); } public Command toTrap() { - whichWay = MotorRotation.COUNTER_CLOCKWISE; - return runOnce(() -> triggerWheel.set(IntakeConstants.TRAP_TRIGGER_WHEEL_SPEED)); - } - - public Command stopCommand() { - whichWay = Optional.empty(); - return runOnce(() -> triggerWheel.set(IntakeConstants.STOP_SPEED)); - } - - public BooleanSupplier hasPiece() { - return this.hasPiece; + return setSpeedCommand(IntakeConstants.TRAP_TRIGGER_WHEEL_SPEED); } - public void setHasPiece(boolean hasPiece) { - this.hasPiece = () -> hasPiece; + public Command stopCommand() { + return setSpeedCommand(IntakeConstants.STOP_SPEED); } } \ No newline at end of file From 2a34c67e24ddc5e2412ab26746d6b8823448e6ba Mon Sep 17 00:00:00 2001 From: PatribotsProgramming Date: Wed, 24 Jan 2024 14:33:45 -0800 Subject: [PATCH 4/4] rename stopCommand to not cause error with pieceControl Co-authored-by: Alexander Hamilton --- src/main/java/frc/robot/subsystems/Indexer.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/Indexer.java b/src/main/java/frc/robot/subsystems/Indexer.java index a29c497d..62330fb6 100644 --- a/src/main/java/frc/robot/subsystems/Indexer.java +++ b/src/main/java/frc/robot/subsystems/Indexer.java @@ -48,7 +48,7 @@ public Command toTrap() { return setSpeedCommand(IntakeConstants.TRAP_TRIGGER_WHEEL_SPEED); } - public Command stopCommand() { + public Command stop() { return setSpeedCommand(IntakeConstants.STOP_SPEED); } } \ No newline at end of file