From e5e9354a51ae489ec80a0003c6d42ea2f1c1f5dc Mon Sep 17 00:00:00 2001
From: gestures5 <moscowskyad@yandex.ru>
Date: Fri, 10 Jul 2026 14:36:49 +0300
Subject: [PATCH] fix angle

---
 gesture_control/arm_control.py | 49 +++++++++++++++++++++-------------
 1 file changed, 31 insertions(+), 18 deletions(-)

diff --git a/gesture_control/arm_control.py b/gesture_control/arm_control.py
index 8a4cd21..e2f4b52 100644
--- a/gesture_control/arm_control.py
+++ b/gesture_control/arm_control.py
@@ -144,39 +144,51 @@ class ArmController:
         #right_hand_y = landmarks[16][1]
         #right_distance = right_shoulder_y - right_hand_y
 
-        angle_elbow_wirst_right = self._get_angle(landmarks[14], landmarks[16])
-        angle_elbow_wirst_left = self._get_angle(landmarks[13], landmarks[15])
-
+        angle_elbow_wirst_left = self._get_angle(landmarks[14], landmarks[16])
+        angle_elbow_wirst_right = self._get_angle(landmarks[13], landmarks[15])
         linear, angular = 0, 0
 
-        if np.deg2rad(-5) < angle_elbow_wirst_left < np.deg2rad(5):
+        if np.deg2rad(-5) < angle_elbow_wirst_right < np.deg2rad(5):
             linear = 0
 
-        elif np.deg2rad(-5) > angle_elbow_wirst_left:
-            linear = min(1, angle_elbow_wirst_left / -np.deg2rad(90))
+        elif np.deg2rad(-5) > angle_elbow_wirst_right:
+            linear = min(1, angle_elbow_wirst_right / -np.deg2rad(90))
 
-        elif np.deg2rad(5) < angle_elbow_wirst_left:
-            linear = min(1, angle_elbow_wirst_left / -np.deg2rad(90))
+        elif np.deg2rad(5) < angle_elbow_wirst_right:
+            linear = min(1, angle_elbow_wirst_right / -np.deg2rad(90))
 
         #print(f"Test: {linear} {angle_elbow_wirst_left}")
 
-        error = 35
+        error = 15
 
-        if np.deg2rad(180 - error) < angle_elbow_wirst_right < np.deg2rad(180 + error):
-            angular = 0
+        #print(f"Test: {angle_elbow_wirst_right} {np.rad2deg(angle_elbow_wirst_right)}")
+
+        #if np.deg2rad(180 - error) < angle_elbow_wirst_left < np.deg2rad(180 + error):
 
-        elif np.deg2rad(180 - error) > angle_elbow_wirst_right:
-            angular = min(1, angle_elbow_wirst_right / -np.deg2rad(90))
+        max_angle = np.deg2rad(180 - error)
+        min_angle = np.deg2rad(-180 + error)
 
-        elif np.deg2rad(180 + error) < angle_elbow_wirst_right:
-            angular = min(1, angle_elbow_wirst_right / -np.deg2rad(90))
+        if angle_elbow_wirst_left < 0:
 
-        if -np.deg2rad(error) < angle_elbow_wirst_right < np.deg2rad(error):
+            if angle_elbow_wirst_left < min_angle:# or angle_elbow_wirst_left > max_angle:
+                angular = 0
+            elif angle_elbow_wirst_left > min_angle:
+                #angular = min(1, angle_elbow_wirst_left / min_angle)
+                angular = -(min_angle - angle_elbow_wirst_left) / (min_angle - np.deg2rad(-90))
+
+        else:
+            if angle_elbow_wirst_left > max_angle:
+                angular = 0
+            elif angle_elbow_wirst_left < max_angle:
+                #angular = -min(1, angle_elbow_wirst_left / max_angle)
+                angular = (max_angle - angle_elbow_wirst_left) / (max_angle - np.deg2rad(90))
+
+        error = 35
+        if -np.deg2rad(error) < angle_elbow_wirst_left < np.deg2rad(error):
             linear = 0
             angular = 0
 
-        angular = angular / 2
-        #print(f"Test: {angular} {angle_elbow_wirst_right}")
+        #angular = angular / 2
 
 
         #left_shoulder_y = landmarks[11][1]
@@ -195,3 +207,4 @@ class ArmController:
         #angular = norm_ang * self.config['max_speed_linear']
 
         return linear, angular
+        #return angular, linear