slapdown and wrist stuff

RegisJesuitRobotics · Feb 22, 2024 · e671bec · e671bec
1 parent d49bc6c
commit e671bec
Show file tree

Hide file tree

Showing 9 changed files with 83 additions and 86 deletions.
diff --git a/.run/Build & Deploy Robot for Debugging.run.xml b/.run/Build & Deploy Robot for Debugging.run.xml
@@ -2,7 +2,7 @@
   <configuration default="false" name="Build &amp; Deploy Robot for Debugging" type="GradleRunConfiguration" factoryName="Gradle">
     <ExternalSystemSettings>
       <option name="executionName" />
-      <option name="externalProjectPath" value="$PROJECT_DIR$" />
+      <option name="externalProjectPath" value="$PROJECT_DIR$/Competition2024" />
       <option name="externalSystemIdString" value="GRADLE" />
       <option name="scriptParameters" value="-PdebugMode=true" />
       <option name="taskDescriptions">
@@ -18,29 +18,6 @@
     <ExternalSystemDebugServerProcess>true</ExternalSystemDebugServerProcess>
     <ExternalSystemReattachDebugProcess>true</ExternalSystemReattachDebugProcess>
     <DebugAllEnabled>false</DebugAllEnabled>
-    <RunAsTest>false</RunAsTest>
-    <method v="2" />
-  </configuration>
-  <configuration default="false" name="Build &amp; Deploy Robot for Debugging" type="GradleRunConfiguration" factoryName="Gradle">
-    <ExternalSystemSettings>
-      <option name="executionName" />
-      <option name="externalProjectPath" value="$PROJECT_DIR$" />
-      <option name="externalSystemIdString" value="GRADLE" />
-      <option name="scriptParameters" value="-PdebugMode=true" />
-      <option name="taskDescriptions">
-        <list />
-      </option>
-      <option name="taskNames">
-        <list>
-          <option value="deploy" />
-        </list>
-      </option>
-      <option name="vmOptions" />
-    </ExternalSystemSettings>
-    <ExternalSystemDebugServerProcess>true</ExternalSystemDebugServerProcess>
-    <ExternalSystemReattachDebugProcess>true</ExternalSystemReattachDebugProcess>
-    <DebugAllEnabled>false</DebugAllEnabled>
-    <RunAsTest>false</RunAsTest>
     <method v="2" />
   </configuration>
 </component>
diff --git a/.run/Build Robot.run.xml b/.run/Build Robot.run.xml
@@ -4,27 +4,7 @@
       <option name="executionName" />
       <option name="externalProjectPath" value="$PROJECT_DIR$" />
       <option name="externalSystemIdString" value="GRADLE" />
-      <option name="scriptParameters" value="build" />
-      <option name="taskDescriptions">
-        <list />
-      </option>
-      <option name="taskNames">
-        <list />
-      </option>
-      <option name="vmOptions" />
-    </ExternalSystemSettings>
-    <ExternalSystemDebugServerProcess>true</ExternalSystemDebugServerProcess>
-    <ExternalSystemReattachDebugProcess>true</ExternalSystemReattachDebugProcess>
-    <DebugAllEnabled>false</DebugAllEnabled>
-    <RunAsTest>false</RunAsTest>
-    <method v="2" />
-  </configuration>
-  <configuration default="false" name="Build Robot" type="GradleRunConfiguration" factoryName="Gradle">
-    <ExternalSystemSettings>
-      <option name="executionName" />
-      <option name="externalProjectPath" value="$PROJECT_DIR$" />
-      <option name="externalSystemIdString" value="GRADLE" />
-      <option name="scriptParameters" />
+      <option name="scriptParameters" value="" />
       <option name="taskDescriptions">
         <list />
       </option>
@@ -38,7 +18,6 @@
     <ExternalSystemDebugServerProcess>true</ExternalSystemDebugServerProcess>
     <ExternalSystemReattachDebugProcess>true</ExternalSystemReattachDebugProcess>
     <DebugAllEnabled>false</DebugAllEnabled>
