def robotInit(self):
"""
Robot-wide initialization code goes here. For the command-based programming framework,
this means creating the subsytem objects and the operator input object. BE SURE TO CREATE
THE SUBSYTEM OBJECTS BEFORE THE OPERATOR INPUT OBJECT (since the operator input object
will almost certainly have subsystem dependencies). For further information on the
command-based programming framework see:
wpilib.screenstepslive.com/s/currentCS/m/java/l/599732-what-is-command-based-programming
"""
# Create the subsytems and the operator interface objects
self.driveTrain = DriveTrain(self)
self.boom = Boom(self)
self.intakeMotors = IntakeMotors(self)
self.intakePneumatics = IntakePneumatics(self)
self.oi = OI(self)
# Create the smartdashboard object
self.smartDashboard = SmartDashboard()
# Create the sendable choosers to get the autonomous preferences
self.scoringElementChooser = SendableChooser()
self.scoreScale = ScoreScale()
self.scoreSwitch = ScoreSwitch()
self.scoringElementChooser.addObject("Score Scale", self.scoreScale)
self.scoringElementChooser.addObject("Score Switch", self.scoreSwitch)
self.scoringElementChooser.addDefault("Score Scale", self.scoreScale)
self.smartDashboard.putData("Score Field Element",
self.scoringElementChooser)
self.crossFieldChooser = SendableChooser()
self.crossFieldEnable = CrossFieldEnable()
self.crossFieldDisable = CrossFieldDisable()
self.crossFieldChooser.addObject("Cross Field Enable",
self.crossFieldEnable)
self.crossFieldChooser.addObject("Cross Field Disable",
self.crossFieldDisable)
self.crossFieldChooser.addDefault("Cross Field Disable",
self.crossFieldDisable)
self.smartDashboard.putData("Cross Field Enable",
self.crossFieldChooser)
self.positionChooser = SendableChooser()
self.startingLeft = StartingLeft()
self.startingRight = StartingRight()
self.startingMiddle = StartingMiddle()
self.positionChooser.addObject("Start Left", self.startingLeft)
self.positionChooser.addObject("Start Right", self.startingRight)
self.positionChooser.addObject("Start Middle", self.startingMiddle)
self.positionChooser.addDefault("Start Middle", self.startingMiddle)
self.smartDashboard.putData("Starting Position", self.positionChooser)
# Create a timer for data logging
self.timer = Timer()
# Create the camera server
CameraServer.launch()
# Boom state start at the scale
self.boomState = BOOM_STATE.Scale
self.autonForward = AutonForward(self)
self.autonMiddleStartLeftSwitch = AutonMiddleStartLeftSwitch(self)
self.autonLeftStartLeftScale = AutonLeftStartLeftScale(self)
# Output debug data to the smartdashboard
if LOGGER_LEVEL == logging.DEBUG:
#=======================================================================================
# self.smartDashboard.putNumber("RightEncPos", 0.0)
# self.smartDashboard.putNumber("RightActPos", 0.0)
# self.smartDashboard.putNumber("RightEncVel", 0.0)
# self.smartDashboard.putNumber("RightActVel", 0.0)
# self.smartDashboard.putNumber("RightPrimaryTarget", 0.0)
# self.smartDashboard.putNumber("RightPrimaryError", 0.0)
# self.smartDashboard.putNumber("TimeStamp", 0.0)
# self.smartDashboard.putNumber("LeftEncPos", 0.0)
# self.smartDashboard.putNumber("LeftActPos", 0.0)
# self.smartDashboard.putNumber("LeftEncVel", 0.0)
# self.smartDashboard.putNumber("LeftActVel", 0.0)
# self.smartDashboard.putNumber("LeftPrimaryTarget", 0.0)
# self.smartDashboard.putNumber("LeftPrimaryError", 0.0)
# self.smartDashboard.putNumber("RightTopBufferCount", 0.0)
# self.smartDashboard.putNumber("LeftTopBufferCount", 0.0)
# self.smartDashboard.putNumber("LeftBottomBufferCount", 0.0)
# self.smartDashboard.putNumber("RightBottomBufferCount", 0.0)
#=======================================================================================
self.smartDashboard.putNumber("EncPos", 0.0)
self.smartDashboard.putNumber("ActPos", 0.0)
self.smartDashboard.putNumber("EncVel", 0.0)
self.smartDashboard.putNumber("ActVel", 0.0)
self.smartDashboard.putNumber("PrimaryTarget", 0.0)
self.smartDashboard.putNumber("PrimaryError", 0.0)
self.smartDashboard.putNumber("TimeStamp", 0.0)
def robotInit(self):
"""
Robot-wide initialization code goes here. For the command-based programming framework,
this means creating the subsytem objects and the operator input object. BE SURE TO CREATE
THE SUBSYTEM OBJECTS BEFORE THE OPERATOR INPUT OBJECT (since the operator input object
will almost certainly have subsystem dependencies). For further information on the
command-based programming framework see:
wpilib.screenstepslive.com/s/currentCS/m/java/l/599732-what-is-command-based-programming
"""
# Create the subsytems and the operator interface objects
self.driveTrain = DriveTrain(self)
self.boom = Boom(self)
self.intakeMotors = IntakeMotors(self)
self.intakePneumatics = IntakePneumatics(self)
self.oi = OI(self)
# Create the smartdashboard object
self.smartDashboard = SmartDashboard()
# Create the sendable choosers to get the autonomous preferences
self.startSpotChooser = SendableChooser()
self.startSpotChooser.addObject("Start Left", 'Left')
self.startSpotChooser.addObject("Start Right", 'Right')
self.startSpotChooser.addDefault("Start Middle", 'Middle')
self.smartDashboard.putData("Starting Spot", self.startSpotChooser)
self.scaleDisableChooser = SendableChooser()
self.scaleDisableChooser.addObject("Enable Scale", 'Scale')
self.scaleDisableChooser.addDefault("Disable Scale", 'No Scale')
self.smartDashboard.putData("Scale Enable", self.scaleDisableChooser)
# Build up the autonomous dictionary. Fist key is the starting position. The second key is the switch. The third key is the scale.
self.chooserOptions = {"Left": {"R": {"R": {"No Scale": {'command': AutonForward},
"Scale": {'command': AutonForward}
},
"L": {"No Scale": {'command': AutonForward},
"Scale": {'command': AutonForward}
},
},
"L": {"R": {"No Scale": {'command': AutonLeftStartLeftSwitch},
"Scale": {'command': AutonLeftStartLeftSwitch}
},
"L": {"No Scale": {'command': AutonLeftStartLeftSwitch},
"Scale": {'command': AutonLeftStartLeftSwitch}
},
},
},
"Middle": {"R": {"R": {"No Scale": {'command': AutonMiddleStartRightSwitch},
"Scale": {'command': AutonMiddleStartRightSwitch}
},
"L": {"No Scale": {'command': AutonMiddleStartRightSwitch},
"Scale": {'command': AutonMiddleStartRightSwitch}
},
},
"L": {"R": {"No Scale": {'command': AutonMiddleStartLeftSwitch},
"Scale": {'command': AutonMiddleStartLeftSwitch}
},
"L": {"No Scale": {'command': AutonMiddleStartLeftSwitch},
"Scale": {'command': AutonMiddleStartLeftSwitch}
},
},
},
"Right": {"R": {"R": {"No Scale": {'command': AutonRightStartRightSwitch},
"Scale": {'command': AutonRightStartRightSwitch}
},
"L": {"No Scale": {'command': AutonRightStartRightSwitch},
"Scale": {'command': AutonRightStartRightSwitch}
},
},
"L": {"R": {"No Scale": {'command': AutonForward},
"Scale": {'command': AutonForward}
},
"L": {"No Scale": {'command': AutonForward},
"Scale": {'command': AutonForward}
},
},
},
}
# Create a timer for data logging
self.timer = Timer()
# Create the camera server
CameraServer.launch()
# Boom state start at the scale
self.boomState = BOOM_STATE.Scale
def robotInit(self):
self.drive = driveTrain(self)
self.interpret = interpret()
self.controllerOne = XboxController(0)
self.controllerTwo = XboxController(1)
self.speedLimiter = 1 #1 = standard speed, greater than 1 to slow down, less than 1 to speed up
self.dashTimer = wpilib.Timer() # Timer for SmartDashboard updating
self.dashTimer.start()
self.dash = SmartDashboard()
self.positionChooser = wpilib.SendableChooser()
self.positionChooser.addDefault('Position Chooser', '1')
self.positionChooser.addObject('left', 'left')
self.positionChooser.addObject('right', 'right')
self.positionChooser.addObject('center', 'center')
self.switchLscaleL = wpilib.SendableChooser()
self.switchLscaleL.addDefault('Switch and Scale LEFT', '1')
self.switchLscaleL.addObject('Scale', 'scale')
self.switchLscaleL.addObject('Switch', 'switch')
#switchLscaleL.addObject('PrepScaleScore', '4')
self.switchLscaleL.addDefault(
'Drive', 'drive') ##Default for all sendable Choosers
self.switchLscaleL.addObject('Two Cube Scale', 'Two Cube Scale')
self.switchRscaleR = wpilib.SendableChooser()
self.switchRscaleR.addDefault('Switch and Scale RIGHT', '1')
self.switchRscaleR.addObject('Scale', 'scale')
self.switchRscaleR.addObject('Switch', 'switch')
#switchRscaleR.addObject('PrepScaleScore', '4')
self.switchRscaleR.addDefault('Drive', 'drive')
self.switchRscaleR.addObject('Two Cube Scale', 'Two Cube Scale')
self.switchRscaleL = wpilib.SendableChooser()
self.switchRscaleL.addDefault('Switch RIGHT, Scale LEFT', '1')
self.switchRscaleL.addObject('Scale', 'scale')
self.switchRscaleL.addObject('Switch', 'switch')
#switchRscaleL.addObject('PrepScaleScore', '4')
self.switchRscaleL.addDefault('Drive', 'drive')
self.switchRscaleL.addObject('Two Cube Scale', 'Two Cube Scale')
self.switchLscaleR = wpilib.SendableChooser()
self.switchLscaleR.addDefault('Switch LEFT, Scale RIGHT', '1')
self.switchLscaleR.addObject('Scale', 'scale')
self.switchLscaleR.addObject('Switch', 'switch')
#switchLscaleR.addObject('PrepScaleScore', '4')
self.switchLscaleR.addDefault('Drive', 'drive')
self.switchLscaleR.addObject('Two Cube Scale', 'Two Cube Scale')
#print('Dashboard Test')
wpilib.SmartDashboard.putData('Starting Position',
self.positionChooser)
wpilib.SmartDashboard.putData('Switch and Scale Left',
self.switchLscaleL)
wpilib.SmartDashboard.putData('Switch Right, Scale Left',
self.switchRscaleL)
wpilib.SmartDashboard.putData('Switch and Scale Right',
self.switchRscaleR)
wpilib.SmartDashboard.putData('Switch Left, Scale Right',
self.switchLscaleR)
#self.dash.putData('Switch Left, Scale Right', switchLscaleR)
self.dash.putString('SanityCheck', '1')
self.dashTimer = wpilib.Timer() # Timer for SmartDashboard updating
self.dashTimer.start()
