def initialize(self):
try:
if self.robot.isReal():
self.f = open("/home/lvuser/py/"+self.name)
else:
self.f = open(self.name)
self.reader_iterator = csv.DictReader(self.f)
except FileNotFoundError:
self.reader_iterator = []
self.setTimeout(15)
start_time = Timer.getFPGATimestamp()
for line in self.reader_iterator:
t_delta = float(line["Time"]) - (Timer.getFPGATimestamp()-start_time)
if t_delta > 0:
Timer.delay(t_delta)
self.robot.drivetrain.driveManual(float(line["Drive_X"]),
float(line["Drive_Y"]),
float(line["Drive_Rotation"]))
self.robot.lift.manualSet(float(line["Lift"]))
self.robot.mast.manualSet(float(line["Mast"]))
self.robot.claw.manualSet(float(line["Claw"]))
# self.robot.lock.spike.set(int(line["Lock"]))
self.robot.winch.manualSet(float(line["Winch"]))
if self.isTimedOut() or self.done_yet:
break
def initialize(self):
"""Figure out the file location and play it back."""
try:
#attempt to access the files required
if self.robot.isReal():
self.f = open("/home/lvuser/py/macros/"+self.name)
else:
self.f = open(self.name)
self.reader_iterator = csv.DictReader(self.f)
except FileNotFoundError:
#This bit runs if the file isn't there
self.reader_iterator = []
#length of time to play the macro.
self.setTimeout(Settings.num_macro_timeout)
#start time is important for making sure everything plays at the right time
start_time = Timer.getFPGATimestamp()
#do the actual playback bit
for line in self.reader_iterator:
t_delta = float(line["Time"]) - (Timer.getFPGATimestamp()-start_time)
if t_delta > 0:
Timer.delay(t_delta)
#Add subsystems in the following manner:
#self.robot.subsystem.manualCommand(float(line["Row_Name"]))
self.robot.drivetrain.driveManual(float(line["Drive_X"]),
float(line["Drive_Y"]),
float(line["Drive_Z"]))
if self.isTimedOut() or self.done_yet:
break
def getConfiguration(self):
success = False
retry_count = 0
while retry_count < 5 and not success:
try:
config = self.io_provider.read(IMURegisters.NAVX_REG_WHOAMI,
IMURegisters.NAVX_REG_SENSOR_STATUS_H+1)
except IOError as e:
logger.warning("Error reading configuration data, retrying (%s)", e)
success = False
Timer.delay(0.5)
else:
board_id = self.board_id
board_id.hw_rev = config[IMURegisters.NAVX_REG_HW_REV]
board_id.fw_ver_major = config[IMURegisters.NAVX_REG_FW_VER_MAJOR]
board_id.fw_ver_minor = config[IMURegisters.NAVX_REG_FW_VER_MINOR]
board_id.type = config[IMURegisters.NAVX_REG_WHOAMI]
self.notify_sink._setBoardID(board_id)
board_state = self.board_state
board_state.cal_status = config[IMURegisters.NAVX_REG_CAL_STATUS]
board_state.op_status = config[IMURegisters.NAVX_REG_OP_STATUS]
board_state.selftest_status = config[IMURegisters.NAVX_REG_SELFTEST_STATUS]
board_state.sensor_status = AHRSProtocol.decodeBinaryUint16(config,IMURegisters.NAVX_REG_SENSOR_STATUS_L)
board_state.gyro_fsr_dps = AHRSProtocol.decodeBinaryUint16(config,IMURegisters.NAVX_REG_GYRO_FSR_DPS_L)
board_state.accel_fsr_g = config[IMURegisters.NAVX_REG_ACCEL_FSR_G]
board_state.update_rate_hz = config[IMURegisters.NAVX_REG_UPDATE_RATE_HZ]
board_state.capability_flags = AHRSProtocol.decodeBinaryUint16(config,IMURegisters.NAVX_REG_CAPABILITY_FLAGS_L)
self.notify_sink._setBoardState(board_state)
success = True
retry_count += 1
return success
def getConfiguration(self):
success = False
retry_count = 0
while retry_count < 5 and not success:
try:
config = self.io_provider.read(IMURegisters.NAVX_REG_WHOAMI,
IMURegisters.NAVX_REG_SENSOR_STATUS_H+1)
except IOError as e:
logger.warning("Error reading configuration data, retrying (%s)", e)
success = False
Timer.delay(0.5)
else:
board_id = self.board_id
board_id.hw_rev = config[IMURegisters.NAVX_REG_HW_REV]
board_id.fw_ver_major = config[IMURegisters.NAVX_REG_FW_VER_MAJOR]
board_id.fw_ver_minor = config[IMURegisters.NAVX_REG_FW_VER_MINOR]
board_id.type = config[IMURegisters.NAVX_REG_WHOAMI]
self.notify_sink._setBoardID(board_id)
board_state = self.board_state
board_state.cal_status = config[IMURegisters.NAVX_REG_CAL_STATUS]
board_state.op_status = config[IMURegisters.NAVX_REG_OP_STATUS]
board_state.selftest_status = config[IMURegisters.NAVX_REG_SELFTEST_STATUS]
board_state.sensor_status = AHRSProtocol.decodeBinaryUint16(config,IMURegisters.NAVX_REG_SENSOR_STATUS_L)
board_state.gyro_fsr_dps = AHRSProtocol.decodeBinaryUint16(config,IMURegisters.NAVX_REG_GYRO_FSR_DPS_L)
board_state.accel_fsr_g = config[IMURegisters.NAVX_REG_ACCEL_FSR_G]
board_state.update_rate_hz = config[IMURegisters.NAVX_REG_UPDATE_RATE_HZ]
board_state.capability_flags = AHRSProtocol.decodeBinaryUint16(config,IMURegisters.NAVX_REG_CAPABILITY_FLAGS_L)
self.notify_sink._setBoardState(board_state)
success = True
retry_count += 1
return success
def initialize(self):
try:
if self.robot.isReal():
self.f = open("/home/lvuser/py/" + self.name)
else:
self.f = open(self.name)
self.reader_iterator = csv.DictReader(self.f)
except FileNotFoundError:
self.reader_iterator = []
self.setTimeout(15)
start_time = Timer.getFPGATimestamp()
for line in self.reader_iterator:
t_delta = float(
line["Time"]) - (Timer.getFPGATimestamp() - start_time)
if t_delta > 0:
Timer.delay(t_delta)
self.robot.drivetrain.driveManual(float(line["Drive_X"]),
float(line["Drive_Y"]),
float(line["Drive_Rotation"]))
self.robot.lift.manualSet(float(line["Lift"]))
self.robot.mast.manualSet(float(line["Mast"]))
self.robot.claw.manualSet(float(line["Claw"]))
# self.robot.lock.spike.set(int(line["Lock"]))
self.robot.winch.manualSet(float(line["Winch"]))
if self.isTimedOut() or self.done_yet:
break
def run(self):
logger.info("NavX io thread starting")
try:
self.io_provider.init()
# initial device configuration
self.setUpdateRateHz(self.update_rate_hz)
if not self.getConfiguration():
logger.warning("-- Did not get configuration data")
else:
logger.info(
"-- Board is %s (rev %s)",
IMURegisters.model_type(self.board_id.type),
self.board_id.hw_rev,
)
logger.info(
"-- Firmware %s.%s",
self.board_id.fw_ver_major,
self.board_id.fw_ver_minor,
)
log_error = True
# Calculate delay to match configured update rate
# Note: some additional time is removed from the
# 1/update_rate value to ensure samples are not
# dropped, esp. at higher update rates.
update_rate = 1.0 / (self.update_rate_hz & 0xFF)
if update_rate > DELAY_OVERHEAD_SECONDS:
update_rate -= DELAY_OVERHEAD_SECONDS
logger.info("-- Update rate: %shz (%.4fs)", self.update_rate_hz,
update_rate)
# IO Loop
while not self._stop:
if self.board_state.update_rate_hz != self.update_rate_hz:
self.setUpdateRateHz(self.update_rate_hz)
try:
self.getCurrentData()
except IOError:
if log_error:
logger.exception("Error getting data")
log_error = False
else:
log_error = True
Timer.delay(update_rate)
except Exception:
logger.exception("Unhandled exception in NavX thread")
finally:
logger.info("NavX i/o thread exiting")
def run(self):
logger.info("NavX io thread starting")
try:
self.io_provider.init()
# initial device configuration
self.setUpdateRateHz(self.update_rate_hz)
if not self.getConfiguration():
logger.warning("-- Did not get configuration data")
else:
logger.info("-- Board is %s (rev %s)",
IMURegisters.model_type(self.board_id.type),
self.board_id.hw_rev)
logger.info("-- Firmware %s.%s", self.board_id.fw_ver_major,
self.board_id.fw_ver_minor)
log_error = True
# Calculate delay to match configured update rate
# Note: some additional time is removed from the
# 1/update_rate value to ensure samples are not
# dropped, esp. at higher update rates.
update_rate = 1.0/(self.update_rate_hz & 0xFF)
if update_rate > DELAY_OVERHEAD_SECONDS:
update_rate -= DELAY_OVERHEAD_SECONDS
logger.info("-- Update rate: %shz (%.4fs)",
self.update_rate_hz, update_rate)
# IO Loop
while not self._stop:
if self.board_state.update_rate_hz != self.update_rate_hz:
self.setUpdateRateHz(self.update_rate_hz)
try:
self.getCurrentData()
except IOError:
if log_error:
logger.exception("Error getting data")
log_error = False
else:
log_error = True
Timer.delay(update_rate)
except Exception:
logger.exception("Unhandled exception in NavX thread")
finally:
logger.info("NavX i/o thread exiting")
def initialize(self):
try:
#attempt to access the files required
if self.robot.isReal():
self.f = open("/home/lvuser/py/"+self.name)
else:
self.f = open(self.name)
self.reader_iterator = csv.DictReader(self.f)
except FileNotFoundError:
#This bit runs if the file isn't there
self.reader_iterator = []
self.setTimeout(15)
start_time = Timer.getFPGATimestamp()
for line in self.reader_iterator:
t_delta = float(line["Time"]) - (Timer.getFPGATimestamp()-start_time)
if t_delta > 0:
Timer.delay(t_delta)
self.robot.subsystem.manualSet(float(line["Subsystem"]))
if self.isTimedOut() or self.done_yet:
break
def run(self):
logger.info("NavX io thread starting")
try:
self.io_provider.init()
# initial device configuration
self.setUpdateRateHz(self.update_rate_hz)
if not self.getConfiguration():
logger.warn("-- Did not get configuration data")
else:
logger.info("-- Board is %s (rev %s)",
IMURegisters.model_type(self.board_id.type),
self.board_id.hw_rev)
logger.info("-- Firmware %s.%s", self.board_id.fw_ver_major,
self.board_id.fw_ver_minor)
log_error = True
# IO Loop
while not self._stop:
if self.board_state.update_rate_hz != self.update_rate_hz:
self.setUpdateRateHz(self.update_rate_hz)
try:
self.getCurrentData()
except IOError:
if log_error:
logger.exception("Error getting data")
log_error = False
else:
log_error = True
Timer.delay(1.0/self.update_rate_hz)
except Exception:
logger.exception("Unhandled exception in NavX thread")
finally:
logger.info("NavX i/o thread exiting")
def run(self):
logger.info("NavX io thread starting")
try:
self.io_provider.init()
# initial device configuration
self.setUpdateRateHz(self.update_rate_hz)
if not self.getConfiguration():
logger.warn("-- Did not get configuration data")
else:
logger.info("-- Board is %s (rev %s)",
IMURegisters.model_type(self.board_id.type),
self.board_id.hw_rev)
logger.info("-- Firmware %s.%s", self.board_id.fw_ver_major,
self.board_id.fw_ver_minor)
log_error = True
# IO Loop
while not self._stop:
if self.board_state.update_rate_hz != self.update_rate_hz:
self.setUpdateRateHz(self.update_rate_hz)
try:
self.getCurrentData()
except IOError:
if log_error:
logger.exception("Error getting data")
log_error = False
else:
log_error = True
Timer.delay(1.0 / self.update_rate_hz)
except Exception:
logger.exception("Unhandled exception in NavX thread")
finally:
logger.info("NavX i/o thread exiting")
def initialize(self):
"""Figure out the file location and play it back."""
print("Initializing macro " + self.name + "...")
try:
#attempt to access the files required
if self.robot.isReal():
self.f = open("/home/lvuser/py/macros/" + self.name)
else:
self.f = open(self.name)
self.reader_iterator = csv.DictReader(self.f)
except FileNotFoundError:
#This bit runs if the file isn't there
self.reader_iterator = []
#length of time to play the macro.
self.setTimeout(Settings.num_macro_timeout)
#start time is important for making sure everything plays at the right time
start_time = Timer.getFPGATimestamp()
#do the actual playback bit
for line in self.reader_iterator:
t_delta = float(
line["Time"]) - (Timer.getFPGATimestamp() - start_time)
if t_delta > 0:
Timer.delay(t_delta)
#Add subsystems in the following manner:
#self.robot.subsystem.manualCommand(float(line["Row_Name"]))
self.robot.drivetrain.driveManual(float(line["Drive_Y"]),
float(line["Drive_Twist"]))
self.robot.ears.manualSet(float(line["Ears"]))
self.robot.hat.manualSet(float(line["Hat"]))
self.robot.tilt.manualSet(float(line["Tilt"]))
if self.isTimedOut() or self.done_yet:
break
def poll(self):
rtn_val = self.get()
Timer.delay(.015)
return rtn_val
def poll(self):
rtn_val = self.get()
Timer.delay(.015)
return rtn_val
