last dimas

gesture_control/arm_control.py

@@ -93,6 +93,7 @@ class ArmController:
         disp = shoulder[1] - wrist[1]
         return disp / torso_height
 
+    '''
     def compute_speeds(self, landmarks):
         if (landmarks[11][3] < 0.5 or landmarks[12][3] < 0.5 or
             landmarks[15][3] < 0.5 or landmarks[16][3] < 0.5):
@@ -123,4 +124,25 @@ class ArmController:
             angular = ang_rel_clipped * self.config['max_speed_angular']
 
         return linear, angular
+    '''
+    def compute_speeds(self, w, h, landmarks):
+        linear = 0
+        angular = 0
+        if 3*w/5>landmarks[19][0]>2*w/5:
+            angular = 0
+        if 3*w/5<landmarks[19][0]:
+            angular = (landmarks[19][0]*5)/(2*w)-1
+        if landmarks[15][0]<2*w/5:
+            angular = (landmarks[19][0]-3*w/5)/(2*w/5)
+        if 3*h/5>landmarks[19][1]>2*h/5:
+            linear = 0
+        if 3*h/5<landmarks[19][1]:
+            linear = -((landmarks[19][1]*5)/(2*h)-1)
+        if landmarks[19][1]<2*h/5:
+            linear = -(landmarks[19][1]-3*h/5)/(2*h/5)
+
+
+
+        print(f"{linear},{angular}")
+        return linear, angular