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
Exemple #2
0
 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 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
class Shoot(Command):
    def __init__(self):
        super().__init__()
        self.timer = Timer()
        self.phase = 0

    def on_start(self):
        self.phase = 0
        RobotMap.shooter_component.set_active()
        RobotMap.shooter_component.disable_intake()
        RobotMap.shooter_component.engage_launcher()
        self.timer.reset()
        self.timer.start()

    def execute(self):
        if self.phase == 0:
            if self.timer.hasPeriodPassed(0.5):
                print("first phase")
                RobotMap.shooter_component.retract_launcher()
                RobotMap.shooter_component.enable_intake()
                self.phase = 1
        elif self.phase == 1:
            if self.timer.hasPeriodPassed(1.0):
                print("second phase")
                RobotMap.shooter_component.disable_intake()
                self.finished()
                return

    def on_end(self):
        self.timer.stop()
        self.timer.reset()
        RobotMap.shooter_component.set_inactive()
 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
Exemple #7
0
    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 __init__(self):
        super().__init__("Respond to Controller")

        self.sd = NetworkTables.getTable("SmartDashboard")
        self.logger = logging.getLogger("robot")

        oi.start_btn.whenPressed(SwitchCamera())

        self.timer = Timer()

        self.right_bumper_last = False
        self.left_bumper_last = False
 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")
class SuckFast(Command):
    def __init__(self):
        super().__init__()
        self.timer = Timer()

    def on_start(self):
        self.timer.start()

    def execute(self):
        RobotMap.gripper_component.set_motor_speeds(1, 1)
        if self.timer.hasPeriodPassed(0.4):
            RobotMap.gripper_component.set_motor_speeds(0, 0)
            self.finished()

    def on_end(self):
        pass
    def __init__(self, inches: float, speed: float, timeout=0.0):
        super().__init__()
        angular_gains = (0.02, 0.0001, 0.02, 0.0)
        self._target_distance = inches
        self._speed = speed
        self._angular = 0

        self.timer = Timer()

        self.angular_controller = PIDController(
            *angular_gains, RobotMap.driver_component.driver_gyro, output=self)
        self.angular_controller.setInputRange(-360, 360)
        self.angular_controller.setOutputRange(-1, 1)
        self.angular_controller.setAbsoluteTolerance(0.5)

        self.angular_controller.setSetpoint(0)
        self._timeout = timeout
class DriveByDistance(Command):
    def __init__(self, inches: float, speed: float, timeout: float = 0):
        super().__init__()
        angular_gains = (0.02, 0.0001, 0.02, 0.0)
        linear_gains = (0.02, 0.0, 0.0, 0.0)
        self._target_distance = inches
        self._speed = speed
        self._angular = 0

        self.angular_controller = PIDController(
            *angular_gains, RobotMap.driver_component.driver_gyro, output=self)
        self.angular_controller.setInputRange(-360, 360)
        self.angular_controller.setOutputRange(-1, 1)
        self.angular_controller.setAbsoluteTolerance(0.5)

        self.angular_controller.setSetpoint(0)

        self._timeout = timeout
        self.timer = Timer()

    def pidWrite(self, output):
        self._angular = output

    def on_start(self):
        self.timer.start()

        RobotMap.driver_component.reset_drive_sensors()

        self.angular_controller.enable()

    def execute(self):
        if abs(RobotMap.driver_component.current_distance) >= abs(
                self._target_distance) or (self._timeout != 0
                                           and self.timer.hasPeriodPassed(
                                               self._timeout)):
            RobotMap.driver_component.drive_train.curvatureDrive(0, 0, False)
            RobotMap.driver_component.neutralMotors()
            self.finished()
            return

        RobotMap.driver_component.drive_train.curvatureDrive(
            self._speed, self._angular, False)

    def on_end(self):
        self.timer.stop()
        self.angular_controller.disable()
Exemple #13
0
    def new_swerve(self):
        throttle = -config.controller.getRawAxis(XboxAxis.R_Y)
        rot = config.controller.getRawAxis(XboxAxis.L_X)

        # deadband zone
        if throttle < 0.25 and throttle > -0.25:
            throttle = 0
        if rot < 0.25 and rot > -0.25:
            rot = 0

        logging.write_log([self.target, rot])

        self.target += rot * 10

        inverse = False
        stop = False

        t = Timer()
        t.start()
        for m in range(len(config.steering_motors)):
            t.reset()
            logging.write_log("Turning")
            while (not config.encoders[m].get() < self.target + 15 or \
                   not config.encoders[m].get() > self.target - 15) and \
                not stop:
                if not inverse:
                    config.steering_motors[m].set(
                        (1 * -copysign(1, rot)) *
                        (self.target - config.encoders[m].get()))

                else:
                    config.steering_motors[m].set(
                        (1 * copysign(1, rot)) *
                        (config.encoders[m].get() - self.target))
                logging.write_log(config.encoders[m].get())
                if t.hasPeriodPassed(1.5):
                    inverse = True
                    stop = True
                    break
            config.steering_motors[m].set(0)

        t.stop()

        for i in range(len(config.driving_motors)):
            if (i % 2):
                config.driving_motors[i].set(throttle)
            else:
                config.driving_motors[i].set(-throttle)
 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")
