RobotsAtWard/robot.py at main · WolverineRobotics/RobotsAtWard · 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
import commands2
from commands2.button import CommandXboxController
from commands2 import CommandScheduler
from subsystems.drive.drive_subsystem import DriveSubsystem
from subsystems.drive.swerve_module import SwerveModule
from subsystems.intake import intake_constants
from subsystems.intake.intake_subsystem import IntakeSubsystem
from subsystems.shooter.shooter_subsystem import ShooterSubsystem
from subsystems.shooter import shooter_constansts
from wolverine_sim.robot_simulation import rs
from wolverine_sim.rev.spark_max_simulation import spark_max_sim
from wolverine_sim.phoenix6.pigeon2_simulation import pigeon2_sim
from wolverine_sim.wpilib.analog_encoder_simulation import analog_encoder_sim
from wolverine_sim.rev.relative_encoder_simulation import relative_encoder_sim
from typing import Callable
import subsystems.drive.drive_constants as drive_constants
import wpilib

class Robot(wpilib.TimedRobot):

    def robotInit(self) -> None:
        # Creating the DriveSubsystem with constants from each swerve module
        self.drive_subsystem = DriveSubsystem(
            [
                SwerveModule(
                    drive_constants.FRONT_LEFT_DRIVE_CAN_ID,
                    drive_constants.FRONT_LEFT_STEER_CAN_ID,
                    drive_constants.FRONT_LEFT_ENCODER_ID,
                    drive_constants.FRONT_LEFT_ENCODER_OFFSET
                ),

                SwerveModule(
                    drive_constants.FRONT_RIGHT_DRIVE_CAN_ID,
                    drive_constants.FRONT_RIGHT_STEER_CAN_ID,
                    drive_constants.FRONT_RIGHT_ENCODER_ID,
                    drive_constants.FRONT_RIGHT_ENCODER_OFFSET
                ),

                SwerveModule(
                    drive_constants.BACK_LEFT_DRIVE_CAN_ID,
                    drive_constants.BACK_LEFT_STEER_CAN_ID,
                    drive_constants.BACK_LEFT_ENCODER_ID,
                    drive_constants.BACK_LEFT_ENCODER_OFFSET
                ),

                SwerveModule(
                    drive_constants.BACK_RIGHT_DRIVE_CAN_ID,
                    drive_constants.BACK_RIGHT_STEER_CAN_ID,
                    drive_constants.BACK_RIGHT_ENCODER_ID,
                    drive_constants.BACK_RIGHT_ENCODER_OFFSET
                ),
            ],

            drive_constants.GYRO_CAN_ID
        )

        self.intake_subsystem = IntakeSubsystem(
            intake_constants.RIGHT_PIVOT_ID,
            intake_constants.LEFT_PIVOT_ID,
            intake_constants.ROLLER_ID,
            intake_constants.RIGHT_PIVOT_INVERTED,
            intake_constants.LEFT_PIVOT_INVERTED
        )

        self.shooter_subsystem = ShooterSubsystem(
            shooter_constansts.FLYWHEEL_ID,
            shooter_constansts.INDEXER_ID,
            shooter_constansts.FLYWHEEL_INVERTED,
            shooter_constansts.INDEXER_INVERTED
        )


        CommandXboxController.getRightX = self.deadband_wrapper(CommandXboxController.getRightX)
        CommandXboxController.getRightY = self.deadband_wrapper(CommandXboxController.getRightY)
        CommandXboxController.getLeftX = self.deadband_wrapper(CommandXboxController.getLeftX)
        CommandXboxController.getLeftY = self.deadband_wrapper(CommandXboxController.getLeftY)

        # Creating the drive controller on port 0 which is our team's standard
        self.drive_controller =CommandXboxController(0)

        # Creating the operator controller on port 1 which is our team's standard
        self.op_controller = CommandXboxController(1)

        # Configuring the controls for the robot
        self.configure_bindings()


        if wpilib.RobotBase.isSimulation():
            # Initializing the mujoco simulation if the robot is being simulated
            rs.initialize("robot/scene.xml")

            # Setting up the wrappers for all the devices used on the robot
            spark_max_sim.setup_wrappers()
            pigeon2_sim.setup_wrappers()
            analog_encoder_sim.setup_wrappers()
            relative_encoder_sim.setup_wrappers()

    def configure_bindings(self) -> None:
        # Setting the default command of the drive subsystem to be the drive command
        # and passing the methods from the drive controller as parameters
        self.drive_subsystem.setDefaultCommand(
            self.drive_subsystem.get_drive_command(
                self.drive_controller.getLeftY,
                self.drive_controller.getLeftX,
                self.drive_controller.getRightX
            )
        )

        self.op_controller.a().whileTrue(
            self.intake_subsystem.get_intake_command()
        )

        self.op_controller.b().onTrue(
            self.intake_subsystem.get_pivot_command()
        )

    def deadband_wrapper(self, func: Callable[[CommandXboxController], float]):

        def deadband():
            if func() > 0.1:
                return func()
            else:
                return 0

        return deadband

    def robotPeriodic(self) -> None:
        CommandScheduler.getInstance().run()

    def teleopPeriodic(self) -> None:
        pass

    def _simulationInit(self) -> None:
        # Starting the simulation
        rs.start()

    def _simulationPeriodic(self) -> None:
        # Moving the simulation forward by 1 step
        rs.step()