2026-robot-code/robot.py at main · TaubatexasRobotics/2026-robot-code · 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
import constants
from drivetrain import Drivetrain
from autonomous.drivestraightpath import DriveStraightPath
from commands2 import TimedCommandRobot, CommandScheduler, Command, ParallelCommandGroup, SequentialCommandGroup
from typing import Optional
from led import LEDController
from commands2.button import JoystickButton, POVButton
from intake import Intake
from wpilib import SmartDashboard, SendableChooser, DriverStation, Joystick
from shooter import Shooter
from turret import Turret
from camera import PhotonVisionCamera
from indexer import Indexer
from commands2.cmd import run

class Robot(TimedCommandRobot):
    autonomous: Optional[Command] = None
    autoChooser: SendableChooser = SendableChooser()

    drivetrain: Drivetrain = Drivetrain()
    intake: Intake = Intake()
    driverJoystick: Joystick = Joystick(constants.kJoystickDriverPort)
    shooter: Shooter = Shooter()
    turret: Turret = Turret()
    indexer: Indexer = Indexer()
    led: LEDController = LEDController()
    turretCamera: PhotonVisionCamera = PhotonVisionCamera(constants.kCameraName)

    def combineAxis(self) -> float:
        leftTrigger = -self.driverJoystick.getRawAxis(2)
        rightTrigger = self.driverJoystick.getRawAxis(3)

        return rightTrigger + leftTrigger

    def robotInit(self) -> None:
        self.led.rainbow().schedule()

        JoystickButton(self.driverJoystick, 5).whileTrue(
            ParallelCommandGroup(
                self.shooter.activateFlywheel(),
                self.intake.releaseGamePiece(),
                self.led.blue()
            )
        )

        JoystickButton(self.driverJoystick, 1).onTrue(self.drivetrain.setSlowMode())

        JoystickButton(self.driverJoystick, 2).whileTrue(
            self.indexer.activateFeed()
        )

        JoystickButton(self.driverJoystick, 6).whileTrue(
            self.turret.followYawTag(self.turretCamera, self.led)
        )

        JoystickButton(self.driverJoystick, 7).whileTrue(
            self.shooter.hoodDown()
        )

        JoystickButton(self.driverJoystick, 8).whileTrue(
            self.shooter.hoodUp()
        )

        POVButton(self.driverJoystick, 90).whileTrue(
            self.intake.up()
        )

        POVButton(self.driverJoystick, 270).whileTrue(
            self.intake.down()
        )

        self.turret.setDefaultCommand(
            self.turret.activateYaw(lambda: self.driverJoystick.getRawAxis(4))
        )

        self.drivetrain.setDefaultCommand(
            self.drivetrain.arcadeDrive(
                lambda: -self.combineAxis(),
                lambda: self.driverJoystick.getRawAxis(0)
            )
        )

        self.autoChooser.addOption(
            "Drive Straight Path", DriveStraightPath(self.drivetrain, 5)
        )
        SmartDashboard.putData("Auto Chooser", self.autoChooser)

    def teleopExit(self) -> None:
        self.led.blinkGreen().schedule()

    def autonomousInit(self) -> None:
        DriverStation.silenceJoystickConnectionWarning(True)
        self.autonomous = self.autoChooser.getSelected()

        if self.autonomous:
            self.autonomous.schedule()

    def autonomousPeriodic(self) -> None:
        pass

    def autonomousExit(self) -> None:
        CommandScheduler.getInstance().cancelAll()