mechanism code

Author: MichaelLesirge

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

@@ -4,6 +4,7 @@
 
 import com.pathplanner.lib.auto.AutoBuilder;
 import com.pathplanner.lib.auto.NamedCommands;
+import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Transform2d;
@@ -68,6 +69,7 @@
 import frc.robot.utility.commands.CustomCommands;
 import java.util.Arrays;
 import java.util.function.BiConsumer;
+import java.util.function.DoubleSupplier;
 import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
 
 /**
@@ -476,23 +478,22 @@ private void configureDriverControllerBindings(
   private void configureOperatorControllerBindings(CommandXboxController xbox) {
 
     new Trigger(DriverStation::isEnabled)
-        .onTrue(
-            Commands.runOnce(() -> coralWrist.setConstraints(1, 3))
-                .andThen(superstructure.runAction(Superstructure.State.STOW))
-                .finallyDo(coralWrist::resetContraints));
+        .onTrue(superstructure.runAction(Superstructure.State.STOW_HIGH));
 
     xbox.back().onTrue(drive.runOnce(drive::stop).withName("CANCEL and stop"));
 
     // Primary scoring
     xbox.leftTrigger()
         .whileTrue(superstructure.run(State.INTAKE).alongWith(superstructure.intake()));
 
-    xbox.rightTrigger().and(xbox.a().negate()).whileTrue(superstructure.outtake());
-    xbox.rightTrigger().and(xbox.a()).whileTrue(superstructure.outtakeL1());
-
     final BiConsumer<Trigger, Superstructure.State> configureOperatorControllerBindingLevel =
         (trigger, level) -> {
-          trigger.whileTrue(superstructure.run(level));
+          trigger.whileTrue(
+              superstructure
+                  .run(level)
+                  .alongWith(
+                      Commands.waitUntil(superstructure::atGoal)
+                          .andThen(superstructure.runWheels(level).asProxy())));
         };
 
     configureOperatorControllerBindingLevel.accept(xbox.y(), Superstructure.State.L4);
@@ -505,11 +506,17 @@ private void configureOperatorControllerBindings(CommandXboxController xbox) {
     // Intake
 
     coralIntake.setDefaultCommand(superstructure.passiveIntake());
-    new Trigger(() -> !JoystickUtil.isDeadband(xbox.getLeftX()))
+
+    DoubleSupplier intakeSpeed =
+        () ->
+            MathUtil.clamp(
+                JoystickUtil.applyDeadband(-xbox.getLeftX())
+                    + JoystickUtil.applyDeadband(-xbox.getLeftY()),
+                -1,
+                +1);
+    new Trigger(() -> intakeSpeed.getAsDouble() != 0)
         .and(DriverStation::isTeleopEnabled)
-        .whileTrue(
-            Commands.run(
-                () -> coralIntake.setMotors(JoystickUtil.applyDeadband(xbox.getRightX()))));
+        .whileTrue(coralIntake.run(() -> coralIntake.setMotors(intakeSpeed.getAsDouble())));
 
     // Hang
     hang.setDefaultCommand(hang.run(() -> hang.set(JoystickUtil.applyDeadband(xbox.getLeftX()))));

src/main/java/frc/robot/subsystems/superstructure/Superstructure.java

@@ -73,17 +73,11 @@ public Superstructure(Elevator elevator, Wrist coralWrist, Intake coralIntake) {
   public Command run(State goal) {
     return runPrepare(goal)
         .andThen(Commands.idle(elevator, coralWrist))
-        .finallyDo(
-            () -> {
-              elevator.setGoalHeightMeters(STOW_HEIGHT);
-              coralWrist.setGoalRotation(STOW_CORAL_ANGLE);
-            });
+        .finallyDo(this::setPositionStow);
   }
 
   public Command runAction(State newGoal) {
-    return runPrepare(newGoal)
-        .andThen(Commands.idle(elevator, coralWrist))
-        .until(() -> elevator.atGoalHeight() && coralWrist.atGoal());
+    return runPrepare(newGoal).andThen(Commands.idle(elevator, coralWrist)).until(this::atGoal);
   }
 
   public Command runPrepare(State goal) {
@@ -98,6 +92,18 @@ public Command runPrepare(State goal) {
     };
   }
 
+  public Command runWheels(State goal) {
+    return switch (goal) {
+      case STOW -> stopIntake();
+      case STOW_HIGH -> stopIntake();
+      case L1 -> outtakeL1();
+      case L2 -> outtake();
+      case L3 -> outtake();
+      case L4 -> outtake();
+      case INTAKE -> intake();
+    };
+  }
+
   public static final double L1_HEIGHT = 0.151148670108898;
   public static final Rotation2d L1_CORAL_ANGLE = Rotation2d.fromDegrees(0);
 
@@ -115,6 +121,8 @@ public Command runPrepare(State goal) {
 
   public static final double STOW_HEIGHT = 0.054886473109919;
   public static final Rotation2d STOW_CORAL_ANGLE = Rotation2d.fromDegrees(-90);
+
+  public static final double STOW_HEIGHT_HIGH = 0;
   public static final Rotation2d STOW_CORAL_ANGLE_HIGH = Rotation2d.fromDegrees(90);
 
   public Command prepareL1() {
@@ -150,7 +158,7 @@ public Command stowLow() {
 
   public Command stowHigh() {
     return Commands.parallel(
-        elevator.runPositionPrepare(STOW_HEIGHT),
+        elevator.runPositionPrepare(STOW_HEIGHT_HIGH),
         coralWrist.runPositionPrepare(STOW_CORAL_ANGLE_HIGH));
   }
 
@@ -174,6 +182,15 @@ public Command stopIntake() {
     return coralIntake.runMotors(0);
   }
 
+  public boolean atGoal() {
+    return elevator.atGoalHeight() && coralWrist.atGoal();
+  }
+
+  public void setPositionStow() {
+    elevator.setGoalHeightMeters(STOW_HEIGHT_HIGH);
+    coralWrist.setGoalRotation(STOW_CORAL_ANGLE_HIGH);
+  }
+
   @Override
   public void periodic() {
     SmartDashboard.putNumber("Elevator Height Meters", elevator.getMeasuredHeightMeters());

src/main/java/frc/robot/subsystems/superstructure/SuperstructureVisualizer.java

@@ -15,11 +15,11 @@ public class SuperstructureVisualizer {
 
   private final LoggedMechanism2d mechanism =
       new LoggedMechanism2d(
-          Units.inchesToMeters(28.0), Units.feetToMeters(9.0), new Color8Bit(Color.kDarkGray));
+          Units.inchesToMeters(28.0), Units.feetToMeters(5.0), new Color8Bit(Color.kDarkGray));
 
   private final LoggedMechanismRoot2d root;
-  private final LoggedMechanismLigament2d carriageMechanism;
   private final LoggedMechanismLigament2d elevatorMechanism;
+  private final LoggedMechanismLigament2d carriage, carriageJoint;
   private final LoggedMechanismLigament2d coralWristLigament;
   private final LoggedMechanismLigament2d coralIntake;
 
@@ -33,7 +33,7 @@ public SuperstructureVisualizer(String name, Color color) {
             new LoggedMechanismLigament2d(
                 name + " Elevator", Units.inchesToMeters(26.0), 90, 4.0, new Color8Bit(color)));
 
-    carriageMechanism =
+    carriage =
         elevatorMechanism.append(
             new LoggedMechanismLigament2d(
                 name + " Carriage",
@@ -42,10 +42,19 @@ public SuperstructureVisualizer(String name, Color color) {
                 4.0,
                 new Color8Bit(color)));
 
+    carriageJoint =
+        carriage.append(
+            new LoggedMechanismLigament2d(
+                name + " Carriage Joint",
+                Units.inchesToMeters(4),
+                90.0,
+                4.0,
+                new Color8Bit(color)));
+
     coralWristLigament =
-        carriageMechanism.append(
+        carriageJoint.append(
             new LoggedMechanismLigament2d(
-                name + " Algae Wrist", Units.inchesToMeters(9.0), 0.0, 4.0, new Color8Bit(color)));
+                name + " Algae Wrist", Units.inchesToMeters(9.0), 0, 4.0, new Color8Bit(color)));
 
     coralIntake =
         coralWristLigament.append(
@@ -55,7 +64,7 @@ public SuperstructureVisualizer(String name, Color color) {
 
   public void update(double carriageHeight, double wristRadians, double coralIntakeOutput) {
     elevatorMechanism.setLength(carriageHeight);
-    coralWristLigament.setAngle(new Rotation2d(-wristRadians).plus(Rotation2d.kCCW_Pi_2));
+    coralWristLigament.setAngle(new Rotation2d(-wristRadians));
 
     coralIntake.setLength(coralIntakeOutput * Units.inchesToMeters(3.0));
 

src/main/java/frc/robot/subsystems/superstructure/elevator/Elevator.java

@@ -7,6 +7,7 @@
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints;
 import edu.wpi.first.math.trajectory.TrapezoidProfile.State;
+import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.Alert;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.Timer;
@@ -45,13 +46,14 @@ public class Elevator extends SubsystemBase {
       factory.getNumber("MaxAcceleration", ElevatorConstants.maxCarriageAcceleration);
 
   private static final LoggedTunableNumber tolerance =
-      factory.getNumber("Tolerance", ElevatorConstants.carriagePositionTolerance);
+      factory.getNumber(
+          "ToleranceInches", Units.metersToInches(ElevatorConstants.carriagePositionTolerance));
 
   private static final LoggedTunableNumber staticCharacterizationVelocityThresh =
-      new LoggedTunableNumber("Elevator/StaticCharacterizationVelocityThresh", 0.1);
+      factory.getNumber("/StaticCharacterizationVelocityThresh", 0.1);
 
   private static final LoggedTunableNumber hardStopVelocityThresh =
-      new LoggedTunableNumber("Elevator/StaticCharacterizationVelocityThresh", 0.05);
+      factory.getNumber("/StaticCharacterizationVelocityThresh", 0.05);
 
   private final ElevatorIO io;
   private final ElevatorIOInputsAutoLogged inputs = new ElevatorIOInputsAutoLogged();
@@ -144,10 +146,11 @@ public void periodic() {
 
       setpoint = profile.calculate(Constants.LOOP_PERIOD_SECONDS, setpoint, goal);
 
-      io.runPosition(
-          setpoint.position / ElevatorConstants.drumRadius,
-          feedforward.calculate(setpoint.velocity));
+      double feedforwardVolts = feedforward.calculate(setpoint.velocity);
 
+      io.runPosition(setpoint.position / ElevatorConstants.drumRadius, feedforwardVolts);
+
+      Logger.recordOutput("Wrist/Feedforward/Volts", feedforwardVolts);
       Logger.recordOutput("Elevator/Profile/SetpointPositionMeters", setpoint.position);
       Logger.recordOutput("Elevator/Profile/SetpointVelocityMetersPerSec", setpoint.velocity);
       Logger.recordOutput("Elevator/Profile/GoalPositionMeters", goal.position);
@@ -156,6 +159,7 @@ public void periodic() {
     } else {
       Logger.recordOutput("Elevator/Profile/ShouldRunProfiled", false);
 
+      Logger.recordOutput("Wrist/FeedForward/Volts", 0);
       Logger.recordOutput("Elevator/Profile/SetpointPositionMeters", 0.0);
       Logger.recordOutput("Elevator/Profile/SetpointVelocityMetersPerSec", 0.0);
       Logger.recordOutput("Elevator/Profile/GoalPositionMeters", 0.0);
@@ -199,7 +203,8 @@ public double getHeightPercent() {
 
   @AutoLogOutput(key = "Elevator/atGoal")
   public boolean atGoalHeight() {
-    return MathUtil.isNear(getGoalHeightMeters(), getGoalHeightMeters(), tolerance.get());
+    return MathUtil.isNear(
+        getMeasuredHeightMeters(), getGoalHeightMeters(), Units.inchesToMeters(tolerance.get()));
   }
 
   public State getGoal() {

src/main/java/frc/robot/subsystems/superstructure/elevator/ElevatorConstants.java

@@ -31,7 +31,7 @@ public static record ElevatorConfig(int leaderCanId, int followerCanId, boolean
   public static final double maxCarriageVelocity = 3.0;
   public static final double maxCarriageAcceleration = 3.5;
 
-  public static final double carriagePositionTolerance = Units.inchesToMeters(1);
+  public static final double carriagePositionTolerance = Units.inchesToMeters(0.2);
 
   public static final PIDConstants pid =
       switch (Constants.getRobot()) {

src/main/java/frc/robot/subsystems/superstructure/elevator/ElevatorIOSim.java

@@ -6,7 +6,6 @@
 import edu.wpi.first.math.system.plant.LinearSystemId;
 import edu.wpi.first.wpilibj.simulation.ElevatorSim;
 import frc.robot.Constants;
-import java.util.Random;
 
 /** Simulation implementation of the TemplateIO. */
 public class ElevatorIOSim implements ElevatorIO {
@@ -24,7 +23,7 @@ public class ElevatorIOSim implements ElevatorIO {
           0.0,
           ElevatorConstants.carriageMaxHeight,
           true,
-          new Random().nextDouble() * ElevatorConstants.carriageMaxHeight);
+          0);
 
   private double appliedVolts = 0.0;
 

