flatbot2026-python/robot_container.py at main · Team4973/flatbot2026-python · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
"""
RobotContainer - Hardware setup and button bindings for Flatbot.

This file creates the motor, controller, and connects buttons to actions.
Think of it as the "wiring diagram" between hardware and software.

See WIRING.md for the physical wiring map (PDH channels, CAN IDs, etc.)
"""

import wpilib
from commands2 import InstantCommand
from commands2.button import CommandXboxController
from phoenix6.hardware import Pigeon2, TalonFX
from wpilib import SmartDashboard


class RobotContainer:
    """
    Sets up hardware and connects controller buttons to robot actions.

    Controls:
        A button (hold): Spin motor at current speed
        A button (release): Stop motor
        B button: Zero IMU heading
        D-pad up: Increase speed by 0.05
        D-pad down: Decrease speed by 0.05
    """

    def __init__(self):
        # Power Distribution Hub (REV-11-1850) on CAN ID 1
        # Monitors power to all channels - see WIRING.md for channel assignments
        self.pdh = wpilib.PowerDistribution(1, wpilib.PowerDistribution.ModuleType.kRev)

        # Hardware setup
        self.motor = TalonFX(30)    # TalonFX on CAN ID 30
        self.motor1 = TalonFX(32)    # TalonFX on CAN ID 32
        self.motor2 = TalonFX(33)    # TalonFX on CAN ID 33
        self.motor3 = TalonFX(34)    # TalonFX on CAN ID 34


        # Pigeon 2 IMU on CAN ID 39
        # Note: Pigeon2 causes timeout in simulation, only init on real robot
        if wpilib.RobotBase.isReal():
             self.imu = Pigeon2(2)
        else:
             self.imu = None

        # Driver controller
        self.controller = CommandXboxController(0)  # Xbox controller on port 0

        # Motor speed (adjustable via D-pad)
        self.speed = 0.01 # defulat speed is 1 percent power

        # Connect buttons to actions
        self._configure_button_bindings()

    def _configure_button_bindings(self):
        """Wire up controller buttons to robot actions."""
        # A button: hold to spin motor, release to stop
        self.controller.a().onTrue(InstantCommand(self._start_motor))
        self.controller.a().onFalse(InstantCommand(self._stop_motor))

        # B button: zero IMU heading
        self.controller.b().onTrue(InstantCommand(self.zero_heading))

        # D-pad: adjust speed
        self.controller.povUp().onTrue(InstantCommand(lambda: self._change_speed(0.02)))
        self.controller.povDown().onTrue(InstantCommand(lambda: self._change_speed(-0.02)))

    def _start_motor(self):
        """Spin the motor at the current speed."""
        print(f"Motor set at {self.speed}")
        self.motor.set(self.speed)
        self.motor1.set(self.speed)
        self.motor2.set(0.01)
        self.motor3.set(0.01)


    def _stop_motor(self):
        """Stop the motor."""
        self.motor.set(0.0)
        self.motor1.set(0.0)
        self.motor2.set(0.0)
        self.motor3.set(0.0)


    def _change_speed(self, delta: float):
        """Adjust the motor speed by the given amount."""
        self.speed += delta
        print(f"Speed changed to {self.speed}")

    def update_telemetry(self):
        """Publish motor and IMU data to SmartDashboard."""
        # Motor telemetry
        SmartDashboard.putNumber("Motor/Target Speed", self.speed)
        SmartDashboard.putNumber("Motor/Output", self.motor.get())
        SmartDashboard.putNumber("Motor/Velocity (rps)", self.motor.get_velocity().value)
        SmartDashboard.putNumber("Motor/Position (rot)", self.motor.get_position().value)
        SmartDashboard.putNumber("Motor/Voltage", self.motor.get_motor_voltage().value)

        # IMU telemetry (only on real robot)
        if self.imu is not None:
           SmartDashboard.putNumber("IMU/Yaw (deg)", self.imu.get_yaw().value)
           SmartDashboard.putNumber("IMU/Pitch (deg)", self.imu.get_pitch().value)
           SmartDashboard.putNumber("IMU/Roll (deg)", self.imu.get_roll().value)

    def zero_heading(self):
        """Reset the IMU yaw to zero."""
        if self.imu is not None:
            self.imu.set_yaw(0)