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
 17# Internal imports
 18from config import HoodConfig, OperatorRobotConfig
 19import constants.swerve_constants as consts
 20from data.telemetry import Telemetry
 21import commands
 22import subsystem
 23from subsystem.health_and_status import HealthAndStatus
 24from subsystem.nfc_battery_tracker import NfcBatteryTracker
 25from subsystem.robot_state import RobotState
 26from utils.input import InputFactory
 27from utils.odometry_logic_2026 import determineShooterTargets2026
 28
 29# Third-party imports
 30import commands2
 31import wpilib
 32from pathplannerlib.auto import AutoBuilder
 33from wpimath.geometry import Rotation2d
 34
 35class RobotSwerve:
 36    def __init__(self, is_disabled: Callable[[], bool]) -> None:
 37        # networktables setup
 38        self.field = wpilib.Field2d()
 39        wpilib.SmartDashboard.putData("Field", self.field)
 40
 41        # In simulation, pre-seed CANcoder supply voltage so the absolute
 42        # encoders report valid data when swerve modules call
 43        # baseline_relative_encoders() during construction.
 44        if wpilib.RobotBase.isSimulation():
 45            import phoenix6
 46            for base_id in OperatorRobotConfig.swerve_module_channels:
 47                encoder_id = base_id + 2
 48                encoder = phoenix6.hardware.CANcoder(encoder_id)
 49                encoder.sim_state.set_supply_voltage(12.0)
 50
 51        # Subsystem instantiation
 52        self.drivetrain = subsystem.drivetrain.swerve_drivetrain.SwerveDrivetrain()
 53        self.health_and_status = HealthAndStatus()
 54        self.nfc_battery_tracker = NfcBatteryTracker()
 55        from subsystem.localization.localization import Localization
 56        self.localization = Localization(self.drivetrain, field=self.field)
 57        self._vision_cycle_counter = 0
 58
 59        # Mechanism subsystems — toggle each to True/False individually
 60        _enable_shooter = False
 61        _enable_feed = False
 62        _enable_hood = False
 63        _enable_hopper = False
 64        _enable_intake_position = False
 65        _enable_intake_roller = True
 66
 67        self.shooter = subsystem.mechanisms.shooter.Shooter() if _enable_shooter else None
 68        self.feed = subsystem.mechanisms.shooter.Feed() if _enable_feed else None
 69        self.hood = subsystem.mechanisms.shooter.createHood(consts.HoodConstants, HoodConfig) if _enable_hood else None
 70        self.hopper = subsystem.Hopper() if _enable_hopper else None
 71        self.intake_position = subsystem.IntakePosition() if _enable_intake_position else None
 72        self.intake_roller = subsystem.IntakeRoller() if _enable_intake_roller else None
 73        self.robot_state = RobotState()
 74        # Alliance instantiation
 75        self.updateAlliance()
 76
 77        # Initialize timer
 78        self.timer = wpilib.Timer()
 79        self.timer.start()
 80
 81        # HID setup — config-driven via InputFactory
 82        wpilib.DriverStation.silenceJoystickConnectionWarning(True)
 83        self.factory = InputFactory(config_path="data/inputs/2026bot.yaml")
 84
 85        # Speed toggle state
 86        self._drive_scale_slow = 0.25
 87        self._drive_scale_fast = 1
 88        self._drive_is_slow = False
 89
 90        # TODO: Move input retrieval and binding into commands/{subsystem}_controls.py
 91        # files as part of the subsystem registry refactor. Each subsystem's controls
 92        # module should own its own factory.get*() calls and command wiring.
 93        self._configure_controls()
 94
 95        # Autonomous setup
 96        self.auto_command = None
 97        self.auto_chooser = AutoBuilder.buildAutoChooser()
 98        wpilib.SmartDashboard.putData("Select auto routine", self.auto_chooser)
 99
