day 3

SciBorgs · Apr 8, 2023 · daa6069 · daa6069
1 parent 243b756
commit daa6069
Show file tree

Hide file tree

Showing 9 changed files with 107 additions and 34 deletions.
diff --git a/.Glass/glass.json b/.Glass/glass.json
@@ -9,12 +9,12 @@
           "elbow feedback": {
             "open": true
           },
-          "open": true,
           "wrist feedback": {
             "open": true
           }
         },
         "Drive": {
+          "open": true,
           "rearLeft": {
             "open": true
           },
@@ -50,21 +50,16 @@
           "drive": {
             "open": true
           },
-          "open": true,
           "scoring": {
             "open": true
           }
         },
         "open": true
       },
-      "SmartDashboard": {
-        "open": true
-      },
       "photonvision": {
         "Microsoft_LifeCam_HD-3000 (1)": {
           "open": true
-        },
-        "open": true
+        }
       }
     },
     "types": {
@@ -109,7 +104,7 @@
     }
   },
   "NetworkTables Settings": {
-    "mode": "Client (NT4)",
+    "mode": "Client (NT3)",
     "serverTeam": "1155"
   },
   "Plots": {

diff --git a/shuffleboard.json b/shuffleboard.json
@@ -238,8 +238,8 @@
     }
   ],
   "windowGeometry": {
-    "x": -5.843478679656982,
-    "y": 0.8347826600074768,
+    "x": -6.6782612800598145,
+    "y": 1.6695653200149536,
     "width": 1614.4696044921875,
     "height": 866.50439453125
   }

