File content as of revision 107:8e8a7a9b6fd8:

/**
 * Movement function library
 * Handels Movement of the Robot
**/

#include "Movement.h"
#define OFFSET_GREIFER_TO_IRSENSOR 0.15                 // Constant for distance between front IR Sensor and the postion where the Greifer is in grabbing Position
#define OFFSET_WHEELS 0.09                              // Offset of the wheels from the center pos

bool is_moving = false;
float wanted_dist = 0;
bool is_turning = false;
float wanted_deg = 0;
bool direction = false;

Timer t;

int search_state = 0;

float left = 0;
float right = 0;

bool devider = true;


int moving()
{

    return 0;
}

/**
 * Stops current movement immediately
**/
void stop_move()
{
    set_speed(0,0);
    wanted_dist = 0;
    is_moving = false;
}

/**
 * Stops current turn immediately
**/
void stop_turn()
{
    set_speed(0,0);
    wanted_deg = 0;
    is_turning = false;
}


/**
 * move for wanted distance on circle with a given radius
 * needs to be called until return < 0
 * if calling distance not 0: distance and radius initilisation.
 * by Claudio Citterio
**/
float move_for_distance_with_radius(float distance, float r)
{

    if(distance != 0) {

        is_moving = true;
        wanted_dist = fabsf(distance);

        float circumference = r*2*(float)M_PI;
        float circumference_inner = ((r-(float)OFFSET_WHEELS)*2*(float)M_PI);
        float circumference_outer = ((r+(float)OFFSET_WHEELS)*2*(float)M_PI);

        float center_speed = 50;
        float inner_speed = center_speed/circumference*circumference_inner;
        float outer_speed = center_speed/circumference*circumference_outer;

        //wanted_deg = 360 / (2*radius*(float)M_PI) * wanted_dist;
        //float time = (10*wanted_dist)/(wheel_r * center_speed);


        if(r > 0) {

        //turn right
        if(distance > 0) {  //move forward
                direction = 1;
                left = outer_speed;
                right = inner_speed;
            } else {            //move backward
                direction = 0;
                left = -outer_speed;
                right = -inner_speed;
            }
        } else if(r < 0) {

        // turn left
        if(distance > 0) {  //move forward
                direction = 1;
                left = inner_speed;
                right = outer_speed;
            } else {            //move backward
                direction = 0;
                left = -inner_speed;
                right = -outer_speed;
            }
        } else {
            //normal straight movement

            if(distance > 0) {  //move forward
                direction = 1;
                left = center_speed;
                right = center_speed;
            } else {            //move backward
                direction = 0;
                left = -center_speed;
                right = -center_speed;
            }
        }

        set_speed(left, right);
        devider = true;
        t.reset();
        t.start();
    } else {
        float speed_multiplier = 0.6f;
        if(wanted_dist < 0.10f && devider == true) {
            //printf("devided\r\n");
            devider = false;
            left = left * speed_multiplier;
            right = right * speed_multiplier;
            //printf("left: %f || right: %f\r\n", left, right);
            set_speed(left, right);
        }

        float speed_left = get_speed_left();
        float speed_right = get_speed_right();
        wanted_dist -= (2*(float)wheel_r*(float)M_PI)/(2*M_PI) * t.read() * ((fabsf(speed_left)+fabsf(speed_right))/2) * 0.1f;
        t.reset();

        if(wanted_dist <= 0) { //distance covered, Stop function
            set_speed(0,0);
            is_moving = false;
            t.stop();
        }
    }
    printf("remaining distance to cover: %f\r\n", wanted_dist);
    return wanted_dist;
}

/**
 * move for wanted distance
 * needs to be called until return < 0
 * if calling distance not 0: distance initilisation.
 * by Claudio Citterio
**/
float move_for_distance(float distance)
{
    printf("move for distance\r\n");
    if(distance != 0) {

        is_moving = true;

        wanted_dist = fabsf(distance);

        if(distance > 0) {  //move forward
            direction = 1;
            left = 50.0f;
            right = 50.0f;
        } else {            //move backward
            direction = 0;
            left = -50.0f;
            right = -50.0f;
        }
        printf("set speed %f\r\n", left);
        set_speed(left, right);
        devider = true;
        t.reset();
        t.start();

    } else {
        float speed_multiplier = 0.6f;
        if(wanted_dist < 0.10f && devider == true) {
            //printf("devided\r\n");
            devider = false;
            left = left * speed_multiplier;
            right = right * speed_multiplier;
            //printf("left: %f || right: %f\r\n", left, right);
            set_speed(left, right);
        }

        float speed_left = get_speed_left();
        printf("speed left: %f\r\n", speed_left);
        wanted_dist -= (2*(float)wheel_r*(float)M_PI)/(2*M_PI) * t.read() * fabsf(speed_left)*0.1f;
        t.reset();

        if(wanted_dist <= 0) { //distance covered, Stop function
            set_speed(0,0);
            is_moving = false;
            t.stop();
        }
    }
    printf("remaining distance to cover: %f\r\n", wanted_dist);
    return wanted_dist;
}