100        # Telemetry setup
101        wpilib.SmartDashboard.putNumber("Drivetrain speed", self._drive_scale_fast)
102        self._enable_telemetry = wpilib.SmartDashboard.getBoolean("enableTelemetry", True)
103        if self._enable_telemetry:
104            self.telemetry = Telemetry(
105                driveTrain=self.drivetrain,
106            )
107
108        wpilib.SmartDashboard.putString("Robot Version", self.getDeployInfo("git-hash"))
109        wpilib.SmartDashboard.putString("Git Branch", self.getDeployInfo("git-branch"))
110
111
112
113    def robotPeriodic(self):
114        self._vision_cycle_counter += 1
115        if self._vision_cycle_counter >= OperatorRobotConfig.vision_update_rate_divisor:
116            self._vision_cycle_counter = 0
117            self.localization.update()
118
119        if self._enable_telemetry and self.telemetry:
120            self.telemetry.runDefaultDataCollections()
121
122        self.field.setRobotPose(self.drivetrain.current_pose())
123
124    def disabledInit(self):
125        self.updateAlliance()
126        self.drivetrain.set_motor_stop_modes(to_drive=True, to_break=True, all_motor_override=True, burn_flash=False)
127        self.drivetrain.stop_driving()
128        self.nfc_battery_tracker.onDisabledInit()
129
130        if self.shooter:
131            self.shooter.setRPM(0)
132            self.shooter.resetOffset()
133        if self.feed:
134            self.feed.stop()
135        if self.hopper:
136            self.hopper.stop()
137        if self.intake_roller:
138            self.intake_roller.stop()
139        if self.intake_position:
140            self.intake_position.disable()
141
142    def disabledPeriodic(self):
143        pass
144
145
146    def autonomousInit(self):
147        self.updateAlliance()
148        self.auto_command = self.auto_chooser.getSelected()
149        if self.auto_command:
150            self.auto_command.schedule()
151            if self.auto_command.getName().lower() == "instantcommand":
152                self.drivetrain.reset_pose_estimator(self.drivetrain.get_default_starting_pose())
153        else:
154            self.drivetrain.reset_pose_estimator(self.drivetrain.get_default_starting_pose())
155
156    def autonomousPeriodic(self):
157        pass
158
159    def teleopInit(self):
160        self.updateAlliance()
161        if self.auto_command:
162            self.auto_command.cancel()
163            if self.auto_command.getName().lower() == "instantcommand":
164                self.drivetrain.reset_pose_estimator(self.drivetrain.get_default_starting_pose())
165        else:
166            self.drivetrain.reset_pose_estimator(self.drivetrain.get_default_starting_pose())
167
168        # DefaultDrive: raw square input (diagonals get full per-axis output)
169        # DefaultDriveCircular: remaps circular stick input so diagonals
170        #   reach full speed (use when stick hardware caps diagonal ~70%)
171        self.drivetrain.setDefaultCommand(
172            commands.default_swerve_drive.DefaultDrive(  # TODO: switch back to DefaultDriveCircular
173                self.drivetrain,
174                self.translate_x,
175                self.translate_y,
176                self.rotate,
177                lambda: not self.robot_relative_btn()
178            )
179        )
180
181        if self.hood:
182            self.hood.setDefaultCommand(self.hood.autoAngleCommand())
183
184    def teleopPeriodic(self):
185        pass
186
187    def testInit(self):
188        self.updateAlliance()
189        commands2.CommandScheduler.getInstance().cancelAll()
190        # See teleopInit for DefaultDrive vs DefaultDriveCircular notes
191        self.drivetrain.setDefaultCommand(
192            commands.default_swerve_drive.DefaultDrive(  # TODO: switch back to DefaultDriveCircular
193                self.drivetrain,
194                self.translate_x,
195                self.translate_y,
196                self.rotate,
197                lambda: not self.robot_relative_btn()
198            )
199        )
200        commands2.cmd.run(lambda: self.drivetrain.drive(2, 0, 0, False), self.drivetrain).withTimeout(5).schedule()
201
202        if self.hood:
203            self.hood.setDefaultCommand(
204                self.hood.manualTestCommand(self.hood_angle_input))
205
206    def testPeriodic(self):
207        pass
208
209    def _configure_controls(self) -> None:
210        """Retrieve managed inputs from the factory and wire command bindings.
211
212        TODO: Move into commands/{subsystem}_controls.py files as part of the
213        subsystem registry refactor. Each subsystem's controls module would
214        call register_controls(subsystem, container) and own its own
215        factory.get*() calls and command wiring.
216        """
217        # Managed drive inputs
218        self.translate_x = self.factory.getAnalog("drivetrain.translate_x")
219        self.translate_y = self.factory.getAnalog("drivetrain.translate_y")
220        self.rotate = self.factory.getAnalog("drivetrain.rotate")
221        self.robot_relative_btn = self.factory.getRawButton("drivetrain.robot_relative")
222
223        # Cancel-all: emergency stop all commands
224        self.factory.getButton("drivetrain.cancel_all").bind(
225            commands2.cmd.runOnce(
226                lambda: commands2.CommandScheduler.getInstance().cancelAll()
227            )
228        )
229
230        # Speed toggle: switch between slow and fast drive scale
231        self.factory.getButton("drivetrain.speed_toggle").bind(
232            commands2.cmd.runOnce(self._toggle_drive_scale)
233        )
234
235        # Auto-rotate: align drivetrain to target while held
236        self.factory.getButton("drivetrain.auto_align").bind(
237            commands.auto.pid_to_angle.PIDAlignToTarget(
238                self.drivetrain,
239                lambda: determineShooterTargets2026(self.drivetrain.current_pose, self.alliance),
240                self.translate_x,
241                self.translate_y,
242                lambda: not self.robot_relative_btn(),
243                alignment_angle=Rotation2d.fromDegrees(180)
244            )
245        )
246
247        # Mechanism controls — only registered when subsystems are enabled
248        if self.shooter:
249            self.factory.getButton("shooter.increment_RPM").bind(
250                commands2.cmd.runOnce(lambda: self.shooter.modifyOffset(consts.ShooterConstants.offsetDelta), self.shooter)
251            )
252            self.factory.getButton("shooter.decrement_RPM").bind(
253                commands2.cmd.runOnce(lambda: self.shooter.modifyOffset(-consts.ShooterConstants.offsetDelta), self.shooter)
254            )
255            self.factory.getButton("shooter.reset_RPM_offset").bind(
256                commands2.cmd.runOnce(self.shooter.resetOffset, self.shooter)
257            )
258
259        if self.hood:
260            self.hood_angle_input = self.factory.getAnalog("hood.angle")
261
262        if self.hopper:
263            self.factory.getButton("hopper.toggle_hopper").bind(
264                commands.ball_transport.run_hopper_while_active(self.hopper)
265            )
266
267        if self.intake_position:
268            self.factory.getButton("intake.toggle_deploy").bind(
269                commands.intake_commands.toggle_intake_deploy(self.intake_position)
270            )
271
272        if self.intake_roller:
273            self.factory.getButton("ball_transport.hold_roller").bind(
274                commands.ball_transport.run_roller_while_held(self.intake_roller)
275            )
276
277        if self.shooter and self.hood:
278            self.factory.getButton("shooter.spinup_toggle").bind(
279                commands.shooter_commands.toggle_spinup(self.shooter, self.hood)
280            )
281
282        if self.hopper and self.feed:
283            self.factory.getButton("ball_transport.run_hopper_feed").bind(
284                commands.ball_transport.run_hopper_and_feed(self.hopper, self.feed)
285            )
286
287        #Turret stop
288        # Map all drive axes' scale to a shared SmartDashboard entry.
289        # Dashboard changes and Y-button toggles both write to this path;
290        # the factory auto-syncs the value into all three analogs each cycle.
291        _SPEED_NT = "/SmartDashboard/Drivetrain speed"
292        for analog in (self.translate_x, self.translate_y, self.rotate):
293            analog.mapParamToNtPath(_SPEED_NT, "scale")
294
295    def _toggle_drive_scale(self) -> None:
296        """Toggle between slow and fast drive scale presets.
297
298        Writes the new scale to SmartDashboard; the factory auto-syncs
299        it into all three drive analogs via mapParamToNtPath each cycle.
300        """
301        self._drive_is_slow = not self._drive_is_slow
302        scale = self._drive_scale_slow if self._drive_is_slow else self._drive_scale_fast
303        wpilib.SmartDashboard.putNumber("Drivetrain speed", scale)
304
305    def getDeployInfo(self, key: str) -> str:
306        """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"
307            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"}
308
309        Args:
310            key (str): The desired json key to get. Popular onces are git-hash, deploy-host, deploy-user
311
312        Returns:
313            str: Returns the value of the desired deploy key
314        """
315        json_object = None
316        home = str(Path.home()) + os.path.sep
317        release_file = home + 'py' + os.path.sep + "deploy.json"
318        try:
319            # Read from ~/deploy.json
320            with open(release_file, "r") as openfile:
321                json_object = json.load(openfile)
322                if key in json_object:
323                    return json_object[key]
324                else:
325                    return f"Key: {key} Not Found in JSON"
326        except OSError:
327            return "unknown"
328        except json.JSONDecodeError:
329            return "bad json in deploy file check for unescaped "
330
331    def updateAlliance(self) -> None:
332        """
333        Update the alliance the robot is on
334        """
335        self.alliance = wpilib.DriverStation.getAlliance()
336        self.drivetrain.update_alliance_flag(self.alliance)