Initial Commit

tests/test_ugv_control.py

@@ -0,0 +1,41 @@
+#!/usr/bin/env python3
+"""Test UGV control logic."""
+
+import pytest
+import numpy as np
+
+
+def test_differential_drive():
+    target_angle = np.pi / 4
+    current_yaw = 0
+    
+    angle_error = target_angle - current_yaw
+    kp_angular = 1.5
+    max_angular = 1.0
+    
+    angular_cmd = np.clip(kp_angular * angle_error, -max_angular, max_angular)
+    assert np.isclose(angular_cmd, 1.0)
+
+
+def test_angle_normalization():
+    def normalize(angle):
+        while angle > np.pi:
+            angle -= 2 * np.pi
+        while angle < -np.pi:
+            angle += 2 * np.pi
+        return angle
+    
+    assert np.isclose(normalize(3 * np.pi), np.pi)
+    assert np.isclose(normalize(-3 * np.pi), np.pi)
+
+
+def test_goal_distance_2d():
+    current = np.array([0, 0])
+    target = np.array([3, 4])
+    
+    distance = np.linalg.norm(target - current)
+    assert np.isclose(distance, 5.0)
+
+
+if __name__ == '__main__':
+    pytest.main([__file__, '-v'])