-    <RunAsTest>false</RunAsTest>
     <method v="2" />
   </configuration>
 </component>
diff --git a/build.gradle b/build.gradle
@@ -35,7 +35,7 @@ deploy {
                     jvmArgs.add("-Dcom.sun.management.jmxremote.local.only=false")
                     jvmArgs.add("-Dcom.sun.management.jmxremote.ssl=false")
                     jvmArgs.add("-Dcom.sun.management.jmxremote.authenticate=false")
-                    jvmArgs.add("-Djava.rmi.server.hostname=127.0.0.1") // Replace XX.XX with team number
+                    jvmArgs.add("-Djava.rmi.server.hostname=10.37.29.2") // Replace XX.XX with team number
                 }
 
                 // Static files artifact

diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java
@@ -7,12 +7,12 @@ 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 = 117;
-  public static final String GIT_SHA = "4bb8c2bf00761887ca8846cd361c7484a200594e";
-  public static final String GIT_DATE = "2024-02-21 11:44:18 MST";
+  public static final int GIT_REVISION = 118;
+  public static final String GIT_SHA = "d49bc6cb82317d462eab6bcff46fa6a1d442efd4";
+  public static final String GIT_DATE = "2024-02-21 14:54:22 MST";
   public static final String GIT_BRANCH = "workingongettingready";
