dominika version

gesture_control/arm_control.py

@@ -14,6 +14,9 @@ class ArmController:
         self.r_elbow_id = 13 #14
         self.r_shoulder_id = 11 # 12
 
+        self.error = 23
+        self.bag = 27
+
         self.debug = config.get('debug', False)
 
 
@@ -44,12 +47,27 @@ class ArmController:
 
         angle_shoulder_elbow = self._get_angle(right_shoulder_x, right_shoulder_y, right_elbow_x, right_elbow_y)
 
-        five_deg_in_rad = np.deg2rad(5)
+        right_wirst_point = landmarks[self.r_wirst_id]
+        right_wirst_x = right_wirst_point[0]
+        right_wirst_y = right_wirst_point[1]
+
+        angle_elbow_wirst = self._get_angle(right_elbow_x, right_elbow_y, right_wirst_x, right_wirst_y)
 
-        print(f"Angle between shoulder and elbow is {angle_shoulder_elbow}")
+        five_deg_in_rad = np.deg2rad(self.error)
 
-        if (angle_shoulder_elbow < five_deg_in_rad) and (angle_shoulder_elbow > -five_deg_in_rad):
+
+        if (angle_shoulder_elbow < np.pi/2.5 + five_deg_in_rad) and (angle_shoulder_elbow > np.pi/2.5 -five_deg_in_rad):
+            print(f"Angle between shoulder and elbow is {angle_shoulder_elbow} {np.rad2deg(angle_shoulder_elbow)}")
+            #print(f"Angle between elbow and wirst is {angle_elbow_wirst}")
+            if (angle_elbow_wirst > np.deg2rad(-90 - self.error)) and (angle_elbow_wirst < np.deg2rad(-90 + self.error)):
                 linear = 1.
+            if (angle_elbow_wirst > np.deg2rad(90 - self.error)) and (angle_elbow_wirst < np.deg2rad(90 + self.error)):
+                linear = -1.
+            if (angle_elbow_wirst > np.deg2rad(-45 - self.bag)) and (angle_elbow_wirst < np.deg2rad(-45 + self.bag)):
+                angular = -1.
+            if (angle_elbow_wirst > np.deg2rad(-135 - self.bag)) and (angle_elbow_wirst < np.deg2rad(-135 + self.bag)):
+                angular = 1.
 
         return linear, angular
 
+