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 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
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()
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")
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 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)
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()
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 __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
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 poll(self):
rtn_val = self.get()
Timer.delay(.015)
return rtn_val
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
def poll(self):
rtn_val = self.get()
Timer.delay(.015)
return rtn_val
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
