118 lines
5.9 KiB
Python
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()
|