src/main/java/frc/robot/subsystems/superstructure/wrist/Wrist.java

@@ -40,6 +40,9 @@ public class Wrist extends SubsystemBase {
   private static final LoggedTunableNumber kA =
       factory.getNumber("kA", WristConstants.FEEDFORWARD.kA());
 
+  private static final LoggedTunableNumber toleranceDegrees =
+      factory.getNumber("ToleranceDegrees", WristConstants.TOLERANCE_DEGREES);
+
   private static final LoggedTunableNumber maxVelocity =
       factory.getNumber("maxVelocity", WristConstants.MAX_VELOCITY);
   private static final LoggedTunableNumber maxAcceleration =
@@ -51,7 +54,7 @@ public class Wrist extends SubsystemBase {
   private TrapezoidProfile profile;
   private ArmFeedforward feedforward;
 
-  private State setpoint = new State();
+  private State setpoint = null;
   private Supplier<State> goalSupplier = State::new;
 
   private final Alert motorConnectedAlert =
@@ -67,7 +70,6 @@ public Wrist(WristIO io) {
 
     io.setPID(kP.get(), kI.get(), kD.get());
     io.setBrakeMode(true);
-
     this.feedforward = new ArmFeedforward(kS.get(), kG.get(), kV.get(), kA.get());
   }
 
@@ -79,20 +81,36 @@ public void periodic() {
 
     io.setBrakeMode(!disabledDebouncer.calculate(DriverStation.isDisabled()));
 
-    setpoint = profile.calculate(Constants.LOOP_PERIOD_SECONDS, setpoint, goalSupplier.get());
+    if (DriverStation.isEnabled()) {
+      Logger.recordOutput("Elevator/shouldRunProfiled", true);
+
+      if (setpoint == null) {
+        setpoint = new State(inputs.positionRad, inputs.velocityRadPerSec);
+      }
+
+      setpoint = profile.calculate(Constants.LOOP_PERIOD_SECONDS, setpoint, goalSupplier.get());
+
+      double feedforwardVolts = feedforward.calculate(setpoint.position, setpoint.velocity);
+
+      io.runPosition(setpoint.position, feedforwardVolts);
 
-    double feedforwardVolts = feedforward.calculate(setpoint.position, setpoint.velocity);
+      Logger.recordOutput("Wrist/Feedforward/Volts", feedforwardVolts);
 
-    io.runPosition(setpoint.position, feedforwardVolts);
+      Logger.recordOutput("Wrist/Profile/SetpointPositionRotations", setpoint.position);
+      Logger.recordOutput("Wrist/Profile/SetpointVelocityRPM", setpoint.velocity);
+      Logger.recordOutput("Wrist/Profile/GoalPositionRotations", goalSupplier.get().position);
+      Logger.recordOutput("Wrist/Profile/GoalVelocityRPM", goalSupplier.get().velocity);
+    } else {
+      Logger.recordOutput("Elevator/shouldRunProfiled", false);
 
-    Logger.recordOutput("Wrist/FeedForward/Volts", feedforwardVolts);
-    Logger.recordOutput(
-        "Wrist/FeedForward/MeasuredVolts[Test]", feedforward.calculate(inputs.positionRad, 0));
+      Logger.recordOutput("Wrist/FeedForward/Volts", 0);
+      Logger.recordOutput("Elevator/Profile/SetpointPositionMeters", 0.0);
+      Logger.recordOutput("Elevator/Profile/SetpointVelocityMetersPerSec", 0.0);
+      Logger.recordOutput("Elevator/Profile/GoalPositionMeters", 0.0);
+      Logger.recordOutput("Elevator/Profile/GoalVelocityMetersPerSec", 0.0);
 
-    Logger.recordOutput("Wrist/Profile/SetpointPositionRotations", setpoint.position);
-    Logger.recordOutput("Wrist/Profile/SetpointVelocityRPM", setpoint.velocity);
-    Logger.recordOutput("Wrist/Profile/GoalPositionRotations", goalSupplier.get().position);
-    Logger.recordOutput("Wrist/Profile/GoalVelocityRPM", goalSupplier.get().velocity);
+      setpoint = new State(inputs.positionRad, inputs.velocityRadPerSec);
+    }
 
     LoggedTunableNumber.ifChanged(
         hashCode(),
@@ -140,7 +158,9 @@ public void setGoalSupplier(Supplier<State> positionSupplier) {
   @AutoLogOutput(key = "Wrist/atGoal")
   public boolean atGoal() {
     return MathUtil.isNear(
-        inputs.positionRad, goalSupplier.get().position, WristConstants.TOLERANCE_DEGREES);
+        inputs.positionRad,
+        goalSupplier.get().position,
+        Units.degreesToRadians(toleranceDegrees.get()));
   }
 
   /** Get position in rotations */
@@ -175,7 +195,7 @@ public void setConstraints(double maxVelocity, double maxAcceleration) {
     profile = new TrapezoidProfile(new Constraints(maxVelocity, maxAcceleration));
   }
 
-  public void resetContraints() {
+  public void resetConstraints() {
     setConstraints(WristConstants.MAX_VELOCITY, WristConstants.MAX_ACCELERATION);
   }
 }