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