working sim pt 2

diff --git a/networktables.json.bck b/networktables.json.bck
index fe51488c7066f6687ef680d6bfaa4f7768ef205c..eefbca660a4132e51d3659e286b5237a3b29a2d4 100644 (file)
--- 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 155016a34d9be522335c9742a8bc652aa42aef80..df4b7b1400e973510e477727cd0ef8fb1f20393a 100644 (file)
--- 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 a612292c3b4c35f6e048a09c7b73c95f2a118c68..4e8e807cd5bd02ee51d003a4b90c65f88a1032b5 100644 (file)
--- 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);