Exemple #16
0
    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")
Exemple #17
0
    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
Exemple #18
0
    def new_swerve(self):
        throttle = -config.controller.getRawAxis(XboxAxis.R_Y)
        rot = config.controller.getRawAxis(XboxAxis.L_X)

        # deadband zone
        if throttle < 0.25 and throttle > -0.25:
            throttle = 0
        if rot < 0.25 and rot > -0.25:
            rot = 0

        logging.write_log([self.target, rot])
            
        self.target += rot * 10

        inverse = False
        stop = False
        
        t = Timer()
        t.start()
        for m in range(len(config.steering_motors)):
            t.reset()
            logging.write_log("Turning")
            while (not config.encoders[m].get() < self.target + 15 or \
                   not config.encoders[m].get() > self.target - 15) and \
                not stop:
                if not inverse:
                    config.steering_motors[m].set((1 * -copysign(1, rot)) *
                                                  (self.target - config.encoders[m].get()))
                                                  
                else:
                    config.steering_motors[m].set((1 * copysign(1, rot)) *
                                                  (config.encoders[m].get() - self.target))
                logging.write_log(config.encoders[m].get())
                if t.hasPeriodPassed(1.5):
                    inverse = True
                    stop = True
                    break
            config.steering_motors[m].set(0)

        t.stop()
    
        for i in range(len(config.driving_motors)):
            if (i % 2):
                config.driving_motors[i].set(throttle)
            else:
                config.driving_motors[i].set(-throttle)
Exemple #19
0
class Shoot(Command):
    def __init__(self):
        super.__init__()
        self.timer = Timer()

    def on_start(self):
        RobotMap.shooter_component.disable_intake()
        RobotMap.shooter_component.engage_launcher()
        self.timer.start()

    def execute(self):
        if self.timer.hasPeriodPassed(1.5):
            RobotMap.shooter_component.enable_intake()
            self.finished()
        elif self.timer.hasPeriodPassed(1.0):
            RobotMap.shooter_component.retract_launcher()

    def on_end(self):
        self.timer.stop()
        self.timer.reset()
class SpitFast(Command):
    def __init__(self, speed=1):
        super().__init__()
        self.timer = Timer()
        self._speed = -speed

    def on_start(self):
        self.timer.start()
        print("started spit")

    def execute(self):
        RobotMap.gripper_component.set_motor_speeds(self._speed, self._speed)
        if self.timer.hasPeriodPassed(1):
            RobotMap.gripper_component.set_motor_speeds(0, 0)
            self.finished()

    def on_end(self):
        self.timer.stop()
        self.timer.reset()
        print("ended spit")
class DriveByTime(Command):
    def __init__(self, seconds: float, speed: float):
        super().__init__()
        self._target_seconds = seconds
        self._speed = speed
        self.timer = Timer()

    def on_start(self):
        print("start driving forward by time")
        self.timer.start()

    def execute(self):
        RobotMap.driver_component.set_curve(
            self._speed,
            -RobotMap.driver_component.driver_gyro.getAngle() * 0.2)
        if self.timer.hasPeriodPassed(self._target_seconds):
            RobotMap.driver_component.set_curve(0, 0)
            self.finished()

    def on_end(self):
        print("end driving forward by time")
        self.timer.stop()
        self.timer.reset()
Exemple #22
0
 def isConnected(self):
     time_since_last_update = Timer.getFPGATimestamp(
     ) - self.last_update_time
     return time_since_last_update <= IO_TIMEOUT_SECONDS
Exemple #23
0
    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 __init__(self, seconds: float, speed: float):
     super().__init__()
     self._target_seconds = seconds
     self._speed = speed
     self.timer = Timer()
