Superstructure implementation

.gitignore

@@ -186,4 +186,4 @@ compile_commands.json
 # Eclipse generated file for annotation processors
 .factorypath
 
-gradle.properties
+gradle.properties

advantagescope_assets/Robot_Chube/config.json

@@ -0,0 +1,51 @@
+{
+    "name": "Chube",
+    "disableSimplification": false,
+    "rotations": [
+        {
+            "axis": "x",
+            "degrees": 90
+        },
+        {
+            "axis": "z",
+            "degrees": 90
+        }
+    ],
+    "position": [],
+    "cameras": [],
+    "components": [
+        {
+            "zeroedRotations": [
+                {
+                    "axis": "x",
+                    "degrees": 90
+                },
+                {
+                    "axis": "z",
+                    "degrees": 90
+                }
+            ],
+            "zeroedPosition": [
+                0,
+                0,
+                0
+            ]
+        },
+        {
+            "zeroedRotations": [],
+            "zeroedPosition": [
+                0,
+                0,
+                0
+            ]
+        },
+        {
+            "zeroedRotations": [],
+            "zeroedPosition": [
+                0,
+                0,
+                0
+            ]
+        }
+    ]
+}

gradle.properties

@@ -0,0 +1 @@
+org.gradle.java.home=C:\\Users\\Public\\wpilib\\2025\\jdk

simgui-ds.json

@@ -0,0 +1,100 @@
+{
+  "keyboardJoysticks": [
+    {
+      "axisConfig": [
+        {
+          "decKey": 65,
+          "incKey": 68
+        },
+        {
+          "decKey": 87,
+          "incKey": 83
+        },
+        {
+          "decKey": 69,
+          "decayRate": 0.0,
+          "incKey": 82,
+          "keyRate": 0.009999999776482582
+        }
+      ],
+      "axisCount": 3,
+      "buttonCount": 4,
+      "buttonKeys": [
+        90,
+        88,
+        67,
+        86
+      ],
+      "povConfig": [
+        {
+          "key0": 328,
+          "key135": 323,
+          "key180": 322,
+          "key225": 321,
+          "key270": 324,
+          "key315": 327,
+          "key45": 329,
+          "key90": 326
+        }
+      ],
+      "povCount": 1
+    },
+    {
+      "axisConfig": [
+        {
+          "decKey": 74,
+          "incKey": 76
+        },
+        {
+          "decKey": 73,
+          "incKey": 75
+        }
+      ],
+      "axisCount": 2,
+      "buttonCount": 4,
+      "buttonKeys": [
+        77,
+        44,
+        46,
+        47
+      ],
+      "povCount": 0
+    },
+    {
+      "axisConfig": [
+        {
+          "decKey": 263,
+          "incKey": 262
+        },
+        {
+          "decKey": 265,
+          "incKey": 264
+        }
+      ],
+      "axisCount": 2,
+      "buttonCount": 6,
+      "buttonKeys": [
+        260,
+        268,
+        266,
+        261,
+        269,
+        267
+      ],
+      "povCount": 0
+    },
+    {
+      "axisCount": 0,
+      "buttonCount": 0,
+      "povCount": 0
+    }
+  ],
+  "robotJoysticks": [
+    {},
+    {},
+    {
+      "guid": "78696e70757401000000000000000000",
+      "useGamepad": true
+    }
+  ]
+}

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

@@ -5,17 +5,17 @@ import edu.wpi.first.hal.FRCNetComm.tInstances
 import edu.wpi.first.hal.FRCNetComm.tResourceType
 import edu.wpi.first.hal.HAL
 import edu.wpi.first.math.MathUtil
+import edu.wpi.first.math.geometry.Pose2d
+import edu.wpi.first.math.geometry.Pose3d
+import edu.wpi.first.networktables.NetworkTableInstance
+import edu.wpi.first.wpilibj.RobotController
 import edu.wpi.first.wpilibj.TimedRobot
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard
 import edu.wpi.first.wpilibj.util.WPILibVersion
 import edu.wpi.first.wpilibj2.command.Command
 import edu.wpi.first.wpilibj2.command.CommandScheduler
 import edu.wpi.first.wpilibj2.command.Commands
-import frc.robot.RobotContainer.ControlledAngularRate
-import frc.robot.RobotContainer.ControlledSpeed
-import frc.robot.RobotContainer.MaxAngularRateConst
-import frc.robot.RobotContainer.MaxSpeedConst
 import frc.robot.RobotContainer.leftJoystick
 import frc.robot.RobotContainer.vision
 
