File content as of revision 84:435ec041fd0d:

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

#include "Grabbing.h"

#define servo4min   280 //unten
#define servo4max   495 //oben
#define servo6min   265 //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 == 450) {
            set_servo_position(6, servo6max);
        }
    }
    //move_forward_for_distance(float distance)
    return 56;  //close grabber
}
    
int arm_position_release(){
    set_servo_position(4, servo4max);
    wait_ms(50);
    for (int i = servo6max; i>=servo6min; i = i-5) {
        set_servo_position(6, i);
    }
    return 55;  //open grabber
   }
    
int open_grabber(){
    set_servo_position(8, servo8min);
    return 35;  //select target
}
    
int close_grabber(){
    set_servo_position(8, servo8max);
    return 53;  //release position -> get_color()...
}