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 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