feat: elevator analog command

Author: Brandon-Harad

src/main/java/org/team1540/robot2024/RobotContainer.java

@@ -12,7 +12,12 @@
 import org.littletonrobotics.junction.networktables.LoggedDashboardNumber;
 import org.team1540.robot2024.commands.FeedForwardCharacterization;
 import org.team1540.robot2024.commands.SwerveDriveCommand;
+import org.team1540.robot2024.commands.elevator.ElevatorAnalog;
 import org.team1540.robot2024.subsystems.drive.*;
+import org.team1540.robot2024.subsystems.elevator.Elevator;
+import org.team1540.robot2024.subsystems.elevator.ElevatorIO;
+import org.team1540.robot2024.subsystems.elevator.ElevatorIOSim;
+import org.team1540.robot2024.subsystems.elevator.ElevatorIOTalonFX;
 import org.team1540.robot2024.subsystems.shooter.*;
 import org.team1540.robot2024.util.swerve.SwerveFactory;
 import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
@@ -29,6 +34,7 @@ public class RobotContainer {
     // Subsystems
     public final Drivetrain drivetrain;
     public final Shooter shooter;
+    public final Elevator elevator;
 
     // Controller
     public final CommandXboxController driver = new CommandXboxController(0);
@@ -56,6 +62,7 @@ public RobotContainer() {
                                 new ModuleIOTalonFX(SwerveFactory.getModuleMotors(SwerveConfig.BACK_LEFT, SwerveFactory.SwerveCorner.BACK_LEFT)),
                                 new ModuleIOTalonFX(SwerveFactory.getModuleMotors(SwerveConfig.BACK_RIGHT, SwerveFactory.SwerveCorner.BACK_RIGHT)));
                 shooter = new Shooter(new ShooterPivotIOTalonFX(), new FlywheelsIOTalonFX());
+                elevator = new Elevator(new ElevatorIOTalonFX());
                 break;
 
             case SIM:
@@ -68,6 +75,7 @@ public RobotContainer() {
                                 new ModuleIOSim(),
                                 new ModuleIOSim());
                 shooter = new Shooter(new ShooterPivotIOSim(), new FlywheelsIOSim());
+                elevator = new Elevator(new ElevatorIOSim());
                 break;
 
             default:
@@ -80,6 +88,7 @@ public RobotContainer() {
                                 new ModuleIO() {},
                                 new ModuleIO() {});
                 shooter = new Shooter(new ShooterPivotIO() {}, new FlywheelsIO() {});
+                elevator = new Elevator(new ElevatorIO() {});
                 break;
         }
 