@@ -36,6 +36,11 @@ object Robot : TimedRobot() {
      *the  AutoChooser on the dashboard.
      */
     private var autonomousCommand: Command = Commands.runOnce({})
+    val robotPosePublisher = NetworkTableInstance.getDefault().getStructTopic("Robot Pose", Pose2d.struct).publish();
+    val elevatorPosePublisher =
+        NetworkTableInstance.getDefault().getStructTopic("Elevator Pose", Pose3d.struct).publish();
+    val pivotPosePublisher = NetworkTableInstance.getDefault().getStructTopic("Pivot Pose", Pose3d.struct).publish();
+    val wristPosePublisher = NetworkTableInstance.getDefault().getStructTopic("Wrist Pose", Pose3d.struct).publish();
 
 
     /**
@@ -66,6 +71,13 @@ object Robot : TimedRobot() {
         // and running subsystem periodic() methods.  This must be called from the robot's periodic
         // block in order for anything in the Command-based framework to work.
         CommandScheduler.getInstance().run()
+
+        robotPosePublisher.set(RobotContainer.drivetrain.getSwervePose())
+        elevatorPosePublisher.set(Pose3d())
+        pivotPosePublisher.set(Pose3d())
+        wristPosePublisher.set(Pose3d())
+
+        SmartDashboard.putNumberArray("Test", arrayOf(0.0, 0.0, 0.0))
     }
 
     /** This method is called once each time the robot enters Disabled mode.  */
@@ -108,13 +120,20 @@ object Robot : TimedRobot() {
 
     }
 
+//    var lastLoopTime = 0.0
+
     /** This method is called once when the robot is first started up.  */
     override fun simulationInit() {
-
+//        lastLoopTime = MiscCalculations.getCurrentTime()
     }
 
     /** This method is called periodically whilst in simulation.  */
     override fun simulationPeriodic() {
-
+//        val currentTime = MiscCalculations.getCurrentTime()
+//        val dtSeconds = (currentTime - lastLoopTime) / 1000
+//
+//        RobotContainer.drivetrain.updateSimState(dtSeconds, RobotController.getBatteryVoltage())
+//
+//        lastLoopTime = currentTime
     }
 }

src/main/java/frc/robot/RobotConfiguration.kt

@@ -0,0 +1,11 @@
+package frc.robot
+
+enum class RobotState {
+    Real,
+    Simulated,
+    Empty
+}
+
+object RobotConfiguration {
+    val robotState = RobotState.Empty
+}

src/main/java/frc/robot/RobotContainer.kt

@@ -8,57 +8,45 @@ import edu.wpi.first.wpilibj2.command.Command
 import edu.wpi.first.wpilibj2.command.Commands
 import edu.wpi.first.wpilibj2.command.FunctionalCommand
 import edu.wpi.first.wpilibj2.command.button.CommandJoystick
+import edu.wpi.first.wpilibj2.command.button.CommandXboxController
+import frc.robot.commands.SimTeleopDriveCommand
 import frc.robot.commands.TeleopDriveCommand
