python-example/robotcontainer.py at main · ApesofWrath/python-example · 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
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
# project imports
import constants
from subsystems.limelight import Limelight
from subsystems.spinner import Spinner
from subsystems.turntable import Turntable
from subsystems.drivetrain import CommandSwerveDrivetrain

# commands imports
import commands2
import commands2.button
import commands2.cmd
from commands2.sysid import SysIdRoutine

# wpi imports
from wpilib import SmartDashboard
from wpimath.geometry import Rotation2d
from phoenix6 import swerve, hardware
from pathplannerlib.auto import AutoBuilder, NamedCommands

from telemetry import Telemetry

class RobotContainer:
    """
    This class is where the bulk of the robot should be declared. Since Command-based is a
    "declarative" paradigm, very little robot logic should actually be handled in the :class:`.Robot`
    periodic methods (other than the scheduler calls). Instead, the structure of the robot (including
    subsystems, commands, and button mappings) should be declared here.

    """

    def __init__(self) -> None:
        """The container for the robot. Contains subsystems, OI devices, and commands."""
        # The robot's subsystems
        self.robotDrive =  CommandSwerveDrivetrain(
            hardware.TalonFX,
            hardware.TalonFX,
            hardware.CANcoder,
            constants.TunerConstants.drivetrain_constants,
            [ constants.TunerConstants.front_left, constants.TunerConstants.front_right, constants.TunerConstants.back_left, constants.TunerConstants.back_right ],
        )
        #self.turntable = Turntable()
        #self.spinner = Spinner()
        self.limelight = Limelight(self.robotDrive)

        # The robot's commands
        #NamedCommands.registerCommand("spinner.off", self.spinner.off())
        #NamedCommands.registerCommand("spinner.on", self.spinner.on())

        # The driver's controller
        self.driverController = commands2.button.CommandXboxController(constants.Global.kDriverControllerPort)

		# Setting up bindings for necessary control of the swerve drive platform
        self.drive = (
            swerve.requests.FieldCentric()
            .with_deadband(constants.Global.max_speed * 0.1)
            .with_rotational_deadband(
                constants.Global.max_angular_rate * 0.1
            )  # Add a 10% deadband
            .with_drive_request_type(
                swerve.SwerveModule.DriveRequestType.OPEN_LOOP_VOLTAGE
            )  # Use open-loop control for drive motors
        )

        # Configure the button bindings

        self.configureButtonBindings()

        # Build an auto chooser. This will use Commands.none() as the default option.
        self.autoChooser = AutoBuilder.buildAutoChooser()
        SmartDashboard.putData("Auto Chooser", self.autoChooser)

    def configureButtonBindings(self) -> None:
        """
        Use this method to define your button->command mappings. Buttons can be created via the button
        factories on commands2.button.CommandGenericHID or one of its
        subclasses (commands2.button.CommandJoystick or command2.button.CommandXboxController).
        """
        # Drive
        self.robotDrive.setDefaultCommand(
            # Drivetrain will execute this command periodically
            self.robotDrive.apply_request(
                lambda: (
                    self.drive.with_velocity_x(
                        -self.driverController.getLeftY()
                        * constants.Global.max_speed
                        * max((self.driverController.leftBumper() | self.driverController.rightBumper()).negate().getAsBoolean(),constants.Global.break_speed_mul)
                    )  # Drive forward with negative Y (forward)
                    .with_velocity_y(
                        -self.driverController.getLeftX()
                        * constants.Global.max_speed
                        * max((self.driverController.leftBumper() | self.driverController.rightBumper()).negate().getAsBoolean(),constants.Global.break_speed_mul)
                    )  # Drive left with negative X (left)
                    .with_rotational_rate(
                        self.driverController.getRightX()
                        * constants.Global.max_angular_rate
                        * max((self.driverController.leftBumper() | self.driverController.rightBumper()).negate().getAsBoolean(),constants.Global.break_speed_mul)
                    )  # Drive counterclockwise with X (right)
                )
            )
        )

        # break on triggers
        (self.driverController.leftTrigger() | self.driverController.rightTrigger()).whileTrue(self.robotDrive.apply_request(lambda: swerve.requests.SwerveDriveBrake()))

        # Run SysId routines when holding back and face buttons.
        # Note that each routine should be run exactly once in a single log.
        (self.driverController.back() & self.driverController.a()).whileTrue(
            self.robotDrive.sys_id_dynamic(SysIdRoutine.Direction.kForward)
        )
        (self.driverController.back() & self.driverController.b()).whileTrue(
            self.robotDrive.sys_id_dynamic(SysIdRoutine.Direction.kReverse)
        )
        (self.driverController.back() & self.driverController.x()).whileTrue(
            self.robotDrive.sys_id_quasistatic(SysIdRoutine.Direction.kForward)
        )
        (self.driverController.back() & self.driverController.y()).whileTrue(
            self.robotDrive.sys_id_quasistatic(SysIdRoutine.Direction.kReverse)
        )

        # reset the field-centric heading on start press
        self.driverController.start().onTrue(
            self.limelight.runOnce(lambda: self.limelight.pigeon2.set_yaw(0))
        )

        # modules turn toward their zeros
        self.driverController.back().onTrue(
               self.robotDrive.apply_request(lambda: swerve.requests.PointWheelsAt().with_module_direction(Rotation2d()))
		)

        # go to the closest alignment target
        self.driverController.povUp().whileTrue(self.limelight.pathfind())
        self.driverController.povDown().whileTrue(self.limelight.align())

        self.logger = Telemetry(constants.Global.max_speed)
        self.robotDrive.register_telemetry(
            lambda state: self.logger.telemeterize(state)
        )

		# Subsystems
        #self.driverController.a().onTrue(self.turntable.freespin(2)).onFalse(self.turntable.freespin(0))
        #self.driverController.b().onTrue(self.turntable.freespin(-2)).onFalse(self.turntable.freespin(0))
        #self.driverController.x().onTrue(self.turntable.turndeg(90))
        #self.driverController.y().onTrue(self.turntable.turnto(0))
        #self.driverController.povUp().onTrue(self.spinner.on())
        #self.driverController.povDown().onTrue(self.spinner.off())

    def getAutonomousCommand(self) -> commands2.Command:
        """
        Use this to pass the autonomous command to the main :class:`.Robot` class.

        :returns: the command to run in autonomous
        """
        return self.autoChooser.getSelected()
        # TODO: sometimes, attempting to correct a wrong location using two limelights at once will cause the odometry to "spiral"