def autonomousPeriodic(self):
global autoManager
try:
if not autoManager.gameData:
autoManager.gameData = str(
DriverStation.getInstance().getGameSpecificMessage())
print("Auto Periodic: Game Data = {}".format(
str(autoManager.gameData)))
if len(str(autoManager.gameData)) > 0:
gameDataNearSwitchSide = autoManager.gameData[0]
gameDataScaleSide = autoManager.gameData[1]
autoCommandToRun = autoManager.getAction(
gameDataNearSwitchSide, gameDataScaleSide)
autoCommandToRun.start()
print(
"Auto Periodic: Started command received from AutoManager"
)
else:
print("Auto Periodic: Error - gameData was zero length!")
except Exception as e:
print(
'autonomousPeriodic: Error! Exception caught running autoManager: {}'
.format(e))
super().autonomousPeriodic()
def dump_info():
if hasattr(tankdrive, "encoders"):
smartdashboard.putNumber("left_enc_speed",
tankdrive.encoders["L"].getRate())
smartdashboard.putNumber("left_enc_distance",
tankdrive.encoders["L"].getDistance())
smartdashboard.putNumber("right_enc_speed",
tankdrive.encoders["R"].getRate())
smartdashboard.putNumber("right_enc_distance",
tankdrive.encoders["R"].getDistance())
# smartdashboard.putNumber("arm_proportion", arm.get_arm_proportion())
smartdashboard.putNumber("battery_voltage",
DriverStation.getInstance().getBatteryVoltage())
disp = sensors.navx.getDisplacement()
smartdashboard.putNumber("navx_distance_x", disp[0])
smartdashboard.putNumber("navx_distance_y", disp[1])
smartdashboard.putNumber("navx_distance_z", disp[2])
smartdashboard.putNumber("navx_yaw", sensors.navx.getYaw())
smartdashboard.putNumber("pressure_psi", sensors.get_pressure())
game_data = None
try:
game_data = str(DriverStation.getInstance().getGameSpecificMessage())
except Exception as e:
print(" !!! EXCEPTION: " + str(e))
smartdashboard.putString("game_data", str(game_data))
smartdashboard.putString("game_data_type", type(game_data).__name__)
#print ("DSMSG: " + wpilib.DriverStation.getInstance().getGameSpecificMessage())
# DEBUG
# smartdashboard.putNumber("[TMP] arm ticks", arm.rotator_encoders["R"].get())
wpilib.LiveWindow.run()
def robotInit(self):
print('2018Mule - robotInit called')
if robotmap.sensors.hasAHRS:
try:
robotmap.sensors.ahrs = navx.AHRS.create_spi()
# use via robotmap.sensors.ahrs.getAngle()
print('robotInit: NavX Setup')
except:
if not DriverStation.getInstance().isFmsAttached():
raise
# subsystems must be initialized before things that use them
subsystems.init()
oi.init()
global autoManager
autoManager = FakeAutoManager()
autoManager.initialize()
def autonomousPeriodic(self):
self.generic_loop()
cur_data = str(DriverStation.getInstance().getGameSpecificMessage())
if cur_data != self.auto_data:
print("WARNING: Game data changed, changing autonomous program")
if self.autonomousProgram is not None:
self.autonomousProgram.cancel()
self.generate_auto_program()
if self.autonomousProgram is not None:
subsystems.smartdashboard.putString(
"auto_program_type",
type(self.autonomousProgram).__name__)
subsystems.smartdashboard.putString(
"auto_program", str(self.autonomousProgram))
self.autonomousProgram.start()
super().autonomousPeriodic()
def autonomousInit(self):
"""
Initialization code for autonomous mode should go here. This method will be called each
time the robot enters autonomous mode.
"""
self.scheduleAutonomous = True
if not self.timer.running:
self.timer.start()
# Get the prioritized scoring element, robot starting posion, and the alliance
# scale/switch data.
self.selectedCrossFieldEnable = self.crossFieldChooser.getSelected()
self.selectedScoringElement = self.scoringElementChooser.getSelected()
self.selectedStartingPosition = self.positionChooser.getSelected()
self.gameData = DriverStation.getInstance().getGameSpecificMessage()
self.crossFieldEnable = self.selectedCrossFieldEnable.getCrossFieldEnable(
)
self.scoringElement = self.selectedScoringElement.getScoringElement()
self.startingPosition = self.selectedStartingPosition.getStartingPosition(
)
def autonomousInit(self):
"""
Initialization code for autonomous mode should go here. This method will be called each
time the robot enters autonomous mode.
"""
self.scheduleAutonomous = True
if not self.timer.running:
self.timer.start()
# The game specific data will be a 3-character string representing where the teams switch,
# scale, switch are located. For example, "LRR" means your teams closest switch is on the
# left (as you look out onto the field from the drivers station). The teams scale is on
# the right, and the switch furthest away is also on the right.
self.startingPosition = self.startSpotChooser.getSelected()
self.scaleDisable = self.scaleDisableChooser.getSelected()
self.gameData = DriverStation.getInstance().getGameSpecificMessage()
logger.info("Game Data: %s" % (self.gameData))
logger.info("Starting Position %s" % (self.startingPosition))
logger.info("Scale Enable %s" % (self.scaleDisable))
self.autonCommand = self.chooserOptions[self.startingPosition][self.gameData[0]][self.gameData[1]][self.scaleDisable]['command'](self)
self.autonCommand.start()
def generate_auto_program(self):
choice = self.chooser.getSelected()
data = None
try:
data = str(DriverStation.getInstance().getGameSpecificMessage())
except Exception as e:
print("!!! exception: " + str(e))
if data is not None and len(data) == 3:
if choice == "l":
self.autonomousProgram = commands.auto.get_left_command(data)
elif choice == "m":
self.autonomousProgram = commands.auto.get_middle_command(data)
elif choice == "r":
self.autonomousProgram = commands.auto.get_right_command(data)
else:
self.autonomousProgram = choice
else:
self.autonomousProgram = DriveToDistance(3.048, 3.048)
#self.autonomousProgram = DoNothing(15)
#self.autonomousProgram = wpilib.command.CommandGroup()
#self.autonomousProgram.addParallel(self.chooser.getSelected())
self.auto_data = data
def ds_except_hook(exctype, value, traceback):
DriverStation.reportError("ERROR Unhandled {}".format(exctype), True)
sys.__excepthook__(exctype, value, traceback)
def update_sd(self):
SmartDashboard.putBoolean("time_running", True)
SmartDashboard.putNumber(
"time_remaining",
DriverStation.getInstance().getMatchTime() - 15)
def __init__(self):
self.oi = Conts()
self.ds = DriverStation.getInstance()
self.infont = NetworkTables.getTable('info')
self.sensors = Sensors()
def get_match_time():
ds = DriverStation.getInstance()
if ds.isFMSAttached():
return ds.getMatchTime()
else:
return calendar.timegm(time.gmtime()) - _t0
def __init__(self):
super().__init__()
self.testSelector = Selector([("row1", "op1", "op2", "op3")], (0, ))
DriverStation.getInstance()