From 8d087ceb8098f6967a0bb53953736aabffc485da Mon Sep 17 00:00:00 2001
From: Iori Yanokura <ab.ioryz@gmail.com>
Date: Sun, 31 Dec 2023 02:04:30 +0900
Subject: [PATCH] Add tests for real robot

---
 tests/rcb4_tests/__init__.py            |  0
 tests/rcb4_tests/test_armh7interface.py | 32 +++++++++++++++++++++++++
 2 files changed, 32 insertions(+)
 create mode 100644 tests/rcb4_tests/__init__.py
 create mode 100644 tests/rcb4_tests/test_armh7interface.py

diff --git a/tests/rcb4_tests/__init__.py b/tests/rcb4_tests/__init__.py
new file mode 100644
index 00000000..e69de29b
diff --git a/tests/rcb4_tests/test_armh7interface.py b/tests/rcb4_tests/test_armh7interface.py
new file mode 100644
index 00000000..afc932a5
--- /dev/null
+++ b/tests/rcb4_tests/test_armh7interface.py
@@ -0,0 +1,32 @@
+import unittest
+import time
+
+import numpy as np
+
+from rcb4.armh7interface import ARMH7Interface
+
+
+class TestRobotModel(unittest.TestCase):
+
+    interface = None
+
+    @classmethod
+    def setUpClass(cls):
+        cls.interface = ARMH7Interface()
+        cls.interface.auto_open()
+        cls.interface.search_servo_ids()
+
+    def test_servo_angle_vector(self):
+        self.interface.hold()
+        self.interface.neutral()
+        self.interface.servo_angle_vector(
+            [32, 34],
+            [8000, 8000],
+            velocity=0)
+        time.sleep(4.0)
+        current_av = self.interface.angle_vector()
+        if np.any(np.abs(self.interface.angle_vector() - 16) > 2.0):
+            self.assertRaises()
+        self.interface.neutral()
+        time.sleep(4.0)
+        self.interface.free()