7407-DriveCode-Reefscape/robot_systems.py at main · Choate-Robotics/7407-DriveCode-Reefscape · 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
import wpilib  # noqa
import config
import constants
from utils.field import (
    FieldConstants,
    ReefFace,
    ReefHeight,
    Branch,
    Reef,
    Barge,
    StagingPositions,
    CoralStation,
    Processor,
    ManualBranchesRed,
    ManualBranchesBlue,
    flip_poses,
    update_table,
    NT_Updater,
)

import sensors
import subsystem


class Robot:
    elevator = subsystem.Elevator()
    wrist = subsystem.Wrist()
    drivetrain = subsystem.Drivetrain()
    intake = subsystem.Intake()
    climber = subsystem.Climber()


class Pneumatics:
    pass


class Sensors:
    right_cam = sensors.PhotonCamCustom(config.right_cam_name, constants.robot_to_right_cam)
    left_cam = sensors.PhotonCamCustom(config.left_cam_name, constants.robot_to_left_cam)
    cam_controller = sensors.PhotonController([left_cam, right_cam])
    # cam_controller = None


class LEDs:
    pass


class PowerDistribution:
    pass


class Field:
    odometry = sensors.FieldOdometry(Robot.drivetrain, Sensors.cam_controller)
    field_constants = FieldConstants()
    reef_face = ReefFace
    branch = Branch
    reef_height = ReefHeight
    reef = Reef
    barge = Barge
    staging_positions = StagingPositions
    coral_station = CoralStation
    processor = Processor
    manual_branches_red = ManualBranchesRed
    manual_branches_blue = ManualBranchesBlue
    nt_reporter = NT_Updater("Field")

    @staticmethod
    def flip_poses():
        print("Flipping Pos")
        flip_poses()

    @staticmethod
    def get_branches():
        if wpilib.DriverStation.getAlliance() == wpilib.DriverStation.Alliance.kRed:
            return Field.manual_branches_red
        else:
            return Field.manual_branches_blue

    @staticmethod
    def update_field_table(debug=False):
        print("Updating Table")
        update_table(Field.nt_reporter, False)