From 3c512c9f8b802a18c5c3d4b1d872973b0c45ad16 Mon Sep 17 00:00:00 2001
From: eileha <eileenhan369@gmail.com>
Date: Thu, 5 Feb 2026 15:35:02 -0800
Subject: [PATCH] d

---
 networktables.json.bck                        | 10 +++++++
 src/main/java/frc/robot/Robot.java            |  3 ++-
 src/main/java/frc/robot/RobotContainer.java   |  4 +++
 .../frc/robot/subsystems/Intake/Intake.java   | 27 ++++++++++++++++++-
 4 files changed, 42 insertions(+), 2 deletions(-)
 create mode 100644 networktables.json.bck

diff --git a/networktables.json.bck b/networktables.json.bck
new file mode 100644
index 0000000..cf5062b
--- /dev/null
+++ b/networktables.json.bck
@@ -0,0 +1,10 @@
+[
+  {
+    "name": "/Preferences/RobotId",
+    "type": "string",
+    "value": "default",
+    "properties": {
+      "persistent": true
+    }
+  }
+]
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 9030802..9e95508 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -74,7 +74,8 @@ public class Robot extends LoggedRobot {
         //   SimGUI: Persistent Values, Preferences, RobotId, then restart Simulation
         //     changes networktables.json, networktables.json.bck (both Untracked)
         //   Uncomment the next line, set the desired RobotId, deploy, and then comment the line out
-        //  RobotId.setRobotId(RobotId.SwerveCompetition);
+        
+        RobotId.setRobotId(RobotId.WaffleHouse);
         DriveConstants.update(RobotId.getRobotId());
         RobotController.setBrownoutVoltage(6.0);
         // obtain this robot's identity
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 5f0e062..155016a 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -26,6 +26,7 @@ import frc.robot.constants.VisionConstants;
 import frc.robot.controls.BaseDriverConfig;
 import frc.robot.controls.Operator;
 import frc.robot.controls.PS5ControllerDriverConfig;
+import frc.robot.subsystems.Intake.Intake;
 import frc.robot.subsystems.drivetrain.Drivetrain;
 import frc.robot.subsystems.drivetrain.GyroIOPigeon2;
 import frc.robot.subsystems.indexer.Spindexer;
@@ -52,6 +53,7 @@ public class RobotContainer {
   // Controllers are defined here
   private BaseDriverConfig driver = null;
   private Operator operator = null;
+  private Intake intake = null;
 
   /**
    * The container for the robot. Contains subsystems, OI devices, and commands.
@@ -70,6 +72,8 @@ public class RobotContainer {
       default:
 
       case WaffleHouse:
+        intake = new Intake();
+
       
       case SwerveCompetition: // AKA "Vantage"
 
diff --git a/src/main/java/frc/robot/subsystems/Intake/Intake.java b/src/main/java/frc/robot/subsystems/Intake/Intake.java
index e4fae86..6803e12 100644
--- a/src/main/java/frc/robot/subsystems/Intake/Intake.java
+++ b/src/main/java/frc/robot/subsystems/Intake/Intake.java
@@ -10,13 +10,18 @@ import com.ctre.phoenix6.signals.MotorArrangementValue;
 import com.ctre.phoenix6.signals.NeutralModeValue;
 import com.revrobotics.spark.config.SparkMaxConfig;
 
-
+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.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
 
 public class Intake extends SubsystemBase {
+
+    private Mechanism2d mechanism;
+    private MechanismLigament2d mechanismLigament2d;
     // set actual IDs
     final int rightID = 1;
     final int leftID = 2;
@@ -29,12 +34,18 @@ public class Intake extends SubsystemBase {
     private double startingPoint; // this should go in a constants file
     private MotionMagicVoltage voltageRequest = new MotionMagicVoltage(0);
 
+    final MechanismLigament2d extensionLigament;
+    final double kMaxRotations = 37.5;
+    final double kMaxVisualLength = 0.75;
+
 
     public Intake() {
         rightMotor = new TalonFX(rightID);
         leftMotor = new TalonFX(leftID);
         rollerMotor = new TalonFX(rollerID); 
 
+        
+
         // right motor configs
         TalonFXConfiguration Config = new TalonFXConfiguration();
         var slot0Configs = Config.Slot0;
@@ -60,6 +71,16 @@ public class Intake extends SubsystemBase {
         SmartDashboard.putData("Extend Intake", new InstantCommand(() -> extend()));
         SmartDashboard.putData("Retract Intake", new InstantCommand(() -> retract()));
 
+        
+
+        Mechanism2d mechanism = new Mechanism2d(1.2, 0.6);
+
+        MechanismRoot2d root = mechanism.getRoot("ExtensionRoot", 0.1, 0.3);
+
+        extensionLigament = root.append(new MechanismLigament2d("Extension", 0.0, 0.0)); // horizontal
+
+        SmartDashboard.putData("Extension Mechanism", mechanism);
+
        
     }
 
@@ -68,7 +89,11 @@ public class Intake extends SubsystemBase {
     }
 
     public void simulationPeriodic(){
+        double percentExtended = getPosition() / kMaxRotations;
+
+        percentExtended = Math.max(0.0, Math.min(1.0, percentExtended));
 
+        extensionLigament.setLength(percentExtended * kMaxVisualLength);
     }
 
     /**
-- 
2.39.5