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 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):
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):
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 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 isConnected(self):
time_since_last_update = Timer.getFPGATimestamp(
) - self.last_update_time
return time_since_last_update <= IO_TIMEOUT_SECONDS
def getCurrentData(self):
first_address = IMURegisters.NAVX_REG_UPDATE_RATE_HZ
displacement_registers = self.board_capabilities._isDisplacementSupported(
)
# If firmware supports displacement data, acquire it - otherwise implement
# similar (but potentially less accurate) calculations on this processor.
if displacement_registers:
read_count = IMURegisters.NAVX_REG_LAST + 1 - first_address
else:
read_count = IMURegisters.NAVX_REG_QUAT_OFFSET_Z_H + 1 - first_address
curr_data = self.io_provider.read(first_address, read_count)
#timestamp_low = AHRSProtocol.decodeBinaryUint16(curr_data, IMURegisters.NAVX_REG_TIMESTAMP_L_L-first_address)
#timestamp_high = AHRSProtocol.decodeBinaryUint16(curr_data, IMURegisters.NAVX_REG_TIMESTAMP_H_L-first_address)
#sensor_timestamp = (timestamp_high << 16) + timestamp_low
ahrspos_update = self.ahrspos_update
ahrspos_update.op_status = curr_data[IMURegisters.NAVX_REG_OP_STATUS -
first_address]
ahrspos_update.selftest_status = curr_data[
IMURegisters.NAVX_REG_SELFTEST_STATUS - first_address]
ahrspos_update.cal_status = curr_data[IMURegisters.NAVX_REG_CAL_STATUS]
ahrspos_update.sensor_status = curr_data[
IMURegisters.NAVX_REG_SENSOR_STATUS_L - first_address]
ahrspos_update.yaw = AHRSProtocol.decodeProtocolSignedHundredthsFloat(
curr_data, IMURegisters.NAVX_REG_YAW_L - first_address)
ahrspos_update.pitch = AHRSProtocol.decodeProtocolSignedHundredthsFloat(
curr_data, IMURegisters.NAVX_REG_PITCH_L - first_address)
ahrspos_update.roll = AHRSProtocol.decodeProtocolSignedHundredthsFloat(
curr_data, IMURegisters.NAVX_REG_ROLL_L - first_address)
ahrspos_update.compass_heading = AHRSProtocol.decodeProtocolUnsignedHundredthsFloat(
curr_data, IMURegisters.NAVX_REG_HEADING_L - first_address)
ahrspos_update.mpu_temp_c = AHRSProtocol.decodeProtocolSignedHundredthsFloat(
curr_data, IMURegisters.NAVX_REG_MPU_TEMP_C_L - first_address)
ahrspos_update.world_linear_accel_x = AHRSProtocol.decodeProtocolSignedThousandthsFloat(
curr_data, IMURegisters.NAVX_REG_LINEAR_ACC_X_L - first_address)
ahrspos_update.world_linear_accel_y = AHRSProtocol.decodeProtocolSignedThousandthsFloat(
curr_data, IMURegisters.NAVX_REG_LINEAR_ACC_Y_L - first_address)
ahrspos_update.world_linear_accel_z = AHRSProtocol.decodeProtocolSignedThousandthsFloat(
curr_data, IMURegisters.NAVX_REG_LINEAR_ACC_Z_L - first_address)
ahrspos_update.altitude = AHRSProtocol.decodeProtocol1616Float(
curr_data, IMURegisters.NAVX_REG_ALTITUDE_D_L - first_address)
ahrspos_update.baro_pressure = AHRSProtocol.decodeProtocol1616Float(
curr_data, IMURegisters.NAVX_REG_PRESSURE_DL - first_address)
ahrspos_update.fused_heading = AHRSProtocol.decodeProtocolUnsignedHundredthsFloat(
curr_data, IMURegisters.NAVX_REG_FUSED_HEADING_L - first_address)
ahrspos_update.quaternionW = AHRSProtocol.decodeBinaryInt16(
curr_data, IMURegisters.NAVX_REG_QUAT_W_L - first_address)
ahrspos_update.quaternionX = AHRSProtocol.decodeBinaryInt16(
curr_data, IMURegisters.NAVX_REG_QUAT_X_L - first_address)
ahrspos_update.quaternionY = AHRSProtocol.decodeBinaryInt16(
curr_data, IMURegisters.NAVX_REG_QUAT_Y_L - first_address)
ahrspos_update.quaternionZ = AHRSProtocol.decodeBinaryInt16(
curr_data, IMURegisters.NAVX_REG_QUAT_Z_L - first_address)
if displacement_registers:
ahrspos_update.vel_x = AHRSProtocol.decodeProtocol1616Float(
curr_data, IMURegisters.NAVX_REG_VEL_X_I_L - first_address)
ahrspos_update.vel_y = AHRSProtocol.decodeProtocol1616Float(
curr_data, IMURegisters.NAVX_REG_VEL_Y_I_L - first_address)
ahrspos_update.vel_z = AHRSProtocol.decodeProtocol1616Float(
curr_data, IMURegisters.NAVX_REG_VEL_Z_I_L - first_address)
ahrspos_update.disp_x = AHRSProtocol.decodeProtocol1616Float(
curr_data, IMURegisters.NAVX_REG_DISP_X_I_L - first_address)
ahrspos_update.disp_y = AHRSProtocol.decodeProtocol1616Float(
curr_data, IMURegisters.NAVX_REG_DISP_Y_I_L - first_address)
ahrspos_update.disp_z = AHRSProtocol.decodeProtocol1616Float(
curr_data, IMURegisters.NAVX_REG_DISP_Z_I_L - first_address)
self.notify_sink._setAHRSPosData(ahrspos_update)
else:
self.notify_sink._setAHRSData(ahrspos_update)
board_state = self.board_state
board_state.cal_status = curr_data[IMURegisters.NAVX_REG_CAL_STATUS -
first_address]
board_state.op_status = curr_data[IMURegisters.NAVX_REG_OP_STATUS -
first_address]
board_state.selftest_status = curr_data[
IMURegisters.NAVX_REG_SELFTEST_STATUS - first_address]
board_state.sensor_status = AHRSProtocol.decodeBinaryUint16(
curr_data, IMURegisters.NAVX_REG_SENSOR_STATUS_L - first_address)
board_state.update_rate_hz = curr_data[
IMURegisters.NAVX_REG_UPDATE_RATE_HZ - first_address]
board_state.gyro_fsr_dps = AHRSProtocol.decodeBinaryUint16(
curr_data, IMURegisters.NAVX_REG_GYRO_FSR_DPS_L)
board_state.accel_fsr_g = curr_data[IMURegisters.NAVX_REG_ACCEL_FSR_G]
board_state.capability_flags = AHRSProtocol.decodeBinaryUint16(
curr_data,
IMURegisters.NAVX_REG_CAPABILITY_FLAGS_L - first_address)
self.notify_sink._setBoardState(board_state)
raw_data_update = self.raw_data_update
raw_data_update.raw_gyro_x = AHRSProtocol.decodeBinaryInt16(
curr_data, IMURegisters.NAVX_REG_GYRO_X_L - first_address)
raw_data_update.raw_gyro_y = AHRSProtocol.decodeBinaryInt16(
curr_data, IMURegisters.NAVX_REG_GYRO_Y_L - first_address)
raw_data_update.raw_gyro_z = AHRSProtocol.decodeBinaryInt16(
curr_data, IMURegisters.NAVX_REG_GYRO_Z_L - first_address)
raw_data_update.raw_accel_x = AHRSProtocol.decodeBinaryInt16(
curr_data, IMURegisters.NAVX_REG_ACC_X_L - first_address)
raw_data_update.raw_accel_y = AHRSProtocol.decodeBinaryInt16(
curr_data, IMURegisters.NAVX_REG_ACC_Y_L - first_address)
raw_data_update.raw_accel_z = AHRSProtocol.decodeBinaryInt16(
curr_data, IMURegisters.NAVX_REG_ACC_Z_L - first_address)
raw_data_update.cal_mag_x = AHRSProtocol.decodeBinaryInt16(
curr_data, IMURegisters.NAVX_REG_MAG_X_L - first_address)
raw_data_update.cal_mag_y = AHRSProtocol.decodeBinaryInt16(
curr_data, IMURegisters.NAVX_REG_MAG_Y_L - first_address)
raw_data_update.cal_mag_z = AHRSProtocol.decodeBinaryInt16(
curr_data, IMURegisters.NAVX_REG_MAG_Z_L - first_address)
raw_data_update.mpu_temp_c = ahrspos_update.mpu_temp
self.notify_sink._setRawData(raw_data_update)
self.last_update_time = Timer.getFPGATimestamp()
self.byte_count += len(curr_data)
self.update_count += 1
def isConnected(self):
time_since_last_update = Timer.getFPGATimestamp() - self.last_update_time
return time_since_last_update <= IO_TIMEOUT_SECONDS
def getCurrentData(self):
first_address = IMURegisters.NAVX_REG_UPDATE_RATE_HZ
displacement_registers = self.board_capabilities._isDisplacementSupported()
# If firmware supports displacement data, acquire it - otherwise implement
# similar (but potentially less accurate) calculations on this processor.
if displacement_registers:
read_count = IMURegisters.NAVX_REG_LAST + 1 - first_address
else:
read_count = IMURegisters.NAVX_REG_QUAT_OFFSET_Z_H + 1 - first_address
curr_data = self.io_provider.read(first_address, read_count)
sensor_timestamp = AHRSProtocol.decodeBinaryUint32(curr_data, IMURegisters.NAVX_REG_TIMESTAMP_L_L-first_address)
if sensor_timestamp == self.last_sensor_timestamp:
return
self.last_sensor_timestamp = sensor_timestamp
ahrspos_update = self.ahrspos_update
ahrspos_update.op_status = curr_data[IMURegisters.NAVX_REG_OP_STATUS - first_address]
ahrspos_update.selftest_status = curr_data[IMURegisters.NAVX_REG_SELFTEST_STATUS - first_address]
ahrspos_update.cal_status = curr_data[IMURegisters.NAVX_REG_CAL_STATUS]
ahrspos_update.sensor_status = curr_data[IMURegisters.NAVX_REG_SENSOR_STATUS_L - first_address]
ahrspos_update.yaw = AHRSProtocol.decodeProtocolSignedHundredthsFloat(curr_data, IMURegisters.NAVX_REG_YAW_L-first_address)
ahrspos_update.pitch = AHRSProtocol.decodeProtocolSignedHundredthsFloat(curr_data, IMURegisters.NAVX_REG_PITCH_L-first_address)
ahrspos_update.roll = AHRSProtocol.decodeProtocolSignedHundredthsFloat(curr_data, IMURegisters.NAVX_REG_ROLL_L-first_address)
ahrspos_update.compass_heading = AHRSProtocol.decodeProtocolUnsignedHundredthsFloat(curr_data, IMURegisters.NAVX_REG_HEADING_L-first_address)
ahrspos_update.mpu_temp_c = AHRSProtocol.decodeProtocolSignedHundredthsFloat(curr_data, IMURegisters.NAVX_REG_MPU_TEMP_C_L - first_address)
ahrspos_update.world_linear_accel_x = AHRSProtocol.decodeProtocolSignedThousandthsFloat(curr_data, IMURegisters.NAVX_REG_LINEAR_ACC_X_L-first_address)
ahrspos_update.world_linear_accel_y = AHRSProtocol.decodeProtocolSignedThousandthsFloat(curr_data, IMURegisters.NAVX_REG_LINEAR_ACC_Y_L-first_address)
ahrspos_update.world_linear_accel_z = AHRSProtocol.decodeProtocolSignedThousandthsFloat(curr_data, IMURegisters.NAVX_REG_LINEAR_ACC_Z_L-first_address)
ahrspos_update.altitude = AHRSProtocol.decodeProtocol1616Float(curr_data, IMURegisters.NAVX_REG_ALTITUDE_D_L - first_address)
ahrspos_update.baro_pressure = AHRSProtocol.decodeProtocol1616Float(curr_data, IMURegisters.NAVX_REG_PRESSURE_DL - first_address)
ahrspos_update.fused_heading = AHRSProtocol.decodeProtocolUnsignedHundredthsFloat(curr_data, IMURegisters.NAVX_REG_FUSED_HEADING_L-first_address)
ahrspos_update.quaternionW = AHRSProtocol.decodeBinaryInt16(curr_data, IMURegisters.NAVX_REG_QUAT_W_L-first_address)/ 32768.0
ahrspos_update.quaternionX = AHRSProtocol.decodeBinaryInt16(curr_data, IMURegisters.NAVX_REG_QUAT_X_L-first_address)/ 32768.0
ahrspos_update.quaternionY = AHRSProtocol.decodeBinaryInt16(curr_data, IMURegisters.NAVX_REG_QUAT_Y_L-first_address)/ 32768.0
ahrspos_update.quaternionZ = AHRSProtocol.decodeBinaryInt16(curr_data, IMURegisters.NAVX_REG_QUAT_Z_L-first_address)/ 32768.0
if displacement_registers:
ahrspos_update.vel_x = AHRSProtocol.decodeProtocol1616Float(curr_data, IMURegisters.NAVX_REG_VEL_X_I_L-first_address)
ahrspos_update.vel_y = AHRSProtocol.decodeProtocol1616Float(curr_data, IMURegisters.NAVX_REG_VEL_Y_I_L-first_address)
ahrspos_update.vel_z = AHRSProtocol.decodeProtocol1616Float(curr_data, IMURegisters.NAVX_REG_VEL_Z_I_L-first_address)
ahrspos_update.disp_x = AHRSProtocol.decodeProtocol1616Float(curr_data, IMURegisters.NAVX_REG_DISP_X_I_L-first_address)
ahrspos_update.disp_y = AHRSProtocol.decodeProtocol1616Float(curr_data, IMURegisters.NAVX_REG_DISP_Y_I_L-first_address)
ahrspos_update.disp_z = AHRSProtocol.decodeProtocol1616Float(curr_data, IMURegisters.NAVX_REG_DISP_Z_I_L-first_address)
self.notify_sink._setAHRSPosData(ahrspos_update, sensor_timestamp)
else:
self.notify_sink._setAHRSData(ahrspos_update, sensor_timestamp)
board_state = self.board_state
board_state.cal_status = curr_data[IMURegisters.NAVX_REG_CAL_STATUS-first_address]
board_state.op_status = curr_data[IMURegisters.NAVX_REG_OP_STATUS-first_address]
board_state.selftest_status = curr_data[IMURegisters.NAVX_REG_SELFTEST_STATUS-first_address]
board_state.sensor_status = AHRSProtocol.decodeBinaryUint16(curr_data,IMURegisters.NAVX_REG_SENSOR_STATUS_L-first_address)
board_state.update_rate_hz = curr_data[IMURegisters.NAVX_REG_UPDATE_RATE_HZ-first_address]
board_state.gyro_fsr_dps = AHRSProtocol.decodeBinaryUint16(curr_data,IMURegisters.NAVX_REG_GYRO_FSR_DPS_L)
board_state.accel_fsr_g = curr_data[IMURegisters.NAVX_REG_ACCEL_FSR_G]
board_state.capability_flags= AHRSProtocol.decodeBinaryUint16(curr_data,IMURegisters.NAVX_REG_CAPABILITY_FLAGS_L-first_address)
self.notify_sink._setBoardState(board_state)
raw_data_update = self.raw_data_update
raw_data_update.raw_gyro_x = AHRSProtocol.decodeBinaryInt16(curr_data, IMURegisters.NAVX_REG_GYRO_X_L-first_address)
raw_data_update.raw_gyro_y = AHRSProtocol.decodeBinaryInt16(curr_data, IMURegisters.NAVX_REG_GYRO_Y_L-first_address)
raw_data_update.raw_gyro_z = AHRSProtocol.decodeBinaryInt16(curr_data, IMURegisters.NAVX_REG_GYRO_Z_L-first_address)
raw_data_update.raw_accel_x = AHRSProtocol.decodeBinaryInt16(curr_data, IMURegisters.NAVX_REG_ACC_X_L-first_address)
raw_data_update.raw_accel_y = AHRSProtocol.decodeBinaryInt16(curr_data, IMURegisters.NAVX_REG_ACC_Y_L-first_address)
raw_data_update.raw_accel_z = AHRSProtocol.decodeBinaryInt16(curr_data, IMURegisters.NAVX_REG_ACC_Z_L-first_address)
raw_data_update.cal_mag_x = AHRSProtocol.decodeBinaryInt16(curr_data, IMURegisters.NAVX_REG_MAG_X_L-first_address)
raw_data_update.cal_mag_y = AHRSProtocol.decodeBinaryInt16(curr_data, IMURegisters.NAVX_REG_MAG_Y_L-first_address)
raw_data_update.cal_mag_z = AHRSProtocol.decodeBinaryInt16(curr_data, IMURegisters.NAVX_REG_MAG_Z_L-first_address)
raw_data_update.mpu_temp_c = ahrspos_update.mpu_temp
self.notify_sink._setRawData(raw_data_update, sensor_timestamp)
self.last_update_time = Timer.getFPGATimestamp()
self.byte_count += len(curr_data)
self.update_count += 1
