File content as of revision 136:906ac19fb850:

/**
 * 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

bool init2 = true; // general init state
int i; // general counter
int grabbing_state = 2;


int grabbing()
{
    //printf("grabbing state: %d\r\n", grabbing_state);
    switch (grabbing_state) {
        case 2:
            if(arm_position_grabbing()) {
                grabbing_state = 3;
            }
            break;
        case 3:
            move_for_distance(0.15f);
            grabbing_state = 4;
            break;
        case 4:
            if(move_for_distance(0) < 0.075f) {
                close_grabber_half();
            }
            if(move_for_distance(0) < 0) {
                grabbing_state = 5;
            }
            break;
        case 5:
            close_grabber();
            wait(0.5f);
            grabbing_state = 6;
            break;
        case 6:
            if (get_color() == 0) {
                grabbing_state = 9;
            } else {
                grabbing_state = 7;
            }
            break;
        case 7:
            if(arm_position_release()) {
                wait(0.5f);
                open_grabber();
                grabbing_state = 10;
            }
            break;
        case 9:
            open_grabber();
            wait(0.05f);
            if(arm_position_release()) {
                grabbing_state = 10;
            }
            break;
        case 10:
            move_for_distance(-0.10f);
            grabbing_state = 11;
            break;
        case 11:
            if(move_for_distance(0) < 0) {
                grabbing_state = 2;
                return 0;
            }
            break;
    }
    return 50;
}

bool arm_position_grabbing()
{
    if(init2 == true) {
        init2 = false;
        i = servo4max;
    } else {

        if(i > servo4min) {
            i -=8;
            set_servo_position(4, i);
        } else {
            init2 = true;
            return 1;  // done
        }
        if (i < 470) {
            set_servo_position(6, servo6max);
        }
    }
    return 0; // recall
}

bool arm_position_release()
{
    if( init2 == true) {
        set_servo_position(4, servo4max);
        wait_ms(300);
        i = servo6max;
        init2 = false;
    } else {
        if(i>=servo6min) {
            i -=20;
            set_servo_position(6, i);
        } else {
            init2 = true;
            return 1; //open grabber
        }
    }
    return 0;  //recall
}

void open_grabber()
{
    set_servo_position(8, servo8min);
}

void close_grabber_half()
{
    set_servo_position(8, ((servo8min+servo8max)/2));
}

void close_grabber()
{
    set_servo_position(8, servo8max);

}