wrist changes

src/main/java/frc/robot/BuildConstants.java

@@ -1,17 +1,19 @@
 package frc.robot;
 
-/** Automatically generated file containing build version information. */
+/**
+ * Automatically generated file containing build version information.
+ */
 public final class BuildConstants {
   public static final String MAVEN_GROUP = "";
   public static final String MAVEN_NAME = "Competition2024";
   public static final String VERSION = "unspecified";
-  public static final int GIT_REVISION = 110;
-  public static final String GIT_SHA = "ff2bac4bb1164b7901cc679e47c3ddabd00b2d3d";
-  public static final String GIT_DATE = "2024-02-19 22:16:55 MST";
+  public static final int GIT_REVISION = 114;
+  public static final String GIT_SHA = "1777ba50499098176f582e0eae64cdea9e35c8ab";
+  public static final String GIT_DATE = "2024-02-20 16:34:07 MST";
   public static final String GIT_BRANCH = "workingongettingready";
-  public static final String BUILD_DATE = "2024-02-20 14:42:29 MST";
-  public static final long BUILD_UNIX_TIME = 1708465349930L;
+  public static final String BUILD_DATE = "2024-02-20 19:36:55 MST";
+  public static final long BUILD_UNIX_TIME = 1708483015286L;
   public static final int DIRTY = 1;
 
-  private BuildConstants() {}
+  private BuildConstants(){}
 }

src/main/java/frc/robot/Constants.java

