robotswerve

  1"""
  2Container to hold the main robot code.
  3
  4## Controller Map
  5
  6![Driver Controller](./assets/2026bot_controller_map_page1.png)
  7
  8![Operator Controller](./assets/2026bot_controller_map_page2.png)
  9"""
 10
 11# Native imports
 12import json
 13import os
 14from pathlib import Path
 15from typing import Callable
 16
 17import wpimath
 18
 19# Internal imports
 20from config import HoodConfig
 21from constants.swerve_constants import HoodConstants
 22from data.telemetry import Telemetry
 23from commands.default_swerve_drive import DefaultDrive
 24from subsystem.drivetrain.swerve_drivetrain import SwerveDrivetrain
 25from subsystem.mechanisms.shooter.hood import createHood
 26from utils.input import InputFactory
 27
 28# Third-party imports
 29import commands2
 30import wpilib
 31from commands2.button import Trigger
 32from pathplannerlib.auto import AutoBuilder
 33
 34
 35class RobotSwerve:
 36    # forward declare critical types for editors
 37    drivetrain: SwerveDrivetrain
 38
 39    def __init__(self, is_disabled: Callable[[], bool]) -> None:
 40        # networktables setup
 41        self.field = wpilib.Field2d()
 42        wpilib.SmartDashboard.putData("Field", self.field)
 43
 44        # Subsystem instantiation
 45        self.drivetrain = SwerveDrivetrain()
 46        self.hood = createHood(HoodConstants, HoodConfig)
 47
 48        # Alliance instantiation
 49        self.updateAlliance()
 50
 51
 52        # Initialize timer
 53        self.timer = wpilib.Timer()
 54        self.timer.start()
 55
 56        # HID setup — config-driven via InputFactory
 57        wpilib.DriverStation.silenceJoystickConnectionWarning(True)
 58        self.factory = InputFactory(config_path="data/inputs/2026bot.yaml")
 59
 60        # Speed toggle state
 61        self._drive_scale_slow = 0.25
 62        self._drive_scale_fast = 1
 63        self._drive_is_slow = False
 64
 65        # TODO: Move input retrieval and binding into commands/{subsystem}_controls.py
 66        # files as part of the subsystem registry refactor. Each subsystem's controls
 67        # module should own its own factory.get*() calls and command wiring.
 68        self._configure_controls()
 69
 70        # Autonomous setup
 71        self.auto_command = None
 72        self.auto_chooser = AutoBuilder.buildAutoChooser()
 73        wpilib.SmartDashboard.putData("Select auto routine", self.auto_chooser)
 74
 75        # Telemetry setup
 76        wpilib.SmartDashboard.putNumber("Drivetrain speed", self._drive_scale_fast)
 77        self.enableTelemetry = wpilib.SmartDashboard.getBoolean("enableTelemetry", True)
 78        if self.enableTelemetry:
 79            self.telemetry = Telemetry(
 80                driveTrain=self.drivetrain,
 81                driverController=self.factory.getController(0),
 82                mechController=self.factory.getController(1),
 83            )
 84
 85        wpilib.SmartDashboard.putString("Robot Version", self.getDeployInfo("git-hash"))
 86        wpilib.SmartDashboard.putString("Git Branch", self.getDeployInfo("git-branch"))
 87        wpilib.SmartDashboard.putString(
 88            "Deploy Host", self.getDeployInfo("deploy-host")
 89        )
 90        wpilib.SmartDashboard.putString(
 91            "Deploy User", self.getDeployInfo("deploy-user")
 92        )
 93
 94        # Update drivetrain motor idle modes 3 seconds after the robot has been disabled.
 95        # to_break should be False at competitions where the robot is turned off between matches
 96        Trigger(is_disabled()).debounce(3).onTrue(
 97            commands2.cmd.runOnce(
 98                self.drivetrain.set_motor_stop_modes(
 99                    to_drive=True, to_break=True, all_motor_override=True, burn_flash=True
100                ),
101                self.drivetrain
102            )
103        )
104
105    def robotPeriodic(self):
106        if self.enableTelemetry and self.telemetry:
107            self.telemetry.runDefaultDataCollections()
108
109        self.field.setRobotPose(self.drivetrain.current_pose())
110
111    def disabledInit(self):
112        self.updateAlliance()
113        self.drivetrain.set_motor_stop_modes(to_drive=True, to_break=True, all_motor_override=True, burn_flash=False)
114        self.drivetrain.stop_driving()
115
116    def disabledPeriodic(self):
117        pass
118
119    def autonomousInit(self):
120        self.updateAlliance()
121        self.auto_command = self.auto_chooser.getSelected()
122        if self.auto_command:
123            self.auto_command.schedule()
124        else:
125            self.drivetrain.reset_pose_estimator(self.drivetrain.get_default_starting_pose())
126
127    def autonomousPeriodic(self):
128        pass
129
130    def teleopInit(self):
131        self.updateAlliance()
132        if self.auto_command:
133            self.auto_command.cancel()
134
135        self.drivetrain.setDefaultCommand(
136            DefaultDrive(
137                self.drivetrain,
138                self.translate_x,
139                self.translate_y,
140                self.rotate,
141                lambda: not self.robot_relative_btn()
142            )
143        )
144
145    def teleopPeriodic(self):
146        pass
147
148    def testInit(self):
149        #TODO Move to NT listener on change listener
150        self.updateAlliance()
151        commands2.CommandScheduler.getInstance().cancelAll()
152        self.drivetrain.setDefaultCommand(
153            DefaultDrive(
154                self.drivetrain,
155                lambda: wpimath.applyDeadband(-1 * self.driver_controller.getLeftY(), 0.06),
156                lambda: wpimath.applyDeadband(-1 * self.driver_controller.getLeftX(), 0.06),
157                lambda: wpimath.applyDeadband(-1 * self.driver_controller.getRightX(), 0.1),
158                lambda: not self.driver_controller.getRightBumperButton()
159            )
160        )
161        commands2.cmd.run(lambda: self.drivetrain.drive(2, 0, 0, False), self.drivetrain).withTimeout(5).schedule()
162
163    def testPeriodic(self):
164        pass
165
166    def _configure_controls(self) -> None:
167        """Retrieve managed inputs from the factory and wire command bindings.
168
169        TODO: Move into commands/{subsystem}_controls.py files as part of the
170        subsystem registry refactor. Each subsystem's controls module would
171        call register_controls(subsystem, container) and own its own
172        factory.get*() calls and command wiring.
173        """
174        # Managed drive inputs
175        self.translate_x = self.factory.getAnalog("drivetrain.translate_x")
176        self.translate_y = self.factory.getAnalog("drivetrain.translate_y")
177        self.rotate = self.factory.getAnalog("drivetrain.rotate")
178        self.robot_relative_btn = self.factory.getRawButton("drivetrain.robot_relative")
179
180        # Cancel-all: event-driven via Trigger instead of polling
181        self.factory.getButton("drivetrain.cancel_all").onTrue(
182            commands2.cmd.runOnce(
183                lambda: commands2.CommandScheduler.getInstance().cancelAll()
184            )
185        )
186
187        # Speed toggle: Y button switches between slow and fast scale
188        self.factory.getButton("drivetrain.speed_toggle").onTrue(
189            commands2.cmd.runOnce(self._toggle_drive_scale)
190        )
191
192        # Map all drive axes' scale to a shared SmartDashboard entry.
193        # Dashboard changes and Y-button toggles both write to this path;
194        # the factory auto-syncs the value into all three analogs each cycle.
195        _SPEED_NT = "/SmartDashboard/Drivetrain speed"
196        for analog in (self.translate_x, self.translate_y, self.rotate):
197            analog.mapParamToNtPath(_SPEED_NT, "scale")
198
199    def _toggle_drive_scale(self) -> None:
200        """Toggle between slow and fast drive scale presets.
201
202        Writes the new scale to SmartDashboard; the factory auto-syncs
203        it into all three drive analogs via mapParamToNtPath each cycle.
204        """
205        self._drive_is_slow = not self._drive_is_slow
206        scale = self._drive_scale_slow if self._drive_is_slow else self._drive_scale_fast
207        wpilib.SmartDashboard.putNumber("Drivetrain speed", scale)
208
209    def getDeployInfo(self, key: str) -> str:
210        """Gets the Git SHA of the deployed robot by parsing ~/deploy.json and returning the git-hash from the JSON key OR if deploy.json is unavailable will return "unknown"
211            example deploy.json: '{"deploy-host": "DESKTOP-80HA89O", "deploy-user": "ehsra", "deploy-date": "2023-03-02T17:54:14", "code-path": "blah", "git-hash": "3f4e89f138d9d78093bd4869e0cac9b61becd2b9", "git-desc": "3f4e89f-dirty", "git-branch": "fix-recal-nbeasley"}
212
213        Args:
214            key (str): The desired json key to get. Popular onces are git-hash, deploy-host, deploy-user
215
216        Returns:
217            str: Returns the value of the desired deploy key
218        """
219        json_object = None
220        home = str(Path.home()) + os.path.sep
221        releaseFile = home + 'py' + os.path.sep + "deploy.json"
222        try:
223            # Read from ~/deploy.json
224            with open(releaseFile, "r") as openfile:
225                json_object = json.load(openfile)
226                print(json_object)
227                print(type(json_object))
228                if key in json_object:
229                    return json_object[key]
230                else:
231                    return f"Key: {key} Not Found in JSON"
232        except OSError:
233            return "unknown"
234        except json.JSONDecodeError:
235            return "bad json in deploy file check for unescaped "
236
237    def updateAlliance(self) -> None:
238        """
239        Update the alliance the robot is on
240        """
241        self.alliance = wpilib.DriverStation.getAlliance()
242        self.drivetrain.update_alliance_flag(self.alliance)