Mechanism2D in AdvantageScope testing

src/main/java/frc/robot/Controls.java

@@ -112,6 +112,14 @@ public Controls(Subsystems s, Sensors sensors, SuperStructure superStructure) {
     configureGroundSpinnyBindings();
     configureGroundArmBindings();
     configureSoloControllerBindings();
+    Shuffleboard.getTab("Elevator")
+        .addBoolean("Intaking mode Algae", () -> intakeMode == ScoringMode.ALGAE);
+    Shuffleboard.getTab("Elevator")
+        .addString("Scoring Mode", () -> getSoloScoringMode().toString());
+  }
+
+  public SoloScoringMode getSoloScoringMode() {
+    return soloScoringMode;
   }
 
   private Trigger connected(CommandXboxController controller) {
@@ -394,12 +402,7 @@ private void configureSuperStructureBindings() {
                           case ALGAE -> soloScoringMode = SoloScoringMode.ALGAE_IN_CLAW;
                         }
                       })
-                  .withName("Set solo scoring mode"));
-
-      sensors
-          .armSensor
-          .inClaw()
-          .and(RobotModeTriggers.teleop())
+                  .withName("Set solo scoring mode"))
           .onFalse(
               Commands.runOnce(
                       () -> {
@@ -457,19 +460,36 @@ private Command getCoralBranchHeightCommand() {
     };
   }
 
-  private Command getSoloCoralBranchHeightCommand() {
+  private Command getSoloCoralBranchHeightCommandL() {
+    return switch (branchHeight) {
+      case CORAL_LEVEL_FOUR -> superStructure
+          .coralLevelFour(soloController.rightBumper())
+          .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE);
+      case CORAL_LEVEL_THREE -> superStructure
+          .coralLevelThree(soloController.rightBumper(), () -> AutoAlign.poseInPlace())
+          .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE);
+      case CORAL_LEVEL_TWO -> superStructure
+          .coralLevelTwo(soloController.rightBumper(), () -> AutoAlign.poseInPlace())
+          .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE);
+      case CORAL_LEVEL_ONE -> superStructure
+          .coralLevelOne(soloController.rightBumper(), () -> AutoAlign.poseInPlaceL1L())
+          .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE);
+    };
+  }
+
+  private Command getSoloCoralBranchHeightCommandR() {
     return switch (branchHeight) {
       case CORAL_LEVEL_FOUR -> superStructure
           .coralLevelFour(soloController.rightBumper())
           .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE);
       case CORAL_LEVEL_THREE -> superStructure
-          .coralLevelThree(soloController.rightBumper())
+          .coralLevelThree(soloController.rightBumper(), () -> AutoAlign.poseInPlace())
           .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE);
       case CORAL_LEVEL_TWO -> superStructure
-          .coralLevelTwo(soloController.rightBumper())
+          .coralLevelTwo(soloController.rightBumper(), () -> AutoAlign.poseInPlace())
           .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE);
       case CORAL_LEVEL_ONE -> superStructure
-          .coralLevelOne(soloController.rightBumper())
+          .coralLevelOne(soloController.rightBumper(), () -> AutoAlign.poseInPlaceL1R())
           .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE);
     };
   }