class RespondToController(Command):
    """
    This command will respond to the controller's buttons.
    """

    def __init__(self):
        super().__init__("Respond to Controller")

        self.sd = NetworkTables.getTable("SmartDashboard")
        self.logger = logging.getLogger("robot")

        oi.start_btn.whenPressed(SwitchCamera())

        self.timer = Timer()

        self.right_bumper_last = False
        self.left_bumper_last = False

    def initialize(self):
        self.timer.start()

    def execute(self):
        if self.timer.hasPeriodPassed(0.05):
            # for xbox controller

            # piston control
            if oi.controller.getAButton():  # open
                ControlGearMech(False).start()
            elif oi.controller.getBButton():  # close
                ControlGearMech(True).start()

                # reversing control
                # if oi.controller.getStartButton():  # reverse camera, motors, and sonar
                # if self.sd.containsKey("camera/dev"):
                #     if self.sd.getNumber("camera/dev") == 1:
                #         self.sd.putNumber("camera/dev", 2)
                #     else:
                #         self.sd.putNumber("camera/dev", 1)
                # else:
                #     self.sd.putNumber("camera/dev", 1)
                # cur_direct = self.sd.getNumber("direction")
                # if cur_direct == 1:
                #     subsystems.motors.reverseDirection()
                # else:
                #     subsystems.motors.forwardDirection()
                # self.sd.putNumber("direction", -cur_direct)

            # slow mode
            if oi.controller.getBumper(GenericHID.Hand.kRight):
                if self.right_bumper_last:
                    pass
                elif oi.divider != 1:
                    self.sd.putBoolean("slowmode", False)
                else:
                    self.sd.putBoolean("slowmode", True)
                self.right_bumper_last = True
            else:
                self.right_bumper_last = False

            # lock on
            if oi.controller.getBumper(GenericHID.Hand.kLeft):
                if self.left_bumper_last:
                    pass
                elif not self.sd.getBoolean("lockonRunning"):
                    LockOn().start()
                else:
                    self.logger.critical("Returning control to the controller!")
                    self.sd.putBoolean("lockonRunning", False)
                    RumbleController(0.5).start()
                    FollowJoystick().start()
                self.left_bumper_last = True
            else:
                self.left_bumper_last = False
Exemple #26
0
    def new_new_swerve(self):
#        if thr is None:
        throttle = -config.controller.getRawAxis(XboxAxis.R_Y)
#        else:
#            throttle = thr

#        if rot is None:
        rot = config.controller.getRawAxis(XboxAxis.L_X)
#        else:
#            self.target = rot

        # deadband zone
        if throttle < 0.25 and throttle > -0.25:
            throttle = 0
        if rot < 0.25 and rot > -0.25:
            rot = 0

        self.enc_ += rot
        self.target += self.seek_to(self.enc_)

        if self.target > self.max:
            self.target = self.max
        elif self.target < self.min:
            self.target = self.min

        while_timer = Timer()
        for_timer = Timer()

        current = [self.seek_to(config.encoders[enc].get()) for enc in range(4)]
        
        motor_values = self.setMotorValues(current[0])

        while_timer.start()
        for_timer.start()
        while self.notInPosition(current, motor_values) and while_timer.hasPeriodPassed(.2):
            for enc in range(4):
                config.steering_motors[enc].set(0.2 * motor_values[enc])
            if for_timer.hasPeriodPassed(.03):
                for enc in range(4):
                    config.steering_motors[enc].set(0)
                for_timer.reset()
            current = [self.seek_to(config.encoders[enc].get()) for enc in range(4)]
        for_timer.stop()
        while_timer.stop()

        for mtr in config.driving_motors:
            mtr.set(throttle)
Exemple #27
0
 def poll(self):
     rtn_val = self.get()
     Timer.delay(.015)
     return rtn_val
Exemple #28
0
 def __init__(self):
     super.__init__()
     self.timer = Timer()
 def isConnected(self):
     time_since_last_update = Timer.getFPGATimestamp() - self.last_update_time
     return time_since_last_update <= IO_TIMEOUT_SECONDS
 def __init__(self, speed=1):
     super().__init__()
     self.timer = Timer()
     self._speed = -speed
Exemple #31
0
 def poll(self):
     rtn_val = self.get()
     Timer.delay(.015)
     return rtn_val
Exemple #32
0
    def new_new_swerve(self):
        #        if thr is None:
        throttle = -config.controller.getRawAxis(XboxAxis.R_Y)
        #        else:
        #            throttle = thr

        #        if rot is None:
        rot = config.controller.getRawAxis(XboxAxis.L_X)
        #        else:
        #            self.target = rot

        # deadband zone
        if throttle < 0.25 and throttle > -0.25:
            throttle = 0
        if rot < 0.25 and rot > -0.25:
            rot = 0

        self.enc_ += rot
        self.target += self.seek_to(self.enc_)

        if self.target > self.max:
            self.target = self.max
        elif self.target < self.min:
            self.target = self.min

        while_timer = Timer()
        for_timer = Timer()

        current = [
            self.seek_to(config.encoders[enc].get()) for enc in range(4)
        ]

        motor_values = self.setMotorValues(current[0])

        while_timer.start()
        for_timer.start()
        while self.notInPosition(
                current, motor_values) and while_timer.hasPeriodPassed(.2):
            for enc in range(4):
                config.steering_motors[enc].set(0.2 * motor_values[enc])
            if for_timer.hasPeriodPassed(.03):
                for enc in range(4):
                    config.steering_motors[enc].set(0)
                for_timer.reset()
            current = [
                self.seek_to(config.encoders[enc].get()) for enc in range(4)
            ]
        for_timer.stop()
        while_timer.stop()

        for mtr in config.driving_motors:
            mtr.set(throttle)
 def __init__(self):
     super().__init__()
     self.timer = Timer()
     self.phase = 0
 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