/**
 * turn for wanted degree
 * needs to be called until return < 0
 * if deg not 0: turn initilisation.
 * Claudio Citterio
**/
float turn_for_deg(float deg)   
{

    if(deg != 0) {

        is_turning = true;
        wanted_deg = fabsf(deg);

        if(deg < 0) { // turn left
            direction = 1;
            left = -30.0f;
            right = 30.0f;
        } else { // turn right
            direction = 0;
            left = 30.0f;
            right = -30.0f;
        }
        set_speed(left, right);
        devider = true;
        t.reset();
        t.start();

    } else {
        float speed_multiplier = 0.6f;
        if(wanted_deg < 10.0f && devider == true) {
            devider = false;
            left = left * speed_multiplier;
            right = right * speed_multiplier;
            set_speed(left, right);
        }

        float speed_left = get_speed_left();
        wanted_deg -= 360/(2*circle_r*M_PI) * ((2*(float)wheel_r*(float)M_PI)/(2*M_PI) * t.read() * fabsf(speed_left)*0.1f);
        t.reset();
        if(wanted_deg <= 0) {
            set_speed(0,0);
            is_turning = false;
            t.stop();
        }
    }
    printf("remaining deg %f\r\n", wanted_deg);
    return (wanted_deg);
}

/** has errors
 * moves to next coordinate from coordinate list
 * by Claudio Citterio
**/

int move_to_next_coord()
{

    float current_heading = get_current_heading();
    position current_pos = get_current_pos();
    position next_pos = get_next_pos();

    float needed_heading = 0;
    float distance = 0;

    // nord(-y) = 0 grad
    if(current_pos.y > next_pos.y) {
        if(current_pos.x > next_pos.x) needed_heading = 315;
        distance = sqrt2;
        if(current_pos.x == next_pos.x) needed_heading = 0;
        distance = 1;
        if(current_pos.x < next_pos.x) needed_heading = 45;
        distance = sqrt2;
    }
    if(current_pos.y == next_pos.y) {
        if(current_pos.x > next_pos.x) needed_heading = 270;
        distance = 1;
        if(current_pos.x == next_pos.x) //error same position;
            if(current_pos.x < next_pos.x) needed_heading = 90;
        distance = 1;
    }
    if(current_pos.y < next_pos.y) {
        if(current_pos.x > next_pos.x) needed_heading = 225;
        distance = sqrt2;
        if(current_pos.x == next_pos.x) needed_heading = 180;
        distance = 1;
        if(current_pos.x < next_pos.x) needed_heading = 135;
        distance = sqrt2;
    }

    if(needed_heading != current_heading) {
        turn_for_deg((needed_heading-current_heading));
    } else {
        move_for_distance(distance);
    }
    return 0;
}

/** 
 * this function searchs a nearby brick, moves towards it and grabbs it
 * by Tobias Berger, state machine by Claudio Citterio
**/
int move_in_search_for_brick()
{
    float upper = getDistanceIR(2);                                             // get distance from upper Center Sensor
    float lower = getDistanceIR(3);                                             // get distance from Lower Center Sensor
    //printf("Current Search State: >%d<\r\n",search_state);
    switch (search_state) {
        case 0: //first cycle right
            turn_for_deg(60.0f);                                               // call function and start turning
            search_state = 1;
            break;

        case 1: // turn right 60 deg
            if((lower<0.75f)&&(lower>0.1f)) {                                            // if something is in the range of 10 to 80cm at the lower Sensor
                if((lower+0.1f)<upper) {                                         // and nothing is detected with the upper Sensor
                    stop_turn();
                    search_state = 5;                                                 //brick found
                    printf("Brick found lower: %f upper:%f",lower,upper);
                }
            } else {
                search_state = 1;                                               // go to same state
                if(turn_for_deg(0) < 0) {
                    stop_turn();
                    search_state = 2;
                }
            }
            break;

        case 2: // first cycle left
            turn_for_deg(-120.0f);
            search_state = 3;
            break;

        case 3: // turn left 120 deg
            if((lower<0.75f)&&(lower>0.1f)) {                                            // if something is in the range of 10 to 75cm at the lower Sensor
                if((lower+0.1f)<upper) {                                         // and nothing is detected with the upper Sensor
                    stop_turn();
                    search_state = 10;                                                  //brick found
                    printf("Brick found lower: %f upper:%f",lower,upper);
                }
            } else {
                search_state = 3;                                               // go to same state
                if(turn_for_deg(0) < 0) {
                    stop_turn();
                    search_state = 20;                                          // error
                }
            }
            break;

        case 5: // turn back left
            turn_for_deg(-10.0f);
            search_state = 8;
            break;

        case 8: // turn back right continue
            if((lower<0.75f)&&(lower>0.1f)) {                                            // if something is in the range of 10 to 75cm at the lower Sensor
                if(fabsf((upper-lower))>0.02f) {                                         // and nothing is detected with the upper Sensor
                    stop_turn();
                    search_state = 10;                                          //brick found
                }
            } else {
                search_state = 8;                                               // go to same state
                if(turn_for_deg(0) < 0) {
                    stop_turn();
                    search_state = 20;                                          // error
                }
            }
            break;


        case 10: // first cycle move
            float distance_to_Brick = lower-(float)OFFSET_GREIFER_TO_IRSENSOR;                   // calculate
            printf("Distance to Brick: %f",distance_to_Brick);                  
            move_for_distance(distance_to_Brick);
            search_state =11;
            break;

        case 11: // move forward
            if(move_for_distance(0) < 0) {
                stop_move();
                search_state = 12;
            }
            break;

        case 12: // Grabbing
            return 50;                                                          //main state machine set as Grabbing

        default:
            printf("default State - move in search for brick\r\n");
            // error
            break;
    }
    return 47;                                                                  //called until function is done
}