elevator is so smooth yum

SciBorgs · Feb 1, 2025 · cb0c3aa · cb0c3aa
1 parent 384c9b7
commit cb0c3aa
Show file tree

Hide file tree

Showing 6 changed files with 35 additions and 26 deletions.
diff --git a/src/main/java/org/sciborgs1155/robot/Constants.java b/src/main/java/org/sciborgs1155/robot/Constants.java
@@ -62,9 +62,9 @@ public static boolean inField(Pose3d pose) {
     }
 
     public enum Level {
-      L1(Meters.of(.3)),
-      L2(Meters.of(.7)),
-      L3(Meters.of(1)),
+      L1(Meters.of(0.3)),
+      L2(Meters.of(0.7)),
+      L3(Meters.of(1.0)),
       L4(Meters.of(1.5));
 
       public final Distance height;

diff --git a/src/main/java/org/sciborgs1155/robot/Ports.java b/src/main/java/org/sciborgs1155/robot/Ports.java
@@ -37,7 +37,7 @@ public static final class Scoral {
   }
 
   public static final class LEDs {
-    public static final int LED_PORT = 1;
+    public static final int LED_PORT = 9;
   }
 
   public static final class Hopper {

diff --git a/src/main/java/org/sciborgs1155/robot/Robot.java b/src/main/java/org/sciborgs1155/robot/Robot.java
@@ -1,5 +1,6 @@
 package org.sciborgs1155.robot;
 
+import static edu.wpi.first.units.Units.Meters;
 import static edu.wpi.first.units.Units.MetersPerSecond;
 import static edu.wpi.first.units.Units.RadiansPerSecond;
 import static edu.wpi.first.units.Units.Second;
@@ -35,6 +36,7 @@
 import org.sciborgs1155.robot.commands.Autos;
 import org.sciborgs1155.robot.drive.Drive;
 import org.sciborgs1155.robot.elevator.Elevator;
+import org.sciborgs1155.robot.elevator.ElevatorConstants;
 import org.sciborgs1155.robot.led.LEDStrip;
 import org.sciborgs1155.robot.scoral.Scoral;
 import org.sciborgs1155.robot.vision.Vision;
@@ -103,11 +105,11 @@ private void configureGameBehavior() {
 
   /** Configures trigger -> command bindings. */
   private void configureBindings() {
-    operator.a().toggleOnTrue(elevator.manualElevator(operator::getLeftY));
-    // operator.a().onTrue(elevator.scoreLevel(Level.L1));
-    // operator.b().onTrue(elevator.scoreLevel(Level.L2));
-    // operator.x().onTrue(elevator.scoreLevel(Level.L3));
-    // operator.y().onTrue(elevator.scoreLevel(Level.L4));
+    // operator.a().toggleOnTrue(elevator.manualElevator(InputStream.of(operator::getLeftY).negate()));//.alongWith(led.elevatorLED(() -> elevator.position() / ElevatorConstants.MAX_EXTENSION.in(Meters))));
+    operator.a().whileTrue(elevator.scoreLevel(Level.L1));
+    operator.b().whileTrue(elevator.scoreLevel(Level.L2));
+    operator.x().whileTrue(elevator.scoreLevel(Level.L3));
+    operator.y().whileTrue(elevator.scoreLevel(Level.L4));
 
     // x and y are switched: we use joystick Y axis to control field x motion
     InputStream x = InputStream.of(driver::getLeftY).negate();
@@ -142,7 +144,8 @@ private void configureBindings() {
             .rateLimit(MAX_ANGULAR_ACCEL.in(RadiansPerSecond.per(Second)));
 
     drive.setDefaultCommand(drive.drive(x, y, omega));
-    led.setDefaultCommand(led.scrolling());
+    led.setDefaultCommand(
+        led.elevatorLED(() -> elevator.position() / ElevatorConstants.MAX_EXTENSION.in(Meters)));
 
     autonomous().whileTrue(Commands.defer(autos::getSelected, Set.of(drive)).asProxy());
     autonomous().whileTrue(led.autos());

diff --git a/src/main/java/org/sciborgs1155/robot/elevator/Elevator.java b/src/main/java/org/sciborgs1155/robot/elevator/Elevator.java
@@ -75,23 +75,29 @@ public Elevator(ElevatorIO hardware) {
     pid.reset(hardware.position());
     pid.setGoal(MIN_EXTENSION.in(Meters));
 
-    // setDefaultCommand(retract());
+    setDefaultCommand(retract());
 
     sysIdRoutine =
         new SysIdRoutine(
             new SysIdRoutine.Config(
                 null,
-                Volts.of(4),
+                Volts.of(2),
                 null,
                 (state) -> SignalLogger.writeString("elevator state", state.toString())),
             new SysIdRoutine.Mechanism(v -> hardware.setVoltage(v.in(Volts)), null, this));
 
     SmartDashboard.putData(
-        "elevator quasistatic forward", sysIdRoutine.quasistatic(Direction.kForward));
+        "elevator quasistatic forward",
+        sysIdRoutine.quasistatic(Direction.kForward).withName("elevator quasistatic forward"));
     SmartDashboard.putData(
-        "elevator quasistatic backward", sysIdRoutine.quasistatic(Direction.kReverse));
-    SmartDashboard.putData("elevator dynamic forward", sysIdRoutine.dynamic(Direction.kForward));
-    SmartDashboard.putData("elevator dynamic backward", sysIdRoutine.dynamic(Direction.kReverse));
+        "elevator quasistatic backward",
+        sysIdRoutine.quasistatic(Direction.kReverse).withName("elevator quasistatic backward"));
+    SmartDashboard.putData(
+        "elevator dynamic forward",
+        sysIdRoutine.dynamic(Direction.kForward).withName("elevator dynamic forward"));
+    SmartDashboard.putData(
+        "elevator dynamic backward",
+        sysIdRoutine.dynamic(Direction.kReverse).withName("elevator dynamic backward"));
   }
 
   /**

diff --git a/src/main/java/org/sciborgs1155/robot/elevator/ElevatorConstants.java b/src/main/java/org/sciborgs1155/robot/elevator/ElevatorConstants.java
@@ -14,21 +14,21 @@
 import edu.wpi.first.units.measure.Mass;
 
 public class ElevatorConstants {
-  public static final double kP = 1;
+  public static final double kP = 5;
   public static final double kI = 0;
   public static final double kD = 0;
-  public static final double kS = 0;
-  public static final double kG = 0;
-  public static final double kV = 0;
-  public static final double kA = 0;
+  public static final double kS = 0.071465;
+  public static final double kG = 0.083107;
+  public static final double kV = 3.7759;
+  public static final double kA = 0.078693;
 
-  public static final Distance POSITION_TOLERANCE = Meters.of(.15);
+  public static final Distance POSITION_TOLERANCE = Meters.of(0.01);
 
   public static final Distance MIN_EXTENSION = Meters.of(0);
-  public static final Distance MAX_EXTENSION = Meters.of(1.4);
+  public static final Distance MAX_EXTENSION = Meters.of(1.405);
 
-  public static final LinearVelocity MAX_VELOCITY = MetersPerSecond.of(1);
-  public static final LinearAcceleration MAX_ACCEL = MetersPerSecondPerSecond.of(1);
+  public static final LinearVelocity MAX_VELOCITY = MetersPerSecond.of(2);
+  public static final LinearAcceleration MAX_ACCEL = MetersPerSecondPerSecond.of(2.8);
 
   public static final Mass WEIGHT = Pounds.of(6.142);
   public static final double GEARING = 9.375;

diff --git a/src/main/java/org/sciborgs1155/robot/led/LEDConstants.java b/src/main/java/org/sciborgs1155/robot/led/LEDConstants.java
@@ -6,7 +6,7 @@
 
 public class LEDConstants {
   // The length of the LED Strip.
-  public static final int LED_LENGTH = 64;
+  public static final int LED_LENGTH = 120;
   // The distance between two LEDs on the LED Strip.
   public static final Distance LED_SPACING = Meters.of(0.01);
 }