From 642eaf9baf5b4891cc068a9828a7540d7f8d82c9 Mon Sep 17 00:00:00 2001
From: Saara21 <113394225+Saara21@users.noreply.github.com>
Date: Thu, 5 Feb 2026 16:28:39 -0800
Subject: [PATCH] working sim pt 2

---
 networktables.json.bck                        | 11 +++++++-
 src/main/java/frc/robot/RobotContainer.java   |  1 +
 .../frc/robot/subsystems/Intake/Intake.java   | 28 +++++++++++--------
 3 files changed, 28 insertions(+), 12 deletions(-)

diff --git a/networktables.json.bck b/networktables.json.bck
index fe51488..eefbca6 100644
--- a/networktables.json.bck
+++ b/networktables.json.bck
@@ -1 +1,10 @@
-[]
+[
+  {
+    "name": "/Preferences/RobotId",
+    "type": "string",
+    "value": "TestBed1",
+    "properties": {
+      "persistent": true
+    }
+  }
+]
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 155016a..df4b7b1 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -64,6 +64,7 @@ public class RobotContainer {
     // dispatch on the robot
     switch (robotId) {
       case TestBed1:
+        intake = new Intake();
         break;
 
       case TestBed2:
diff --git a/src/main/java/frc/robot/subsystems/Intake/Intake.java b/src/main/java/frc/robot/subsystems/Intake/Intake.java
index a612292..4e8e807 100644
--- a/src/main/java/frc/robot/subsystems/Intake/Intake.java
+++ b/src/main/java/frc/robot/subsystems/Intake/Intake.java
@@ -78,28 +78,30 @@ public class Intake extends SubsystemBase {
         mechanism = new Mechanism2d(80, 80);
         MechanismRoot2d root = mechanism.getRoot("Root", 0, 1);
         robotPos = root.append(new MechanismLigament2d("robotPos", 40, 0.0, 1, new Color8Bit(0, 0, 0)));
-        robotFrame = robotPos.append(new MechanismLigament2d("Robot Frame",28,0.0, 2, new Color8Bit(0, 0, 255)));
-        robotHeight = robotPos.append(new MechanismLigament2d("Robot Height", 22.5, 90, 1, new Color8Bit(0,0,255)));
-        robotExtension = robotHeight.append(new MechanismLigament2d("Robot Extension", 12, 180,2, new Color8Bit(255, 0, 0) ));
+        robotFrame = robotPos.append(new MechanismLigament2d("Robot Frame",28,0.0, 2, new Color8Bit(0, 255, 255)));
+        robotHeight = robotPos.append(new MechanismLigament2d("Robot Height", 22.5, 90, 1, new Color8Bit(0,255,255)));
+        // extensiion is initially retracted.
+        robotExtension = robotHeight.append(new MechanismLigament2d("Robot Extension", 0, 90, 2, new Color8Bit(255, 0, 0) ));
+
         SmartDashboard.putData("Extension Mechanism", mechanism);
         SmartDashboard.putData("Extend Intake", new InstantCommand(this::extend));
         SmartDashboard.putData("Retract Intake", new InstantCommand(this::retract));
-}
-
-
-       
-    
+    }
 
     public void periodic() {
         SmartDashboard.putNumber("Intake Position:", getPosition());
-    }
 
-    public void simulationPeriodic(){
+        // report the position of the extension
         double percentExtended = getPosition() / kMaxRotations;
 
         percentExtended = Math.max(0.0, Math.min(1.0, percentExtended));
 
-        robotExtension.setLength(percentExtended * maxExtension);
+        // robotExtension.setLength(percentExtended * maxExtension);
+
+    }
+
+    public void simulationPeriodic(){
+        // simulate the motor activity
     }
 
     /**
@@ -108,6 +110,9 @@ public class Intake extends SubsystemBase {
      */
     public void setPosition(double setpoint) {
         rightMotor.setControl(voltageRequest.withPosition(setpoint));
+
+        // set the position quickly (should be in simultation and move slowly)
+        robotExtension.setLength(12.0 * setpoint/kMaxRotations);
     }
 
     /**
@@ -126,6 +131,7 @@ public class Intake extends SubsystemBase {
        setPosition(maxExtension);
 
     }
+
     public void retract(){
         setPosition(startingPoint);
         
-- 
2.39.5