Files
a7-can-sdk/tests/test_sdk_inherited.py

118 lines
5.9 KiB
Python

import math
from pathlib import Path
import struct
import sys
import time
import unittest
sys.path.insert(0,str(Path(__file__).resolve().parents[1]))
from a7_can_sdk import MotorBus, NotCommissionedError
from a7_can_sdk.fake import FakeCan
from a7_can_sdk.protocol import Frame, PARAMETERS, command, decode_state, read_parameter
class SDKTests(unittest.TestCase):
def setUp(self):
self.motors=[dict(motor_id=i,host_id=253,uid=f'{i:016x}',raw_limits=[-2.,2.]) for i in (61,62)]
self.peer=FakeCan(self.motors,[.1,.2]);self.bus=MotorBus(self.peer,self.motors,allow_control=True).start()
time.sleep(.03)
def tearDown(self):self.bus.close()
def test_extended_golden_vector_and_state(self):
f=read_parameter(67,PARAMETERS['position'])
self.assertEqual(f.can_id,0x1100fd43);self.assertEqual(f.data.hex(),'1970000000000000')
s=decode_state(Frame(0x028243fd,struct.pack('>4H',32768,13,24,335)),123.)
self.assertTrue(s.enabled);self.assertEqual(s.faults,['phase_overcurrent'])
self.assertEqual(s.temperature_c,33.5);self.assertEqual(s.raw_velocity,13)
self.assertIsNone(s.velocity_rad_s);self.assertIsNone(s.torque_nm)
def test_read_only_and_force_no_commands(self):
self.bus.allow_control=False
self.assertEqual(self.bus.identify(61),self.motors[0]['uid'])
self.assertAlmostEqual(self.bus.read_parameter(61,'position'),.1)
count=len(self.peer.frames)
for fn in (self.bus.enable,self.bus.disable,lambda:self.bus.set_positions({61:.2}),lambda:self.bus.write_parameter(61,'position_target',.2)):
with self.assertRaises(PermissionError):fn()
with self.assertRaises(NotCommissionedError):self.bus.set_torque(61,1.)
with self.assertRaises(NotCommissionedError):self.bus.set_impedance(61,0.,0.,1.,1.)
self.assertEqual(len(self.peer.frames),count)
def test_typed_queries_missing_and_stale_optional_values(self):
self.assertEqual(self.bus.read_parameter(61,'control_mode'),5)
self.assertEqual(self.bus.read_parameter(61,'bus_voltage'),48.)
self.assertEqual(self.bus.get_state(61).bus_voltage_v,48.)
self.peer.unsupported.add((61,0x302c))
with self.assertRaises(NotImplementedError):self.bus.read_parameter(61,'torque')
time.sleep(.61)
self.assertIsNone(self.bus.get_state(61).bus_voltage_v)
def test_enable_preload_move_stop(self):
self.bus.enable();self.assertEqual(self.peer.enabled,{61,62})
kinds=[f.can_id>>24 for f in self.peer.frames]
self.assertGreater(kinds.index(3),kinds.index(18))
self.bus.set_positions({61:.102,62:.202});time.sleep(.15)
self.assertAlmostEqual(self.bus.get_state(61).position_rad,.102,delta=.0004)
self.assertTrue(self.bus.disable());self.assertFalse(self.peer.enabled)
def test_velocity_telemetry_does_not_block_enable(self):
original = self.bus.read_parameter
def read(mid, name, *args, **kwargs):
return .1 if name == 'velocity' else original(mid, name, *args, **kwargs)
self.bus.read_parameter = read
self.bus.enable()
self.assertEqual(self.peer.enabled, {61, 62})
for mid, q in self.peer.positions.items():
self.assertAlmostEqual(self.peer.targets[mid], q, places=6)
def test_explicit_speed_setting_replaces_old_lower_cap(self):
self.bus.enable(speed_limit=math.radians(4), apply_configured_speed_limit=True)
for mid in (61,62):
self.assertAlmostEqual(self.bus.read_parameter(mid,'speed_limit'),math.radians(4),places=7)
self.assertAlmostEqual(self.bus.effective_speed_limits[mid],math.radians(4),places=7)
def test_higher_drive_speed_is_written_and_read_back(self):
self.bus.enable(speed_limit=math.radians(10), apply_configured_speed_limit=True)
for mid in (61,62):
self.assertAlmostEqual(self.bus.read_parameter(mid,'speed_limit'),math.radians(10),places=7)
count=len(self.peer.frames)
for invalid in [math.radians(21), float('nan'), float('inf')]:
with self.assertRaises(ValueError):
self.bus.write_parameter(61,'speed_limit',invalid)
self.assertEqual(len(self.peer.frames),count)
def test_wrong_uid_prevents_enable(self):
self.peer.motors[61]=dict(self.motors[0],uid='ff'*8)
with self.assertRaises(ValueError):self.bus.enable()
self.assertNotIn(3,[f.can_id>>24 for f in self.peer.frames])
def test_partial_enable_failure_stops_every_motor(self):
self.peer.ignore_enable.add(62)
with self.assertRaises(ValueError):self.bus.enable()
self.assertFalse(self.peer.enabled)
self.assertEqual({f.can_id&255 for f in self.peer.frames if f.can_id>>24==4},{61,62})
def test_stale_and_fault_stop(self):
self.bus.enable();self.peer.drop_feedback.add(61)
time.sleep(.65)
self.assertFalse(self.peer.enabled);self.assertIn('stale',self.bus.fault)
def test_fault_report_and_guard(self):
self.bus.enable()
self.peer.q.put(Frame((21<<24)|(61<<8)|253,struct.pack('<II',4,8)))
time.sleep(.4)
self.assertFalse(self.peer.enabled)
s=self.bus.get_state(61);self.assertEqual(s.fault_report,4);self.assertEqual(s.warning_report,8)
def test_all_targets_validated_before_any_send(self):
self.bus.enable();count=len(self.peer.frames)
with self.assertRaises(ValueError):self.bus.set_positions({61:.2,62:float('nan')})
self.assertEqual(len(self.peer.frames),count)
with self.assertRaises(ValueError):self.bus.write_parameter(61,'current_limit',99.)
def test_stop_ack_failure_is_not_success(self):
self.bus.enable();self.peer.ignore_stop.add(61)
self.assertFalse(self.bus.disable());self.assertIn(61,self.peer.enabled)
self.peer.ignore_stop.clear();self.assertTrue(self.bus.disable())
if __name__=='__main__':unittest.main()