Formatted and functional. (Added PID gains that arent 0)

FRC2539 · Feb 7, 2025 · 01a4ef0 · 01a4ef0
1 parent 9586714
commit 01a4ef0
Show file tree

Hide file tree

Showing 7 changed files with 18 additions and 9 deletions.
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -321,6 +321,7 @@ private void configureBindings() {
         ARMWRIST.and(operatorController.getA()).whileTrue(armSubsystem.armpivotDown());
         ARMWRIST.and(operatorController.getX()).whileTrue(wristSubsystem.turnWristLeft());
         ARMWRIST.and(operatorController.getB()).whileTrue(wristSubsystem.turnWristRight());
+
         CORAL.and(operatorController.getY()).onTrue(stateManager.moveToPosition(Position.L4));
         CORAL.and(operatorController.getX()).onTrue(stateManager.moveToPosition(Position.L3));
         CORAL.and(operatorController.getB()).onTrue(stateManager.moveToPosition(Position.L2));
@@ -332,7 +333,6 @@ private void configureBindings() {
                 .onTrue(stateManager.moveToPosition(Position.Home));
         CORAL.and(operatorController.getDPadUp())
                 .onTrue(stateManager.moveToPosition(Position.Handoff));
-        CORAL.and(operatorController.getBack()).onTrue(Commands.none());
 
         ALGAE.and(operatorController.getY()).onTrue(stateManager.moveToPosition(Position.L4Algae));
         ALGAE.and(operatorController.getX()).onTrue(stateManager.moveToPosition(Position.L3Algae));
@@ -348,7 +348,8 @@ private void configureBindings() {
                 .onTrue(stateManager.moveToPosition(Position.Quick34));
         ALGAE.and(operatorController.getDPadRight())
                 .onTrue(stateManager.moveToPosition(Position.Quick23));
-        ALGAE.and(operatorController.getBack()).onTrue(Commands.none());
+
+        operatorController.getBack().onTrue(wristSubsystem.flipWristPosition());
 
         // Driver Align Bindings, for a different/later day
         // CORAL.and(leftDriveController.getTrigger()).whileTrue(alignToReef(9, 0));

diff --git a/src/main/java/frc/robot/constants/ArmConstants.java b/src/main/java/frc/robot/constants/ArmConstants.java
@@ -28,11 +28,11 @@ public class ArmConstants {
 
     public static final CurrentLimitsConfigs currentLimitConfigs = new CurrentLimitsConfigs();
 
-    public static final double ARM_KP = 0;
+    public static final double ARM_KP = 5;
     public static final double ARM_KD = 0;
     public static final double ARM_KI = 0;
 
-    public static final double ARM_TOLERANCE = 0.1;
+    public static final double ARM_TOLERANCE = 0.01;
 
     public static final double upperLimit = 100;
     public static final double lowerLimit = 0;

diff --git a/src/main/java/frc/robot/constants/WristConstants.java b/src/main/java/frc/robot/constants/WristConstants.java
@@ -7,11 +7,11 @@ public class WristConstants {
 
     public static final int WRIST_THROUGHBORE_ENCODER_ID = 1;
 
-    public static final double WRIST_KP = 0;
+    public static final double WRIST_KP = 5;
     public static final double WRIST_KD = 0;
     public static final double WRIST_KI = 0;
 
-    public static final double WRIST_TOLERANCE = 0.1;
+    public static final double WRIST_TOLERANCE = 0.01;
 
     public static final double WristCurrent = 0;
 

diff --git a/src/main/java/frc/robot/subsystems/ModeManager/SuperstructureStateManager.java b/src/main/java/frc/robot/subsystems/ModeManager/SuperstructureStateManager.java
@@ -40,7 +40,7 @@ public interface StateChecker {
                 (p, s) -> {
                     // return (s.internalPosition == p);
                     double armPosition = s.armSubsystem.getPosition();
-                    double wristPosition = s.wristSubsystem.getPosition();
+                    double wristPosition = s.wristSubsystem.getFlippedPosition();
                     double elevatorPosition = s.elevatorSubsystem.getPosition();
 
                     if ((Math.abs(armPosition - p.armheight) < 0.1)

diff --git a/src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java
@@ -14,7 +14,7 @@ public class ArmSubsystem extends SubsystemBase {
     public LoggedNetworkNumber armTuneables = new LoggedNetworkNumber("arm tuneable", 9);
 
     private PIDController controller =
-            new PIDController(ArmConstants.ARM_KP, ArmConstants.ARM_KI, ArmConstants.ARM_KP);
+            new PIDController(ArmConstants.ARM_KP, ArmConstants.ARM_KI, ArmConstants.ARM_KD);
 
     private double reference = 0;
 

diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java
@@ -9,8 +9,10 @@ public void updateInputs(ElevatorIOInputs inputs) {
         final double stepAmount = 1;
         if (positionSetpoint > position) {
             position += stepAmount * 0.02;
+            if (position > positionSetpoint) position = positionSetpoint;
         } else if (positionSetpoint < position) {
             position -= stepAmount * 0.02;
+            if (position < positionSetpoint) position = positionSetpoint;
         }
 
         inputs.position = position;

diff --git a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java
@@ -22,7 +22,7 @@ public class WristSubsystem extends SubsystemBase {
     private boolean isWristFlipped = false;
 
     public WristSubsystem(WristIO wristIO) {
-        controller.setTolerance(0.1);
+        controller.setTolerance(WristConstants.WRIST_TOLERANCE);
         this.wristIO = wristIO;
         setDefaultCommand(setVoltage(0));
     }
@@ -94,6 +94,12 @@ public double getPosition() {
         return wristInputs.throughboreEncoderPosition;
     }
 
+    public double getFlippedPosition() {
+        return isWristFlipped
+                ? -wristInputs.throughboreEncoderPosition
+                : wristInputs.throughboreEncoderPosition;
+    }
+
     public double getInternalEncoderPosition() {
         return wristInputs.position;
     }