Diff: source/Grabbing.cpp

Revision:

66:87b615e4a842

Parent:

60:b57577b0072f

Parent:

65:1f237921216d

Child:

69:1fdcef6a7577

--- a/source/Grabbing.cpp	Tue Apr 18 16:19:18 2017 +0000
+++ b/source/Grabbing.cpp	Wed Apr 19 08:59:53 2017 +0000
@@ -5,6 +5,15 @@
 
 #include "Grabbing.h"
 
+#define servo4min   280 //unten
+#define servo4max   495 //oben
+#define servo6min   270 //oben
+#define servo6max   480 //unter
+#define servo8min   120 //offen
+#define servo8max   260 //zu
+//Servo 4 Ausleger  Servo 6 Gelenk  Servo 8 Greifer
+
+
 int grabbing(){
     //void
     return 0;
@@ -16,24 +25,28 @@
     }
     
 int arm_position_grabbing(){
-    void set_servo_position(int servo, int deg);
+    for(int i = servo4max; i > servo4min; i = i-5) {
+        set_servo_position(4, i);
+        if (i == 450) {
+            set_servo_position(6, servo6max);
+        }
+    }
     return 0;
-    }
+}
     
 int arm_position_release(){
-    
+    set_servo_position(4, servo4max);
+    wait_ms(400);
+    set_servo_position(6, servo6min);
     return 0;
-    }
+   }
     
 int open_grabber(){
-    
+    set_servo_position(8, servo8min);
     return 0;
-    }
+}
     
 int close_grabber(){
-    
+    set_servo_position(8, servo8max);
     return 0;
-    }
-    
-
-    
\ No newline at end of file
+}