feat: sim now functions

flamingchickens1540 · Feb 1, 2024 · 1862263 · 1862263
1 parent 924a65a
commit 1862263
Show file tree

Hide file tree

Showing 4 changed files with 26 additions and 5 deletions.
diff --git 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
@@ -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
@@ -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
@@ -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;
     }
+
+
 }