-  public static final String BUILD_DATE = "2024-02-21 14:46:36 MST";
-  public static final long BUILD_UNIX_TIME = 1708551996489L;
+  public static final String BUILD_DATE = "2024-02-21 19:00:25 MST";
+  public static final long BUILD_UNIX_TIME = 1708567225933L;
   public static final int DIRTY = 1;
 
   private BuildConstants(){}

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -46,22 +46,21 @@ public static class SlapdownConstants {
     public static final int ROTATION_STALL_MOTOR_CURRENT = 20;
     public static final int ROTATION_FREE_MOTOR_CURRENT = 10;
 
-    public static final double ROTATION_UP_ANGLE = Units.degreesToRadians(-24.404689 - 90.0);
-    public static final double ROTATION_DOWN_ANGLE =
-        ROTATION_UP_ANGLE + Units.degreesToRadians(128.817943);
+    public static final double ROTATION_UP_ANGLE = -0.0648 - Units.degreesToRadians(90);
+    public static final double ROTATION_DOWN_ANGLE = 2.3 - Units.degreesToRadians(90);
 
     public static final int ROTATION_LIMIT_SWITCH_ID = 1;
 
     public static final double FEEDER_VOLTAGE = 6.0;
 
     public static final TunablePIDGains ROTATION_GAINS =
-        new TunablePIDGains("/slapdown/rotation/gains", 0, 0, 0, MiscConstants.TUNING_MODE);
+        new TunablePIDGains("/slapdown/rotation/gains", 4.0, 0, 0, MiscConstants.TUNING_MODE);
     public static final TunableTrapezoidalProfileGains ROTATION_TRAP_GAINS =
         new TunableTrapezoidalProfileGains(
-            "/slapdown/rotation/trapGains", 0, 0, MiscConstants.TUNING_MODE);
+            "/slapdown/rotation/trapGains", 25, 40, MiscConstants.TUNING_MODE);
     public static final TunableArmElevatorFFGains ROTATION_FF_GAINS =
         new TunableArmElevatorFFGains(
-            "/slapdown/rotation/FFGains", 0, 0, 0, 0, MiscConstants.TUNING_MODE);
+            "/slapdown/rotation/FFGains", 0.10403, 0.17546, 0.61704, 0.084257, MiscConstants.TUNING_MODE);
   }
 
   public static class ElevatorConstants {
@@ -84,12 +83,12 @@ public static class ElevatorConstants {
 
     // TODO: Do These PID GAINS
     public static final TunablePIDGains PID_GAINS =
-        new TunablePIDGains("gains/elevator", 57, 0, 0, MiscConstants.TUNING_MODE);
+        new TunablePIDGains("gains/elevator", 20.0, 0, 0, MiscConstants.TUNING_MODE);
     public static final TunableTrapezoidalProfileGains TRAPEZOIDAL_PROFILE_GAINS =
         new TunableTrapezoidalProfileGains(
             "/gains/extension",
-            Units.inchesToMeters(25),
-            Units.inchesToMeters(100),
+            Units.inchesToMeters(28),
+            Units.inchesToMeters(125),
             MiscConstants.TUNING_MODE);
 
     // TODO: TUNE FF GAINS
@@ -213,9 +212,9 @@ public static class WristConstants {
 
     // TODO FIND WRIST CLAMPS
     public static final Rotation2d WRIST_MAX = new Rotation2d(Units.degreesToRadians(60));
-    public static final Rotation2d WRIST_MIN = new Rotation2d(0);
+    public static final Rotation2d WRIST_MIN = Rotation2d.fromRadians(-0.027);
 
-    public static final double WRIST_OFFSET = 0.0;
+    public static final double WRIST_OFFSET = -0.288 + Units.degreesToRadians(90.0);
     public static final int WRIST_ENCODER_PORT = 2;
 
     public static final int WRIST_MOTOR_ID = 2;
@@ -226,12 +225,12 @@ public static class WristConstants {
     // TODO: TUNE PID & TRAP & FF
     public static final TunableArmElevatorFFGains WRIST_FF_GAINS =
         new TunableArmElevatorFFGains(
-            "/wrist/ffGains", 0.078, 0.292, 1.0951, 0.261, MiscConstants.TUNING_MODE);
+            "/wrist/ffGains", .16624, 0.35068, 1.0512, 0.2769, 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);
+        new TunablePIDGains("/wrist/pidGains", 2, 0, 0, MiscConstants.TUNING_MODE);
 
     public static final TunableTrapezoidalProfileGains TRAPEZOIDAL_PROFILE_GAINS =
         new TunableTrapezoidalProfileGains(
@@ -246,7 +245,7 @@ public static class TransportConstants {
     public static final boolean INVERTED = false;
     public static final int STALL_MOTOR_CURRENT = 45;
     public static final int FREE_MOTOR_CURRENT = 25;
-    public static final double TRANSPORT_VOLTAGE = 2.0;
+    public static final double TRANSPORT_VOLTAGE = 6.0;
     public static final int SHOOTER_SENSOR_ID = 8;
   }
 

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -77,6 +77,14 @@ public RobotContainer() {
   }
 
   private void configureAutos() {
+    autoCommand.addOption("Slapdown Q Forward", slapdownSuperstructure.getSlapdownRotationSubsystem().sysIdQuasistatic(SysIdRoutine.Direction.kForward));
+    autoCommand.addOption("Slapdown Q Back", slapdownSuperstructure.getSlapdownRotationSubsystem().sysIdQuasistatic(SysIdRoutine.Direction.kReverse));
+    autoCommand.addOption("Slapdown D Forward", slapdownSuperstructure.getSlapdownRotationSubsystem().sysIdDynamic(SysIdRoutine.Direction.kForward));
+    autoCommand.addOption("Slapdown D Back", slapdownSuperstructure.getSlapdownRotationSubsystem().sysIdDynamic(SysIdRoutine.Direction.kReverse));
+  autoCommand.addOption("slapdown bottom", slapdownSuperstructure.setDownAndRunCommand());
+    autoCommand.addOption("slapdown top", slapdownSuperstructure.setUpCommand());
+
+  autoCommand.addOption("Probe Elevator", elevatorSubsystem.probeHomeCommand());
     autoCommand.addOption(
         "Wrist Q Forward", wristSubsystem.sysIdQuasistatic(SysIdRoutine.Direction.kForward));
     autoCommand.addOption(
@@ -87,7 +95,7 @@ private void configureAutos() {
         "Wrist D Back", wristSubsystem.sysIdDynamic(SysIdRoutine.Direction.kReverse));
     autoCommand.addOption(
         "Wrist 5 deg",
-        wristSubsystem.setPositonCommand(new Rotation2d(Units.degreesToRadians(40))));
+        wristSubsystem.setPositonCommand(new Rotation2d(Units.degreesToRadians(5))));
     autoCommand.addOption(
         "Elevator Quastatic Backward",
         elevatorSubsystem.sysIdQuasistatic(SysIdRoutine.Direction.kReverse));
@@ -113,6 +121,10 @@ private void configureAutos() {
     autoCommand.addOption(
         "Shooter Quastatic Forward Command",
         shooterSubsystem.sysIdQuasistatic(SysIdRoutine.Direction.kForward));
+    autoCommand.addOption("WristElevatorZero", Commands.parallel(
+            wristSubsystem.setPositonCommand(Constants.WristConstants.WRIST_MIN), elevatorSubsystem.setElevatorPositionCommand(0.0)));
+    autoCommand.addOption("WristElevatorNot", Commands.parallel(wristSubsystem.setPositonCommand(Rotation2d.fromDegrees(40.0)), elevatorSubsystem.setElevatorPositionCommand(Units.inchesToMeters(8.0))));
+
     autoCommand.addOption(
         "Shooter Quastatic Backward Command",
         shooterSubsystem.sysIdQuasistatic(SysIdRoutine.Direction.kReverse));
@@ -136,7 +148,7 @@ private void configureAutos() {
 
   private void configureDriverBindings() {
     configureDriving();
-
+    driverController.leftTrigger().onTrue(transportSubsystem.setVoltageCommand(Constants.TransportConstants.TRANSPORT_VOLTAGE).until(()->!transportSubsystem.atSensor()).andThen(transportSubsystem.setVoltageCommand(0)));
     driverController
         .home()
         .onTrue(
@@ -158,9 +170,14 @@ private void configureDriverBindings() {
         .povUp()
         .toggleOnTrue(slapdownSuperstructure.setUpCommand()); // TODO: THIS
     driverController.a().onTrue(intakeSubsystem.checkIntakeCommand());
+    Command intakeAndFeedUntilDone = Commands.parallel(intakeSubsystem.setIntakeVoltageCommand(Constants.IntakeConstants.INTAKE_VOLTAGE), transportSubsystem.setVoltageCommand(Constants.TransportConstants.TRANSPORT_VOLTAGE)).until(transportSubsystem::atSensor).unless(transportSubsystem::atSensor);
+    driverController.leftBumper().whileTrue(Commands.parallel(slapdownSuperstructure.setDownAndRunCommand(), intakeAndFeedUntilDone.asProxy(), elevatorSubsystem.setElevatorPositionCommand(0), wristSubsystem.setPositonCommand(new Rotation2d(0))));
+    driverController.leftBumper().onFalse(slapdownSuperstructure.setUpCommand());
   }
 
   private void configureOperatorBindings() {
+    operatorController.triangle().onTrue(Commands.sequence(shooterSubsystem.runVelocityCommand(10000.0/60)));
+    operatorController.x().onTrue(shooterSubsystem.setVoltageCommand(0));
     operatorController
         .leftTrigger()
         .onTrue(
@@ -204,6 +221,7 @@ private void configureOperatorBindings() {
                     .setVoltageCommand(4.0)
                     .until(transportSubsystem::atSensor)
                     .andThen(transportSubsystem.setVoltageCommand(0.0))));
+
     //
     // operatorController.share().onTrue(transportSubsystem.setVoltageCommand(6.0).until(transportSubsystem::atSensor));
     double rpm = 10000;

diff --git a/src/main/java/frc/robot/subsystems/slapdown/SlapdownRotationSubsystem.java b/src/main/java/frc/robot/subsystems/slapdown/SlapdownRotationSubsystem.java
@@ -1,6 +1,6 @@
 package frc.robot.subsystems.slapdown;
 
-import static edu.wpi.first.units.Units.Volts;
+import static edu.wpi.first.units.Units.*;
 import static frc.robot.Constants.SlapdownConstants.*;
 
 import com.revrobotics.CANSparkBase.IdleMode;
@@ -16,6 +16,8 @@
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import frc.robot.Constants;
 import frc.robot.telemetry.tunable.TunableTelemetryProfiledPIDController;
+import frc.robot.telemetry.types.BooleanTelemetryEntry;
+import frc.robot.telemetry.types.DoubleTelemetryEntry;
 import frc.robot.telemetry.types.EventTelemetryEntry;
 import frc.robot.telemetry.wrappers.TelemetryCANSparkMax;
 import frc.robot.utils.Alert;
@@ -37,13 +39,17 @@ public class SlapdownRotationSubsystem extends SubsystemBase {
 
   private final SysIdRoutine slapdownRotationSysId =
       new SysIdRoutine(
-          new SysIdRoutine.Config(),
+          new SysIdRoutine.Config(Volts.per(Second).of(0.5), Volts.of(2), Seconds.of(5), null),
           new SysIdRoutine.Mechanism(
               (voltage) -> setRotationVoltage(voltage.in(Volts)),
               null, // No log consumer, since data is recorded by URCL
               this));
 
   private final ArmFeedforward rotationFF = ROTATION_FF_GAINS.createArmFeedforward();
+  private final BooleanTelemetryEntry atLimitEntry = new BooleanTelemetryEntry("/slapdown/rotation/atLimit", true);
+
+  private final BooleanTelemetryEntry isHomedEntry = new BooleanTelemetryEntry("/slapdown/rotation/isHomed", true);
+  private final DoubleTelemetryEntry voltageRequestEntry = new DoubleTelemetryEntry("/slapdown/rotation/voltageReq", Constants.MiscConstants.TUNING_MODE);
   private final TunableTelemetryProfiledPIDController rotationController =
       new TunableTelemetryProfiledPIDController(
           "/slapdown/rotation/controller", ROTATION_GAINS, ROTATION_TRAP_GAINS);
@@ -123,6 +129,7 @@ private void configMotors() {
 
   public void setRotationVoltage(double voltage) {
     rotationMotor.setVoltage(voltage);
+    voltageRequestEntry.append(voltage);
   }
 
   private double getPosition() {
@@ -141,13 +148,17 @@ public Command setRotationGoalCommand(Rotation2d goal) {
     return RaiderCommands.ifCondition(this::isHomed).then(
             this.run(
                     () -> {
-                      rotationController.setGoal(goal.getRadians());
                       double feedbackOutput = rotationController.calculate(getPosition());
                       TrapezoidProfile.State currentSetpoint = rotationController.getSetpoint();
 
                       setRotationVoltage(
-                              feedbackOutput + rotationFF.calculate(getPosition(), currentSetpoint.velocity));
-                    })
+                              feedbackOutput + rotationFF.calculate(currentSetpoint.position, currentSetpoint.velocity));
+                    }).beforeStarting(
+                    () -> {
+                      rotationController.setGoal(goal.getRadians());
+                      rotationController.reset(getPosition(), rotationEncoder.getVelocity());
+                    }
+            )
     ).otherwise(Commands.none());
   }
 
@@ -172,7 +183,10 @@ public void periodic() {
     if (atLimit()) {
       rotationEncoder.setPosition(ROTATION_UP_ANGLE);
       isHomed = true;
+
     }
+    isHomedEntry.append(isHomed);
+    atLimitEntry.append(atLimit());
 
     rotationMotor.logValues();
   }

diff --git a/src/main/java/frc/robot/subsystems/slapdown/SlapdownSuperstructure.java b/src/main/java/frc/robot/subsystems/slapdown/SlapdownSuperstructure.java
@@ -16,6 +16,9 @@ public Command setDownAndRunCommand() {
         feederSubsystem.setVoltageCommand(FEEDER_VOLTAGE),
         rotationSubsystem.setRotationGoalCommand(Rotation2d.fromRadians(ROTATION_DOWN_ANGLE)));
   }
+  public SlapdownRotationSubsystem getSlapdownRotationSubsystem(){
+    return rotationSubsystem;
+  }
 
   public Command setUpCommand() {
     return Commands.parallel(