Diff: source/Grabbing.cpp

Revision:

65:1f237921216d

Parent:

55:a1e6fb87d648

Child:

66:87b615e4a842

--- a/source/Grabbing.cpp	Tue Apr 18 15:10:39 2017 +0000
+++ b/source/Grabbing.cpp	Tue Apr 18 15:53:45 2017 +0000
@@ -5,6 +5,15 @@
 
 #include "Grabbing.h"
 
+#define servo4min   285 //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,28 +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 == 390) {
+            set_servo_position(6, servo6max);
+        }
+    }
     return 0;
-    }
+}
     
 int arm_position_release(){
-    
+    set_servo_position(4, servo4max);
+    wait_ms(300);
+    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;
-    }
-    
-bool get_color(){
-    //return true for green and false for red
-    if(
-    return 0;
-    }
-    
\ No newline at end of file
+}