From 1862263699c458bcbfe9c81c8cba9f492eed20f9 Mon Sep 17 00:00:00 2001 From: Brandon Harad Date: Wed, 31 Jan 2024 17:03:26 -0800 Subject: [PATCH] feat: sim now functions --- .../java/org/team1540/robot2024/Constants.java | 6 +++--- .../org/team1540/robot2024/RobotContainer.java | 6 +++++- .../elevator/ElevatorManualCommand.java | 2 +- .../robot2024/subsystems/elevator/Elevator.java | 17 +++++++++++++++++ 4 files changed, 26 insertions(+), 5 deletions(-) diff --git a/src/main/java/org/team1540/robot2024/Constants.java b/src/main/java/org/team1540/robot2024/Constants.java index 1134de16..a7431c35 100644 --- a/src/main/java/org/team1540/robot2024/Constants.java +++ b/src/main/java/org/team1540/robot2024/Constants.java @@ -129,9 +129,9 @@ public static class Elevator { public static final double KI = 0; public static final double KD = 0.1; public static final double KG = 0; - public static final int CRUISE_VELOCITY_MPS = 2; - public static final int MAXIMUM_ACCELERATION_MPS2 = 20; - public static final int JERK_MP3 = 40; + public static final double CRUISE_VELOCITY_MPS = 2; + public static final double MAXIMUM_ACCELERATION_MPS2 = 20; + public static final double JERK_MP3 = 40; public static final double SPROCKET_CIRCUMFERENCE = .044 * Math.PI; public static final double MOTOR_ROTS_TO_METERS = GEAR_RATIO * SPROCKET_CIRCUMFERENCE; public static final double ERROR_TOLERANCE = 0.03; diff --git a/src/main/java/org/team1540/robot2024/RobotContainer.java b/src/main/java/org/team1540/robot2024/RobotContainer.java index 4b442afd..6d66c622 100644 --- a/src/main/java/org/team1540/robot2024/RobotContainer.java +++ b/src/main/java/org/team1540/robot2024/RobotContainer.java @@ -10,9 +10,11 @@ import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import org.littletonrobotics.junction.networktables.LoggedDashboardNumber; +import org.team1540.robot2024.Constants.Elevator.ElevatorState; import org.team1540.robot2024.commands.FeedForwardCharacterization; import org.team1540.robot2024.commands.SwerveDriveCommand; import org.team1540.robot2024.commands.elevator.ElevatorManualCommand; +import org.team1540.robot2024.commands.elevator.ElevatorSetpointCommand; import org.team1540.robot2024.subsystems.drive.*; import org.team1540.robot2024.subsystems.elevator.Elevator; import org.team1540.robot2024.subsystems.elevator.ElevatorIO; @@ -118,7 +120,9 @@ public RobotContainer() { */ private void configureButtonBindings() { drivetrain.setDefaultCommand(new SwerveDriveCommand(drivetrain, driver)); - elevator.setDefaultCommand(new ElevatorManualCommand(elevator, driver)); + elevator.setDefaultCommand(new ElevatorManualCommand(elevator, copilot)); + copilot.rightBumper().onTrue(new ElevatorSetpointCommand(elevator, ElevatorState.TOP)); + copilot.leftBumper().onTrue(new ElevatorSetpointCommand(elevator, ElevatorState.BOTTOM)); driver.x().onTrue(Commands.runOnce(drivetrain::stopWithX, drivetrain)); driver.b().onTrue( Commands.runOnce( diff --git a/src/main/java/org/team1540/robot2024/commands/elevator/ElevatorManualCommand.java b/src/main/java/org/team1540/robot2024/commands/elevator/ElevatorManualCommand.java index 22acf4fe..65f5958b 100644 --- a/src/main/java/org/team1540/robot2024/commands/elevator/ElevatorManualCommand.java +++ b/src/main/java/org/team1540/robot2024/commands/elevator/ElevatorManualCommand.java @@ -20,7 +20,7 @@ public ElevatorManualCommand(Elevator elevator, CommandXboxController copilot) { @Override public void execute() { // elevator.setVoltage(copilot.getRightTriggerAxis() - copilot.getLeftTriggerAxis() + Constants.Elevator.KG); - elevator.goToSetpoint(elevator.getSetpoint() + Constants.Elevator.CRUISE_VELOCITY_MPS*2/100*(copilot.getRightTriggerAxis()-copilot.getLeftTriggerAxis())); + elevator.goToSetpoint(elevator.getSetpoint() + Constants.Elevator.CRUISE_VELOCITY_MPS/50.0*(copilot.getRightTriggerAxis()-copilot.getLeftTriggerAxis())); } @Override diff --git a/src/main/java/org/team1540/robot2024/subsystems/elevator/Elevator.java b/src/main/java/org/team1540/robot2024/subsystems/elevator/Elevator.java index ea1c60fe..a9399038 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/elevator/Elevator.java +++ b/src/main/java/org/team1540/robot2024/subsystems/elevator/Elevator.java @@ -1,12 +1,18 @@ package org.team1540.robot2024.subsystems.elevator; import edu.wpi.first.math.MathUtil; + +import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; import edu.wpi.first.wpilibj2.command.SubsystemBase; + +import org.team1540.robot2024.Constants; import org.team1540.robot2024.util.MechanismVisualiser; import org.team1540.robot2024.util.math.AverageFilter; +import com.ctre.phoenix6.Utils; + import static org.team1540.robot2024.Constants.Elevator.*; public class Elevator extends SubsystemBase { @@ -27,9 +33,17 @@ public void periodic(){ MechanismVisualiser.setElevatorPosition(elevatorInputs.positionMeters); elevatorPositionFilter.add(elevatorInputs.positionMeters); + + // if (elevatorInputs.lowerLimit) { + // setpointMeters = Constants.Elevator.ELEVATOR_MINIMUM_HEIGHT; + // } + // else if (elevatorInputs.upperLimit) { + // setpointMeters = Constants.Elevator.ELEVATOR_MAX_HEIGHT; + // } } public void goToSetpoint(double newSetpointMeters) { + newSetpointMeters = MathUtil.clamp(newSetpointMeters, Constants.Elevator.ELEVATOR_MINIMUM_HEIGHT, Constants.Elevator.ELEVATOR_MAX_HEIGHT); setpointMeters = newSetpointMeters; elevatorIO.setPositionMeters(setpointMeters); @@ -48,7 +62,10 @@ public void stop() { elevatorIO.setVoltage(0.0); } + @AutoLogOutput public double getSetpoint() { return setpointMeters; } + + }