d

diff --git a/networktables.json.bck b/networktables.json.bck
new file mode 100644 (file)
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 903080269460504010ee8d7f9fce4ac392bccbe6..9e95508d592e05e4d0ab41791dc027e5d81c04bd 100644 (file)
--- 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 5f0e062867a58ad4ccd49ac45dcfd0ac80957ce8..155016a34d9be522335c9742a8bc652aa42aef80 100644 (file)
--- 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 e4fae86da2b45127b531dc71ab13af26d530ecd4..6803e12823973210bea20632bc0fc92b3e310514 100644 (file)
--- 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);
     }
 
     /**