Skip to content

Commit

Permalink
Add reference angle vector test
Browse files Browse the repository at this point in the history
  • Loading branch information
iory committed Jan 3, 2024
1 parent 19ff9ab commit 43ef75a
Show file tree
Hide file tree
Showing 2 changed files with 15 additions and 4 deletions.
12 changes: 9 additions & 3 deletions rcb4/armh7interface.py
Original file line number Diff line number Diff line change
Expand Up @@ -375,9 +375,14 @@ def read_jointbase_sensor_ids(self):
self.id_vector = list(sorted(ret))
return self.id_vector

def reference_angle_vector(self):
return self.read_cstruct_slot_vector(
ServoStruct, slot_name='ref_angle')
def reference_angle_vector(self, servo_ids=None):
if servo_ids is None:
servo_ids = self.search_servo_ids()
if len(servo_ids) == 0:
return np.empty(shape=0)
ref_angles = self.read_cstruct_slot_vector(
ServoStruct, slot_name='ref_angle')[servo_ids]
return ref_angles

def servo_id_to_index(self, servo_ids=None):
if servo_ids is None:
Expand Down Expand Up @@ -409,6 +414,7 @@ def search_servo_ids(self):
servo = self.memory_cstruct(ServoStruct, idx)
if servo.flag > 0:
indices.append(idx)
indices = np.array(indices)
self.servo_sorted_ids = indices
return indices

Expand Down
7 changes: 6 additions & 1 deletion tests/rcb4_tests/test_armh7interface.py
Original file line number Diff line number Diff line change
Expand Up @@ -2,6 +2,7 @@
import unittest

import numpy as np
from numpy import testing

from rcb4.armh7interface import ARMH7Interface

Expand All @@ -19,10 +20,14 @@ def setUpClass(cls):
def test_servo_angle_vector(self):
self.interface.hold()
self.interface.neutral()
reference = [8000, 8000]
self.interface.servo_angle_vector(
[32, 34],
[8000, 8000],
reference,
velocity=0)
testing.assert_array_almost_equal(
self.interface.reference_angle_vector(),
reference)
time.sleep(4.0)
if np.any(np.abs(self.interface.angle_vector() - 16) > 2.0):
self.assertRaises()
Expand Down

0 comments on commit 43ef75a

Please sign in to comment.