diff --git a/src/main/deploy/pathplanner/balance.path b/src/main/deploy/pathplanner/balance.path
@@ -24,11 +24,11 @@
     },
     {
       "anchorPoint": {
-        "x": 3.8,
+        "x": 3.6830055171309457,
         "y": 2.76
       },
       "prevControl": {
-        "x": 3.7254292226456323,
+        "x": 3.6084347397765786,
         "y": 2.76
       },
       "nextControl": null,

diff --git a/src/main/deploy/pathplanner/cube intake l.path b/src/main/deploy/pathplanner/cube intake l.path
@@ -0,0 +1,61 @@
+{
+  "waypoints": [
+    {
+      "anchorPoint": {
+        "x": 1.830403287542686,
+        "y": 4.41
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.3229362930332513,
+        "y": 4.682694500460275
+      },
+      "holonomicAngle": 0.0,
+      "isReversal": false,
+      "velOverride": null,
+      "isLocked": false,
+      "isStopPoint": false,
+      "stopEvent": {
+        "names": [
+          "backHighCube",
+          "outtakeCube"
+        ],
+        "executionBehavior": "sequential",
+        "waitBehavior": "none",
+        "waitTime": 0
+      }
+    },
+    {
+      "anchorPoint": {
+        "x": 6.749126487245923,
+        "y": 4.59918147792796
+      },
+      "prevControl": {
+        "x": 5.790933383359715,
+        "y": 4.644875992928101
+      },
+      "nextControl": null,
+      "holonomicAngle": 0.0,
+      "isReversal": false,
+      "velOverride": null,
+      "isLocked": false,
+      "isStopPoint": false,
+      "stopEvent": {
+        "names": [
+          "initialIntake"
+        ],
+        "executionBehavior": "parallel",
+        "waitBehavior": "none",
+        "waitTime": 0
+      }
+    }
+  ],
+  "markers": [
+    {
+      "position": 0.029407068557070072,
+      "names": [
+        "frontIntake"
+      ]
+    }
+  ]
+}
diff --git a/src/main/java/org/sciborgs1155/robot/Constants.java b/src/main/java/org/sciborgs1155/robot/Constants.java
@@ -175,6 +175,8 @@ public static final class Elevator {
     public static final Constraints CONSTRAINTS = new Constraints(3, 3.2);
 
     public static final double ZERO_OFFSET = 0.61842;
+
+    public static final double FAILING_DEBOUNCE_TIME = 0.2;
   }
 
   public static final class Intake {
@@ -186,7 +188,7 @@ public static final class Intake {
 
     public static final double DEFAULT_SPEED = 0.1;
     public static final double INTAKE_SPEED = 1;
-    public static final double OUTTAKE_SPEED = -0.25;
+    public static final double OUTTAKE_SPEED = -0.3;
 
     public static final double THRESHOLD = 0.5;
   }
@@ -231,11 +233,11 @@ private SpeedMultiplier(double multiplier) {
     public static final PIDConstants TRANSLATION = new PIDConstants(0.6, 0, 0);
     public static final PIDConstants ROTATION = new PIDConstants(0.4, 0, 0);
 
-    public static final double MIN_PITCH = 12; // 12.5; // deg
+    public static final double MIN_PITCH = 12.5; // 12.5; // deg
     public static final double BALANCE_SPEED = 0.35; // m / s
 
     public static final PathConstraints CONSTRAINTS =
-        new PathConstraints(MAX_SPEED / 2.5, MAX_ACCEL / 1.2);
+        new PathConstraints(MAX_SPEED / 1.8, MAX_ACCEL / 1.2);
   }
 
   public static final class SwerveModule {

diff --git a/src/main/java/org/sciborgs1155/robot/RobotContainer.java b/src/main/java/org/sciborgs1155/robot/RobotContainer.java
@@ -134,6 +134,8 @@ private void configureAutoChooser() {
      */
     autoChooser.addOption("cube -> leave", autos::cubeLeave);
 
+    autoChooser.addOption("cube -> intake", autos::cubeIntake);
+
     // backups
 
     /* cube score setup instructions:
@@ -278,6 +280,7 @@ private void configureBindings() {
     // operator.leftTrigger().onTrue(placement.setStopped(true));
 
     arm.onElbowFailing().onTrue(placement.setStopped(true));
+    elevator.onFailin().onTrue(placement.setStopped(true));
   }
 
   /** A command to run when the robot is enabled */

diff --git a/src/main/java/org/sciborgs1155/robot/commands/Autos.java b/src/main/java/org/sciborgs1155/robot/commands/Autos.java
@@ -110,19 +110,15 @@ public Command twoGamepiece() {
   }
 
   public Command fullBalance() {
-    return drive
-        .follow("balance", true, true)
+    // return drive
+    // .follow("balance", true, true).withTimeout(4)
+    return Commands.run(() -> drive.drive(0.6, 0, 0, false), drive)
+        .until(() -> Math.abs(drive.getPitch()) >= 13.5)
+        .withTimeout(4)
         .andThen(drive.balance())
-        .withTimeout(10)
-        .andThen(Commands.print("balanced!"))
         .andThen(Commands.run(() -> drive.drive(-0.3, 0, 0, false), drive).withTimeout(0.1))
         .andThen(drive.lock())
-        .andThen(Commands.print("locked!"))
         .withName("balance auto");
-    // .andThen(drive.balance())
-    // .andThen(Commands.run(() -> drive.drive(-0.60, 0, 0, false), drive).withTimeout(0.1))
-    // .andThen(drive.lock())
-    // .withName("balance auto").withTimeout(8);
   }
 
   public Command highConeScore() {
@@ -153,7 +149,7 @@ public Command frontHighCubeScore() {
   /** no PPL */
   public Command cubeBalance() {
     return Commands.sequence(
-        backHighCubeScore(), placement.goTo(BALANCE, false).withTimeout(2.5), fullBalance());
+        backHighCubeScore(), placement.goTo(BALANCE, false).withTimeout(3.5), fullBalance());
   }
 
   /** no PPL */
@@ -170,6 +166,10 @@ public Command cubeLeave() {
     return followAutoPath("cube leaveComm" + startingPosChooser.getSelected().suffix);
   }
 
+  public Command cubeIntake() {
+    return followAutoPath("cube intake l");
+  }
+
   public Command lowCubeLeave() {
     return Commands.sequence(
         placement.goTo(FRONT_INTAKE, false),

diff --git a/src/main/java/org/sciborgs1155/robot/subsystems/Arm.java b/src/main/java/org/sciborgs1155/robot/subsystems/Arm.java
@@ -272,9 +272,10 @@ public void periodic() {
             && wristEncoder.getVelocity() == 0
             && wristSetpoint.position() != 0;
     butAScratch =
-        elbowEncoder.getPosition() == 0
-            && elbowEncoder.getVelocity() == 0
-            && elbowSetpoint.position() != Elbow.ELBOW_OFFSET;
+        elbowEncoder.getPosition() == 0 // no position reading
+            && elbowEncoder.getVelocity() == 0 // no velocity reading
+            && elbowSetpoint.position() != Elbow.ELBOW_OFFSET // elbow is not going to 0
+            && elbow.getAppliedOutput() != 0; // elbow is trying to move
 
     double elbowFB =
         elbowFeedback.calculate(getElbowPosition().getRadians(), elbowSetpoint.position());

diff --git a/src/main/java/org/sciborgs1155/robot/subsystems/Elevator.java b/src/main/java/org/sciborgs1155/robot/subsystems/Elevator.java
@@ -17,6 +17,7 @@
 import edu.wpi.first.wpilibj.simulation.ElevatorSim;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import edu.wpi.first.wpilibj2.command.button.Trigger;
 import io.github.oblarg.oblog.Loggable;
 import io.github.oblarg.oblog.annotations.Log;
 import org.sciborgs1155.lib.Derivative;
@@ -69,6 +70,7 @@ public class Elevator extends SubsystemBase implements Loggable, AutoCloseable {
           MAX_HEIGHT,
           true);
 
+  @Log private boolean encoderUnplugged;
   private boolean stopped;
 
   private final Visualizer positionVisualizer;
@@ -149,9 +151,13 @@ public Command followTrajectory(Trajectory trajectory) {
         .until(() -> setpoint.position() == trajectory.getLast());
   }
 
-  public boolean stalling() {
-    // return stalling;
-    return false;
+  /**
+   * If elevator is far enough from setpoint (we always use trapezoid profiling or trajectories), it
+   * is dangerous. This method returns a debounced trigger for when the elbow encoder is likely
+   * failing/not plugged in.
+   */
+  public Trigger onFailin() {
+    return new Trigger(() -> encoderUnplugged).debounce(FAILING_DEBOUNCE_TIME);
   }
 
   public Command setStopped(boolean stopped) {
@@ -160,9 +166,14 @@ public Command setStopped(boolean stopped) {
 
   @Override
   public void periodic() {
-    if (bottomSwitch.get()) {
-      offset = -encoder.getPosition();
-    }
+    // if (bottomSwitch.get()) {
+    //   offset = -encoder.getPosition();
+    // }
+    encoderUnplugged =
+        encoder.getPosition() == 0
+            && encoder.getVelocity() == 0
+            && setpoint.position() != offset
+            && lead.getAppliedOutput() != 0;
 
     setpoint =
         new State(