File content as of revision 65:1f237921216d:

/**
 * Grabbing function library
 * Handels grabbing of the LEGO-stones
**/

#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;
    }
    
int arm_position_move(){
    //void (Mittelstellung für Fahrt)
    return 0;
    }
    
int arm_position_grabbing(){
    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;
}