-import frc.robot.constants.TunerConstants
-import frc.robot.subsystems.CommandSwerveDrivetrain
+import frc.robot.subsystems.swerve.TunerConstants
+import frc.robot.subsystems.swerve.CommandSwerveDrivetrain
+import frc.robot.subsystems.swerve.SwerveIOBase
+import frc.robot.subsystems.swerve.SwerveIOReal
+import frc.robot.subsystems.swerve.SwerveIOSim
+import frc.robot.util.Telemetry
 
 object RobotContainer {
-    public val MaxSpeedConst = TunerConstants.kSpeedAt12Volts.`in`(MetersPerSecond) // kSpeedAt12Volts desired top
-    // speed
-    public var ControlledSpeed = MaxSpeedConst
-    public val MaxAngularRateConst =
-        RotationsPerSecond.of(0.75).`in`(RadiansPerSecond) // 3/4 of a rotation per second max angular velocity
-
-    public var ControlledAngularRate = MaxAngularRateConst
-
-    /* Setting up bindings for necessary control of the swerve drive platform */
-    public val drive: SwerveRequest.FieldCentric = SwerveRequest.FieldCentric()
-        .withDeadband(MaxSpeedConst * 0.1).withRotationalDeadband(MaxAngularRateConst * 0.1) // Add a 10% deadband
-        .withDriveRequestType(DriveRequestType.OpenLoopVoltage) // Use open-loop control for drive motors
-
-    public val robotRelative: SwerveRequest.RobotCentric = SwerveRequest.RobotCentric()
-        .withDeadband(MaxSpeedConst * 0.1).withRotationalDeadband(MaxAngularRateConst * 0.1) // Add a 10% deadband
-        .withDriveRequestType(DriveRequestType.OpenLoopVoltage) // Use open-loop control for drive motors
-
-
-    private val brake = SwerveRequest.SwerveDriveBrake()
-    private val point = SwerveRequest.PointWheelsAt()
-
-    private val logger: Telemetry = Telemetry(MaxSpeedConst)
-
-//    private val xbox = CommandXboxController(0)
-
     val leftJoystick: CommandJoystick = CommandJoystick(0)
     val rightJoystick: CommandJoystick = CommandJoystick(1)
 
-    val vision = VisionSystem()
+    val xbox = CommandXboxController(2)
 
+    val vision = VisionSystem()
 
-    val drivetrain: CommandSwerveDrivetrain = TunerConstants.createDrivetrain()
+    val drivetrain = when (RobotConfiguration.robotState) {
+        RobotState.Real -> SwerveIOReal()
+        RobotState.Simulated -> SwerveIOSim()
+        RobotState.Empty -> SwerveIOBase()
+    }
 
-    val teleopDriveCommand = TeleopDriveCommand()
+    val teleopDriveCommand = when (RobotConfiguration.robotState) {
+        RobotState.Real -> TeleopDriveCommand()
+        RobotState.Simulated -> SimTeleopDriveCommand()
+        RobotState.Empty -> Commands.run({})
+    }
 
     init {
         configureBindings()
     }
 
     private fun configureBindings() {
-
         drivetrain.setDefaultCommand(teleopDriveCommand)
 
-        drivetrain.registerTelemetry(logger::telemeterize)
 
-//        teleopDriveCommand.schedule()
+        drivetrain.registerTelemetry(logger::telemeterize)
     }
 
     val autonomousCommand: Command

src/main/java/frc/robot/Vision.kt

@@ -18,11 +18,12 @@ import kotlin.math.max
 class VisionSystem {
     val max_distance_m = 6.0
 
-    val limelightNames: Array<String> =
-        arrayOf("limelight-front")
+    val limelightNames: Array<String> = when (RobotConfiguration.robotState) {
+        RobotState.Real -> arrayOf("limelight-front")
+        else -> emptyArray()
+    }
 
     fun updateOdometryFromDisabled() {
-
         var namesToSearch: Array<String>;
 
 
@@ -41,7 +42,7 @@ class VisionSystem {
 
             if (llMeasure.pose.x != 0.0 && llMeasure.pose.y != 0.0) {
                 val poseDifference =
-                    llMeasure.pose.translation.getDistance(RobotContainer.drivetrain.state.Pose.translation)
+                    llMeasure.pose.translation.getDistance(RobotContainer.drivetrain.getSwervePose().translation)
 
                 val distanceToTag = llMeasure.avgTagDist
 
@@ -81,7 +82,7 @@ class VisionSystem {
 
         var namesToSearch: Array<String>;
 
-            namesToSearch = limelightNames
+        namesToSearch = limelightNames
 
         for (llName in namesToSearch) {
             if (DriverStation.getAlliance().isEmpty) {
@@ -90,11 +91,11 @@ class VisionSystem {
             }
 
             var llMeasure: LimelightHelpers.PoseEstimate =
-                    LimelightHelpers.getBotPoseEstimate_wpiBlue(llName)
+                LimelightHelpers.getBotPoseEstimate_wpiBlue(llName)
 
             if (llMeasure.tagCount >= tagCount && llMeasure.pose.x != 0.0 && llMeasure.pose.y != 0.0) {
                 val poseDifference =
-                    llMeasure.pose.translation.getDistance(RobotContainer.drivetrain.state.Pose.translation)
+                    llMeasure.pose.translation.getDistance(RobotContainer.drivetrain.getSwervePose().translation)
                 if (!poseDifferenceCheck || poseDifference < max_distance_m) {
                     val distanceToTag = llMeasure.avgTagDist