@@ -50,7 +50,7 @@ public static class SlapdownConstants {
     public static final double ROTATION_DOWN_ANGLE =
         ROTATION_UP_ANGLE + Units.degreesToRadians(128.817943);
 
-    public static final int ROTATION_LIMIT_SWITCH_ID = 0;
+    public static final int ROTATION_LIMIT_SWITCH_ID = 1;
 
     public static final TunablePIDGains ROTATION_GAINS =
         new TunablePIDGains("/slapdown/rotation/gains", 0, 0, 0, MiscConstants.TUNING_MODE);
@@ -74,7 +74,7 @@ public static class ElevatorConstants {
 
     public static final int ELEVATOR_MOTOR_ID = 4;
 
-    public static final int ELEVATOR_LIMIT_SWITCH = 2;
+    public static final int ELEVATOR_LIMIT_SWITCH = 0;
 
     public static final double ELEVATOR_GEAR_RATIO = 5.0 * 3.0;
     public static final double METERS_PER_REV =
@@ -214,18 +214,21 @@ public static class WristConstants {
     public static final Rotation2d WRIST_MIN = new Rotation2d(0);
 
     public static final int WRIST_MOTOR_ID = 2;
-    public static final boolean INVERTED = false;
+    public static final boolean INVERTED = true;
     public static final int STALL_MOTOR_CURRENT = 45;
     public static final int FREE_MOTOR_CURRENT = 25;
 
     // TODO: TUNE PID & TRAP & FF
     public static final TunableArmElevatorFFGains WRIST_FF_GAINS =
-        new TunableArmElevatorFFGains("/wrist/ffGains", 0, 0, 0, 0, MiscConstants.TUNING_MODE);
-    public static final TunablePIDGains WRIST_PID_GAINS =
+        new TunableArmElevatorFFGains("/wrist/ffGains", 0.078, 0.292, 1.0951, 0.261, MiscConstants.TUNING_MODE);
+//    public static final TunablePIDGains WRIST_PID_GAINS =
+//        new TunablePIDGains("/wrist/pidGains", 56.599, 0, 4.8897, MiscConstants.TUNING_MODE);
+//
+public static final TunablePIDGains WRIST_PID_GAINS =
         new TunablePIDGains("/wrist/pidGains", 0, 0, 0, MiscConstants.TUNING_MODE);
 
     public static final TunableTrapezoidalProfileGains TRAPEZOIDAL_PROFILE_GAINS =
-        new TunableTrapezoidalProfileGains("/wrist/trapGains", 0, 0, MiscConstants.TUNING_MODE);
+        new TunableTrapezoidalProfileGains("/wrist/trapGains", Units.rotationsToRadians(0.5), Units.rotationsToRadians(0.5), MiscConstants.TUNING_MODE);
   }
 
   public static class TransportConstants {

src/main/java/frc/robot/RobotContainer.java

@@ -79,9 +79,11 @@ public RobotContainer() {
   }
 
   private void configureAutos() {
-    autoCommand.addOption(
-        "Elevator Quastatic Forward",
-        elevatorSubsystem.sysIdQuasistatic(SysIdRoutine.Direction.kForward));
+    autoCommand.addOption("Wrist Q Forward", wristSubsystem.sysIdQuasistatic(SysIdRoutine.Direction.kForward));
+    autoCommand.addOption("Wrist Q Back", wristSubsystem.sysIdQuasistatic(SysIdRoutine.Direction.kReverse));
+    autoCommand.addOption("Wrist D Forward", wristSubsystem.sysIdDynamic(SysIdRoutine.Direction.kForward));
+    autoCommand.addOption("Wrist D Back", wristSubsystem.sysIdDynamic(SysIdRoutine.Direction.kReverse));
+    autoCommand.addOption("Wrist 5 deg", wristSubsystem.setPositonCommand(new Rotation2d(Units.degreesToRadians(40))));
     autoCommand.addOption(
         "Elevator Quastatic Backward",
         elevatorSubsystem.sysIdQuasistatic(SysIdRoutine.Direction.kReverse));

src/main/java/frc/robot/commands/elevator/AmpPlaceCommand.java

@@ -27,7 +27,7 @@ public AmpPlaceCommand(
     this.transportSubsystem = transportSubsystem;
     addCommands(
         elevatorSubsystem.setElevatorPositionCommand(AMP_ELEVATOR_HEIGHT),
-        wristSubsystem.setPosiitonCommand(WRIST_AMP_POSITION),
+        wristSubsystem.setPositonCommand(WRIST_AMP_POSITION),
         new ShootAtAngleCommand(
             shooterSubsystem, transportSubsystem, wristSubsystem, WRIST_AMP_POSITION));
   }

src/main/java/frc/robot/commands/elevator/ElevatorWristParallelCommand.java

@@ -16,7 +16,7 @@ public ElevatorWristParallelCommand(
     this.wristSubsystem = wristSubsystem;
 
     addCommands(
-        wristSubsystem.setPosiitonCommand(WRIST_MIN),
+        wristSubsystem.setPositonCommand(WRIST_MIN),
         elevatorSubsystem.setElevatorPositionCommand(0));
   }
 }

src/main/java/frc/robot/commands/shooter/RunWristAndFlywheels.java

@@ -18,7 +18,7 @@ public RunWristAndFlywheels(
     this.shooterSubsystem = shooterSubsystem;
 
     addCommands(
-        wristSubsystem.setPosiitonCommand(shootingAngle),
+        wristSubsystem.setPositonCommand(shootingAngle),
         shooterSubsystem.runVelocityCommand(SHOOTING_RPM));
   }
 }

src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java

@@ -174,6 +174,10 @@ public Command sysIdDynamic(SysIdRoutine.Direction direction) {
 
   @Override
   public void periodic() {
+    // TODO: make this or elegant
+    if (bottomLimit.get()) {
+      elevatorEncoder.setPosition(0.0);
+    }
     logValues();
   }
 

src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java

@@ -1,5 +1,6 @@
 package frc.robot.subsystems.wrist;
 
+import static edu.wpi.first.units.Units.Second;
 import static edu.wpi.first.units.Units.Volts;
 import static frc.robot.Constants.WristConstants.*;
 
@@ -27,14 +28,14 @@ public class WristSubsystem extends SubsystemBase {
       new Alert("Wrist motor had a fault initializing", Alert.AlertType.ERROR);
   private final SysIdRoutine wristSysId =
       new SysIdRoutine(
-          new SysIdRoutine.Config(),
+          new SysIdRoutine.Config(Volts.per(Second).of(.5), Volts.of(2), null, null),
           new SysIdRoutine.Mechanism(
               (voltage) -> setVoltage(voltage.in(Volts)),
               null, // No log consumer, since data is recorded by URCL
               this));
 
   private DoubleTelemetryEntry absoluteEncoderEntry =
-      new DoubleTelemetryEntry("/wrist/encoders", true);
+      new DoubleTelemetryEntry("/wrist/absoluteEncoder", true);
 
   private final TelemetryCANSparkMax wristMotor =
       new TelemetryCANSparkMax(
@@ -149,7 +150,7 @@ public Command setVotageCommand(double voltage) {
     return this.run(() -> setVoltage(voltage));
   }
 
-  public Command setPosiitonCommand(Rotation2d desiredPosition) {
+  public Command setPositonCommand(Rotation2d desiredPosition) {
     return this.run(
         () -> {
           controller.setGoal(desiredPosition.getRadians());