@@ -926,7 +946,7 @@ private void configureSoloControllerBindings() {
                   switch (soloScoringMode) {
                     case CORAL_IN_CLAW -> {
                       scoreCommand =
-                          getSoloCoralBranchHeightCommand()
+                          getSoloCoralBranchHeightCommandL()
                               .until(
                                   () ->
                                       soloController.a().getAsBoolean()
@@ -969,6 +989,11 @@ private void configureSoloControllerBindings() {
         .and(() -> soloScoringMode == soloScoringMode.CORAL_IN_CLAW)
         .and(() -> branchHeight != BranchHeight.CORAL_LEVEL_ONE)
         .whileTrue(AutoAlign.autoAlignLeft(s.drivebaseSubsystem, this));
+    soloController
+        .leftTrigger()
+        .and(() -> soloScoringMode == soloScoringMode.CORAL_IN_CLAW)
+        .and(() -> branchHeight == BranchHeight.CORAL_LEVEL_ONE)
+        .whileTrue(AutoAlign.autoAlignL1L(s.drivebaseSubsystem, this));
     // Processor + Auto align right + Select scoring mode Coral
     soloController
         .rightTrigger()
@@ -977,7 +1002,7 @@ private void configureSoloControllerBindings() {
                     () -> {
                       Command scoreCommand =
                           switch (soloScoringMode) {
-                            case CORAL_IN_CLAW -> getSoloCoralBranchHeightCommand()
+                            case CORAL_IN_CLAW -> getSoloCoralBranchHeightCommandR()
                                 .until(
                                     () ->
                                         soloController.a().getAsBoolean()
@@ -1006,6 +1031,11 @@ private void configureSoloControllerBindings() {
         .and(() -> soloScoringMode == soloScoringMode.CORAL_IN_CLAW)
         .and(() -> branchHeight != BranchHeight.CORAL_LEVEL_ONE)
         .whileTrue(AutoAlign.autoAlignRight(s.drivebaseSubsystem, this));
+    soloController
+        .rightTrigger()
+        .and(() -> soloScoringMode == soloScoringMode.CORAL_IN_CLAW)
+        .and(() -> branchHeight == BranchHeight.CORAL_LEVEL_ONE)
+        .whileTrue(AutoAlign.autoAlignL1R(s.drivebaseSubsystem, this));
     // Ground Intake
     soloController
         .leftBumper()
@@ -1095,11 +1125,11 @@ private void configureSoloControllerBindings() {
                 .startMovingVoltage(
                     () -> Volts.of(ElevatorSubsystem.UP_VOLTAGE * -soloController.getLeftY()))
                 .withName("Elevator Manual Control"));
-    // Ready to score rumble
-    Trigger readyToScore = new Trigger(() -> AutoAlign.poseInPlace());
+    // Ready to score rumble (Removal for autoscoring)
+    /* Trigger readyToScore = new Trigger(() -> AutoAlign.poseInPlace());
     readyToScore.onTrue(
         Commands.runOnce(() -> soloController.setRumble(RumbleType.kBothRumble, 1.0)));
     readyToScore.onFalse(
-        Commands.runOnce(() -> soloController.setRumble(RumbleType.kBothRumble, 0.0)));
+        Commands.runOnce(() -> soloController.setRumble(RumbleType.kBothRumble, 0.0))); */
   }
 }

src/main/java/frc/robot/Robot.java

@@ -6,6 +6,7 @@
 
 import au.grapplerobotics.CanBridge;
 import com.pathplanner.lib.commands.FollowPathCommand;
+import edu.wpi.first.math.util.Units;
 import edu.wpi.first.net.WebServer;
 import edu.wpi.first.wpilibj.DataLogManager;
 import edu.wpi.first.wpilibj.DriverStation;
@@ -16,7 +17,12 @@
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj.livewindow.LiveWindow;
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
+import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d;
+import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d;
+import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj.util.Color;
+import edu.wpi.first.wpilibj.util.Color8Bit;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import frc.robot.Subsystems.SubsystemConstants;
 import frc.robot.subsystems.SuperStructure;
@@ -43,6 +49,11 @@ public static Robot getInstance() {
   public final SuperStructure superStructure;
   private final PowerDistribution PDH;
 
+  Mechanism2d mech;
+  MechanismRoot2d root;
+  MechanismLigament2d m_elevator;
+  MechanismLigament2d m_wrist;
+
   protected Robot() {
     // non public for singleton. Protected so test class can subclass
 
@@ -52,6 +63,19 @@ protected Robot() {
     PDH = new PowerDistribution(Hardware.PDH_ID, ModuleType.kRev);
     LiveWindow.disableAllTelemetry();
     LiveWindow.enableTelemetry(PDH);
+    mech = new Mechanism2d(1, 2);
+    root = mech.getRoot("climber", 0.5 + Units.inchesToMeters(5.5), Units.inchesToMeters(19.5));
+    SmartDashboard.putData("Mechanism", mech);
+    m_elevator =
+        root.append(new MechanismLigament2d("elevator", 1, 90, 2, new Color8Bit(Color.kRed)));
+    var pivot =
+        m_elevator.append(
+            new MechanismLigament2d(
+                "pivot offset", Units.inchesToMeters(4), -90, 2, new Color8Bit(Color.kDarkRed)));
+    m_wrist =
+        pivot.append(
+            new MechanismLigament2d(
+                "wrist", Units.inchesToMeters(14.5), 270, 6, new Color8Bit(Color.kFirstRed)));
 
     sensors = new Sensors();
     subsystems = new Subsystems(sensors);
@@ -117,6 +141,8 @@ public void robotPeriodic() {
     if (subsystems.visionSubsystem != null) {
       subsystems.visionSubsystem.update();
     }
+    m_elevator.setLength(subsystems.elevatorSubsystem.getHeightMeters());
+    m_wrist.setAngle(subsystems.armPivotSubsystem.getAngle());
   }
 
   @Override

src/main/java/frc/robot/subsystems/ArmPivot.java

@@ -15,6 +15,7 @@
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.filter.Debouncer;
 import edu.wpi.first.math.filter.Debouncer.DebounceType;
+import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.units.measure.Voltage;
 import edu.wpi.first.wpilibj.Alert;
@@ -146,6 +147,10 @@ private double getCurrentPosition() {
     return curPos.getValueAsDouble();
   }
 
+  public Rotation2d getAngle() { // 0 is forward, + is up
+    return Rotation2d.fromRotations(getCurrentPosition());
+  }
+  // preset command placeholder
   /*  moves arm to the input position
         - sets the target position to the inputted position
         - shrinks the difference between the current position and the target position until they are close enough to work

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

@@ -9,13 +9,22 @@
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.filter.Debouncer;
 import edu.wpi.first.math.filter.Debouncer.DebounceType;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.networktables.DoublePublisher;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.networktables.StructPublisher;
 import edu.wpi.first.units.Units;
 import edu.wpi.first.units.measure.MutVoltage;
+import edu.wpi.first.units.measure.Time;
 import edu.wpi.first.units.measure.Voltage;
 import edu.wpi.first.wpilibj.Alert;
 import edu.wpi.first.wpilibj.Alert.AlertType;
 import edu.wpi.first.wpilibj.RobotBase;
 import edu.wpi.first.wpilibj.RobotController;
+import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
 import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog;
 import edu.wpi.first.wpilibj2.command.Command;
@@ -24,6 +33,12 @@
 import edu.wpi.first.wpilibj2.command.button.Trigger;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import frc.robot.Hardware;
+import frc.robot.Robot;
+import frc.robot.sensors.ArmSensor;
+import frc.robot.subsystems.auto.AutoLogic;
+import jdk.jfr.Timestamp;
+
+import java.util.concurrent.TimeUnit;
 import java.util.function.DoubleConsumer;
 import java.util.function.DoubleSupplier;
 import java.util.function.Supplier;
@@ -52,6 +67,7 @@ public class ElevatorSubsystem extends SubsystemBase {
   public static final double CORAL_QUICK_INTAKE = 1.6;
   public static final double MIN_EMPTY_GROUND_INTAKE = 4.5;
   public static final double MIN_FULL_GROUND_INTAKE = 8.0;
+  private static final double MOTOR_ROTATIONS_PER_METER = 19.68; // Inaccurate
   public static final double MANUAL = 0.1;
   private static final double POS_TOLERANCE = 0.1;
   private final double ELEVATOR_KP = 7.804;
@@ -90,6 +106,11 @@ public class ElevatorSubsystem extends SubsystemBase {
       new Alert("Elevator", "Motor 2 not connected", AlertType.kError);
   private final Debouncer notConnectedDebouncerOne = new Debouncer(.1, DebounceType.kBoth);
   private final Debouncer notConnectedDebouncerTwo = new Debouncer(.1, DebounceType.kBoth);
+  private  StructPublisher<Pose3d> elevatorPose3d = NetworkTableInstance.getDefault().getStructTopic("elevator/heightPose", Pose3d.struct).publish();
+  public  StructPublisher<Pose3d> TESTpose = NetworkTableInstance.getDefault().getStructTopic("debug/TEST", Pose3d.struct).publish();
+  //public  StructPublisher<Pose3d> TESTpose2 = NetworkTableInstance.getDefault().getStructTopic("debug/TEST2", Pose3d.struct).publish();
+
+
 
   // Creates a SysIdRoutine
   SysIdRoutine routine =
@@ -107,6 +128,8 @@ public ElevatorSubsystem() {
     motorConfigs();
 
     Shuffleboard.getTab("Elevator").addDouble("Motor Current Position", () -> getCurrentPosition());
+    //Elevator pose test
+
     Shuffleboard.getTab("Elevator").addDouble("Target Position", () -> getTargetPosition());
     Shuffleboard.getTab("Elevator")
         .addDouble("M1 supply current", () -> m_motor.getSupplyCurrent().getValueAsDouble());
@@ -133,6 +156,16 @@ public ElevatorSubsystem() {
             "M2 at reverse softstop", () -> m_motor2.getFault_ReverseSoftLimit().getValue());
     Shuffleboard.getTab("Elevator")
         .addDouble("Elevator Speed", () -> m_motor.getVelocity().getValueAsDouble());
+
+    // Test commands
+    Shuffleboard.getTab("Elevator")
+        .add("Move to Level Four", setLevel(CORAL_LEVEL_FOUR_PRE_POS));
+    Shuffleboard.getTab("Elevator")
+        .add("Move to Level Three", setLevel(CORAL_LEVEL_THREE_PRE_POS));
+    Shuffleboard.getTab("Elevator")
+        .add("Move to Level Two", setLevel(CORAL_LEVEL_TWO_PRE_POS));
+    Shuffleboard.getTab("Elevator")
+        .add("Move to Level One", setLevel(CORAL_LEVEL_ONE_POS));
   }
 
   public Command sysIdQuasistatic(SysIdRoutine.Direction direction) {
@@ -249,6 +282,10 @@ private double getCurrentPosition() {
     return curPos;
   }
 
+  public double getHeightMeters() { // Elevator height converted to Meters
+    return getCurrentPosition() / MOTOR_ROTATIONS_PER_METER;
+  }
+
   private void setCurrentPosition(double pos) {
     m_motor.setPosition(pos);
   }
@@ -270,6 +307,7 @@ public Command setLevel(double pos) {
     return runOnce(
             () -> {
               if (hasBeenZeroed) {
+                System.out.println("Setting elevator level to: " + pos);
                 m_motor.setControl(m_request.withPosition(pos));
                 m_motor2.setControl(new Follower(m_motor.getDeviceID(), true));
                 targetPos = pos;
@@ -360,16 +398,33 @@ public Command stop() {
         .ignoringDisable(true)
         .withName("ElevatorStop");
   }
-
+  double smoothedAngleZ = 0.4;
+  double smoothingFactor = 0.1;
   @Override
   public void periodic() {
     NotConnectedError.set(
         notConnectedDebouncerOne.calculate(!m_motor.getMotorVoltage().hasUpdated()));
     NotConnectedError2.set(
         notConnectedDebouncerTwo.calculate(!m_motor2.getMotorVoltage().hasUpdated()));
     if (RobotBase.isSimulation()) {
-      m_motorOneSimState.setRawRotorPosition(targetPos);
-      m_motorTwoSimState.setRawRotorPosition(targetPos);
+        if (!Robot.getInstance().sensors.armSensor.booleanInClaw()) {
+        }
+        m_motorOneSimState.setRawRotorPosition(targetPos);
+        m_motorTwoSimState.setRawRotorPosition(targetPos);
+        //elevatorPose3d.set(new Pose3d(0.0, 0.0, getHeightMeters(), new Rotation3d()));
+
+        double curPos = getCurrentPosition();
+        double smoothingFactor = 0.5;// Percentage Scaler
+        double bottomZ = 0.2;
+        double topZ = 1.55;
+        double minPos = 0.0;
+        double maxPos = 37.5;
+        double targetZ = (bottomZ + ((curPos - minPos) / (maxPos - minPos)) * (topZ - bottomZ));
+
+        TESTpose.set(new Pose3d(
+          0.2, 0.0, targetZ,
+          new Rotation3d(0.0, 0.0, -135)));
     }
+
   }
 }