2018PowerUp/automanager.py at master · FRCTeam279/2018PowerUp · 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
155
156
157
158
159
160
161
162
163
164
165
166
167
import wpilib
from wpilib.smartdashboard import SmartDashboard

from commands.autodriveforwardtoswitch import AutoDriveForwardToSwitch
from commands.autoloadscaletoleft import AutoLoadScaleToLeft
from commands.autoloadscaletoright import AutoLoadScaleToRight
from commands.autoloadswitchfrommid import AutoLoadSwitchFromMid
from commands.autoloadswitchtoleftfromside import AutoLoadSwitchToLeftFromSide
from commands.autoloadswitchtorightfromside import AutoLoadSwitchToRightFromSide

class AutoManager:
    """
    preferred element:
        'crossLine' - cross auto line
        'switch - prefer switch
        'scale' - prefer scale
    starting side:
        'left'
        'middle'
        'right'
    """
    def __init__(self):
        self.preferredElement = None
        self.startingPosition = None
        self.initialized = False

        self.gameData = None
        self.gameDataNearSwitchSide = None
        self.gameDataScaleSide = None

    def initialize(self):
        if self.initialized:
            return

        # initializing auto chooser
        self.preferredElement = wpilib.SendableChooser()
        self.preferredElement.addObject('crossLine', 0)
        self.preferredElement.addObject('switch', 1)
        self.preferredElement.addObject('scale', 2)
        self.preferredElement.addObject('switch only no scale', 3)

        self.startingPosition = wpilib.SendableChooser()
        self.startingPosition.addObject('left', 0)
        self.startingPosition.addObject('middle', 1)
        self.startingPosition.addObject('right', 2)

        SmartDashboard.putData("Preferred Element", self.preferredElement)
        SmartDashboard.putData("Starting Position", self.startingPosition)
        print("AutoManager: Initialized")
        self.initialized = True

    def getStartingPosition(self):
        return self.startingPosition.getSelected()

    def getPreferredElement(self):
        return self.preferredElement.getSelected()

    # parameters are the L or R for each object from game data
    def getAction(self, nearSwitchSide, scaleSide):

        # ---------------------------------------------
        # Middle Starting Spot
        # ---------------------------------------------
        if self.getStartingPosition() == 1:     # 1 = 'middle'
            print("AutoManager.GetAction: Starting in middle, nearSwitchSide={}, returning AutoLoadSwitchFromMid".format(nearSwitchSide))
            return AutoLoadSwitchFromMid(nearSwitchSide)

        # ---------------------------------------------
        # Left Starting Spot
        # ---------------------------------------------
        if self.getStartingPosition() == 0:     # 0 = 'left'

            if self.getPreferredElement() == 0:     # 0 = 'crossLine'
                print("AutoManager.GetAction: AutoDriveForward - Preferred={}, Starting={}, NearSwitch={}, Scale={}".format(
                        self.getPreferredElement(), self.getStartingPosition(), nearSwitchSide, scaleSide))
                return AutoDriveForwardToSwitch()

            if self.getPreferredElement() == 1 and nearSwitchSide=='L':     #1 = 'switch'
                print("AutoManager.GetAction: AutoLoadSwitchToLeftFromSide - Preferred={}, Starting={}, NearSwitch={}, Scale={}".format(
                        self.getPreferredElement(), self.getStartingPosition(), nearSwitchSide, scaleSide))
                return AutoLoadSwitchToLeftFromSide()

            if self.getPreferredElement() == 2 and scaleSide=='L':      # = 'scale'
                print("AutoManager.GetAction: AutoLoadScaleToLeft - Preferred={}, Starting={}, NearSwitch={}, Scale={}".format(
                        self.getPreferredElement(), self.getStartingPosition(), nearSwitchSide, scaleSide))
                return AutoLoadScaleToLeft()
                #return AutoDriveForwardToSwitch()

            if self.getPreferredElement() == 3 and nearSwitchSide=='L':  # = 'scale'
                print(
                    "AutoManager.GetAction: AutoLoadSwitchToLeftFromSide - Preferred={}, Starting={}, NearSwitch={}, Scale={}".format(
                        self.getPreferredElement(), self.getStartingPosition(), nearSwitchSide, scaleSide))
                # return AutoLoadScaleToLeft()
                return AutoLoadSwitchToLeftFromSide()

            # Fall back number 1 - the other element
            # if the preferred element isn't on this side
            #  return either of the others if they are...
            if (self.getPreferredElement() == 1 and nearSwitchSide != 'L') or (             # 1 = 'switch'
                            self.getPreferredElement() == 2 and scaleSide != 'L'):          # 2 = 'scale'
                if nearSwitchSide=='L':
                    print("AutoManager.GetAction: AutoLoadSwitchToLeftFromSide - Preferred={}, Starting={}, NearSwitch={}, Scale={}".format(
                            self.getPreferredElement(), self.getStartingPosition(), nearSwitchSide, scaleSide))
                    return AutoLoadSwitchToLeftFromSide()
                if scaleSide == 'L':
                    print("AutoManager.GetAction: AutoLoadScaleToLeft - Preferred={}, Starting={}, NearSwitch={}, Scale={}".format(
                            self.getPreferredElement(), self.getStartingPosition(), nearSwitchSide, scaleSide))
                    #return AutoLoadScaleToLeft()
                    return AutoDriveForwardToSwitch()
            if self.getPreferredElement() == 3 and nearSwitchSide != 'L':
                print(
                    "AutoManager.GetAction: AutoLoadScaleToLeft - Preferred={}, Starting={}, NearSwitch={}, Scale={}".format(
                        self.getPreferredElement(), self.getStartingPosition(), nearSwitchSide, scaleSide))
                return AutoDriveForwardToSwitch()

        # ---------------------------------------------
        # Right Starting Spot
        # ---------------------------------------------
        if self.getStartingPosition() == 2:     # 2= 'right'
            if self.getPreferredElement() == 0:        # 0 = 'crossLine'
                print("AutoManager.GetAction: AutoDriveForward - Preferred={}, Starting={}, NearSwitch={}, Scale={}".format(
                        self.getPreferredElement(), self.getStartingPosition(), nearSwitchSide, scaleSide))
                return AutoDriveForwardToSwitch()

            if self.getPreferredElement() == 1 and nearSwitchSide=='R':     # 1 = 'switch'
                print("AutoManager.GetAction: AutoLoadSwitchToRightFromSide - Preferred={}, Starting={}, NearSwitch={}, Scale={}".format(
                        self.getPreferredElement(), self.getStartingPosition(), nearSwitchSide, scaleSide))
                return AutoLoadSwitchToRightFromSide()

            if self.getPreferredElement() == 2 and scaleSide=='R':          # 2 = 'scale'
                print("AutoManager.GetAction: AutoLoadScaleToRight - Preferred={}, Starting={}, NearSwitch={}, Scale={}".format(
                        self.getPreferredElement(), self.getStartingPosition(), nearSwitchSide, scaleSide))
                return AutoLoadScaleToRight()
                #return AutoDriveForwardToSwitch()

            if self.getPreferredElement() == 3 and nearSwitchSide=='R':          # 3 = 'switch only no scale'
                print("AutoManager.GetAction: AutoLoadSwitchToRightFromSide - Preferred={}, Starting={}, NearSwitch={}, Scale={}".format(
                        self.getPreferredElement(), self.getStartingPosition(), nearSwitchSide, scaleSide))
                return AutoLoadSwitchToRightFromSide()

            # Fall back number 1 - the other element
            # if the preferred element isn't on this side
            #  return either of the others if they are...
            if (self.getPreferredElement() == 1 and nearSwitchSide != 'R') or (             # 1 = 'switch'
                            self.getPreferredElement() == 2 and scaleSide != 'R'):          # 2 = 'scale'
                if nearSwitchSide=='R':
                    print("AutoManager.GetAction: AutoLoadSwitchToRightFromSide - Preferred={}, Starting={}, NearSwitch={}, Scale={}".format(
                            self.getPreferredElement(), self.getStartingPosition(), nearSwitchSide, scaleSide))
                    return AutoLoadSwitchToRightFromSide()
                if scaleSide == 'R':
                    print("AutoManager.GetAction: AutoLoadScaleToRight - Preferred={}, Starting={}, NearSwitch={}, Scale={}".format(
                            self.getPreferredElement(), self.getStartingPosition(), nearSwitchSide, scaleSide))
                    return AutoLoadScaleToRight()
                    #return AutoDriveForwardToSwitch()
            if self.getPreferredElement() == 3 and nearSwitchSide != 'R':
                print(
                    "AutoManager.GetAction: AutoDriveForward - Preferred={}, Starting={}, NearSwitch={}, Scale={}".format(
                        self.getPreferredElement(), self.getStartingPosition(), nearSwitchSide, scaleSide))
                return AutoDriveForwardToSwitch()

        # ---------------------------------------------
        # Fall back number 2 - cross the line
        # ---------------------------------------------
        print(
            "AutoManager.GetAction: Defaulting to CrossLine - Preferred={}, Starting={}, NearSwitch={}, Scale={}".format(
                self.getPreferredElement(), self.getStartingPosition(), nearSwitchSide, scaleSide))
        return AutoDriveForwardToSwitch()