async def _setup(self):
# subscribe to param_response messages
await self.bus.subscribe(talon_srx.PARAM_RESPONSE | self.device_id,
self.param_response_buffer,
extended=True)
# subscribe to Status 3 messages
await self.bus.subscribe(talon_srx.STATUS_3 | self.device_id,
self.status_3_buffer,
extended=True)
# for externalenable being false, control_1 is needed
# Construct Control 1 message
self.msg_control_1 = aiocan.Message(arb_id=talon_srx.CONTROL_1
| self.device_id,
extended_id=True,
data=[0 for i in range(0, 2)])
# Send Control 1 periodically
self.control_1_task = await self.bus.send_periodic(
self.msg_control_1, 0.05)
# Construct Control 5 message
self.msg_control_5 = aiocan.Message(arb_id=talon_srx.CONTROL_5
| self.device_id,
extended_id=True,
data=[0 for i in range(0, 8)])
# Send Control 5 periodically
self.control_5_task = await self.bus.send_periodic(
self.msg_control_5, 0.01)
await self.set_override_limit_switches(
talon_srx.kLimitSwitchOverride.EnableFwd_EnableRev.value)
async def set_param(self, param_id, param_val):
frame = 0
if param_id in [
talon_srx.Param.ProfileParamSlot0_P.value,
talon_srx.Param.ProfileParamSlot0_I.value,
talon_srx.Param.ProfileParamSlot0_D.value,
talon_srx.Param.ProfileParamSlot1_P.value,
talon_srx.Param.ProfileParamSlot1_I.value,
talon_srx.Param.ProfileParamSlot1_D.value
]:
frame = talon_srx.float_to_fxp_10_22(param_val) & 0xFFFFFFFF
elif param_id in [
talon_srx.Param.ProfileParamSlot0_F.value,
talon_srx.Param.ProfileParamSlot1_F.value
]:
param_val = min(max(param_val, -255), 255)
else:
frame = int(param_val)
frame <<= 8
frame |= param_id
arbId = talon_srx.PARAM_SET | self.device_id
msg = aiocan.Message(arb_id=arbId,
extended_id=True,
data=struct.pack('<Q', frame)[:5])
await self.bus.send(msg)
while True:
msg = await self.param_response_buffer.get(timeout=None)
if msg.data[0] == param_id:
return
async def status_3(bus, device_id):
status_3_msg = aiocan.Message(arb_id=(talon_srx.STATUS_3 | device_id),
extended_id=True,
data=make_status_3_msg())
status_3_task = await bus.send_periodic(status_3_msg, 0.1)
while True:
status_3_msg.data = make_status_3_msg()
await status_3_task.modify(status_3_msg)
await asyncio.sleep(0.05)
def process_param_set_msg(bus, msg, device_id):
param = talon_srx.Param(msg.data[0])
raw_bits = struct.unpack('<I', msg.data[1:5])[0]
val = raw_bits
if param in (talon_srx.Param.ProfileParamSlot0_P,
talon_srx.Param.ProfileParamSlot0_I,
talon_srx.Param.ProfileParamSlot0_D,
talon_srx.Param.ProfileParamSlot1_P,
talon_srx.Param.ProfileParamSlot1_I,
talon_srx.Param.ProfileParamSlot1_D):
val = talon_srx.fxp_10_22_to_float(raw_bits)
print('setting parameter {} to {}'.format(param.name, val))
msg = aiocan.Message(arb_id=(talon_srx.PARAM_RESPONSE | device_id),
extended_id=True,
data=msg.data)
exec_later(bus.send(msg))