@@ -109,6 +118,7 @@ public RobotContainer() {
      */
     private void configureButtonBindings() {
         drivetrain.setDefaultCommand(new SwerveDriveCommand(drivetrain, driver));
+        elevator.setDefaultCommand(new ElevatorAnalog(copilot, elevator));
         driver.x().onTrue(Commands.runOnce(drivetrain::stopWithX, drivetrain));
         driver.b().onTrue(
                 Commands.runOnce(

src/main/java/org/team1540/robot2024/commands/climb/ClimbSequence.java

@@ -3,7 +3,7 @@
 import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import org.team1540.robot2024.Constants.Elevator.ElevatorState;
-import org.team1540.robot2024.commands.ElevatorSetpointCommand;
+import org.team1540.robot2024.commands.elevator.ElevatorSetpointCommand;
 import org.team1540.robot2024.subsystems.elevator.Elevator;
 import org.team1540.robot2024.subsystems.fakesubsystems.Hooks;
 

src/main/java/org/team1540/robot2024/commands/climb/DeclimbSequence.java

@@ -2,7 +2,7 @@
 
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import org.team1540.robot2024.Constants.Elevator.ElevatorState;
-import org.team1540.robot2024.commands.ElevatorSetpointCommand;
+import org.team1540.robot2024.commands.elevator.ElevatorSetpointCommand;
 import org.team1540.robot2024.subsystems.elevator.Elevator;
 import org.team1540.robot2024.subsystems.fakesubsystems.Hooks;
 

src/main/java/org/team1540/robot2024/commands/climb/TrapAndClimbSequence.java

@@ -6,7 +6,7 @@
 import edu.wpi.first.wpilibj2.command.WaitCommand;
 import org.team1540.robot2024.Constants;
 import org.team1540.robot2024.Constants.Elevator.ElevatorState;
-import org.team1540.robot2024.commands.ElevatorSetpointCommand;
+import org.team1540.robot2024.commands.elevator.ElevatorSetpointCommand;
 import org.team1540.robot2024.subsystems.elevator.Elevator;
 import org.team1540.robot2024.subsystems.fakesubsystems.Hooks;
 import org.team1540.robot2024.subsystems.fakesubsystems.Tramp;

src/main/java/org/team1540/robot2024/commands/elevator/ElevatorAnalog.java

@@ -0,0 +1,24 @@
+package org.team1540.robot2024.commands.elevator;
+
+import org.team1540.robot2024.Constants;
+import org.team1540.robot2024.subsystems.elevator.Elevator;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
+
+public class ElevatorAnalog extends Command {
+
+    private final Elevator elevator;
+    private final CommandXboxController copilot;
+
+    public ElevatorAnalog(CommandXboxController copilot, Elevator elevator) {
+        this.elevator = elevator;
+        this.copilot = copilot;
+        addRequirements(elevator);
+    }
+
+    @Override
+    public void execute() {
+        elevator.setVoltage(copilot.getRightTriggerAxis() + Constants.Elevator.KG);
+    }
+}

src/main/java/org/team1540/robot2024/commands/elevator/ElevatorSetpointCommand.java

@@ -0,0 +1,29 @@
+package org.team1540.robot2024.commands.elevator;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import org.team1540.robot2024.Constants.Elevator.ElevatorState;
+import org.team1540.robot2024.subsystems.elevator.Elevator;
+
+public class ElevatorSetpointCommand extends Command {
+    private final Elevator elevator;
+    private final ElevatorState state;
+    public ElevatorSetpointCommand(Elevator elevator, ElevatorState state) {
+        this.elevator = elevator;
+        this.state = state;
+        addRequirements(elevator);
+    }
+    @Override
+    public void initialize() {
+        elevator.goToSetpoint(state.heightMeters);
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+        elevator.stop();
+    }
+    @Override
+    public boolean isFinished() {
+        return elevator.isAtSetpoint();
+    }
+
+}

src/main/java/org/team1540/robot2024/subsystems/elevator/Elevator.java

@@ -21,7 +21,6 @@ public void periodic(){
         Logger.processInputs("Elevator", elevatorInputs);
 
     }
-    // do positional control stuff here
 
     public void goToSetpoint(double setpointMeters) {
         this.setpointRots = setpointMeters / Constants.Elevator.SOCKET_DIAMETER;
@@ -32,6 +31,10 @@ public boolean isAtSetpoint() {
         return elevatorInputs.positionMeters == setpointRots;
     }
 
+    public void setVoltage(double voltage) {
+        elevatorIO.setVoltage(voltage);
+    }
+
     public void stop() {
         elevatorIO.stop();
     }

src/main/java/org/team1540/robot2024/subsystems/elevator/ElevatorIO.java

@@ -17,6 +17,8 @@ default void updateInputs(ElevatorIOInputs inputs) {}
 
     default void setSetpoint(double position) {}
 
+    default void setVoltage(double voltage) {}
+
     default void stop() {}
 
 

src/main/java/org/team1540/robot2024/subsystems/elevator/ElevatorIOTalonFX.java

@@ -85,6 +85,11 @@ public void setSetpoint(double position) {
         leader.setControl(motionMagicOut.withPosition(position));
     }
 
+    @Override
+    public void setVoltage(double voltage) {
+        leader.set(voltage*12);
+    }
+
     @Override
     public void stop() {
         leader.set(0);