File content as of revision 74:d9c387b83196:

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

#include "Movement.h"
#define OFFSET_GREIFER_TO_IRSENSOR 100                                          // Constant for distance between front IR Sensor and the postion where the Greifer is in grabbing Position

bool is_turning = false;
bool direction = false;
float wanted_deg = 0;
float current_deg = 0;
Timer t;
float previous_t = 0;
bool first_search_cycle = true;                                                 // flag for state first time in function "move in search for brick" 
bool brick_found = false;                                                       // flag for saving whether a brick was found or not
bool movement_to_brick_finished = false;                                        // flag for saving whether movement to brick is finished or not

int moving()
{

    return 0;
}

int move_forward_for_distance(float distance)
{

    return 0;
}

int move_backward_for_distance()
{

    return 0;
}

void turn_for_deg(float deg)
{

    if(is_turning == false) {

        is_turning = true;
        float left = 0;
        float right = 0;

        wanted_deg = deg;
        current_deg = 0;

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

    } else {

        float speed_left = get_speed_left();
        float delta_t = t - previous_t;
        previous_t = t;
        current_deg +=  360.0f / (2*radius*M_PI) * delta_t * speed_left;
        if(current_deg * current_deg > wanted_deg * wanted_deg) {
            set_speed(0,0);
            is_turning = false;
            t.stop();
        }
    }
}


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_forward_for_distance(distance);
    }
    return 0;
}

// Tobias Berger
int move_in_search_for_brick()
{
    
    float distance_to_Brick;                                                    // variable how far away the brick is
    // Init State turn for 60 degrees CW
    if(first_search_cycle==true){
        first_search_cycle=false;                                               // delet flag for initial condition
        float restdeg=turn_for_deg(60);                                         // call function and start turning
    }
    
    // Search for Brick and evaluation
    float upper = getDistanceIR(4);                                             // get distance from upper Center Sensor            CHECK SENSORNUMBERS NOT SURE
    float lower = getDistanceIR(6);                                             // get distance from Lower Center Sensor
    
    if((lower<800.0)&&(lower>100)){                                             // if something is in the range of 10 to 80cm at the lower Sensor
        if((upper>800.0)&&(upper<100)){                                         // and nothing is detected with the upper Sensor
            brick_found = true;                                                   
        }
    }
    else
    {
        brick_found = false;
        
        if(restdeg>1)||(restdeg<-1){                                             // continue turning until restdegree nearly 0                                     
            turn_for_deg(restdeg);
        }
        else                                                                    // if restdegree nearly 0 and nothing found => turn in other direction
        {
            restdeg=-60;                                                        //                                                                                      60 DEGREES FROM YET WILL BE THE SAME AREA AS PREVIOUSLY
        }                                                     
                                                          
    }

    if(brick_found==true){
        turn_for_deg(0);                                                        // stop turning
        first_search_cyle=true;                                                 // set flag to start turning once again respectivly to get in Initialstate
        lower=getDistaceIR(6);                                                  // Measure distance to Brick for Movement
        distance_to_Brick = lower-OFFSET_GREIFER_TO_IRSENSOR;                   // calculate
        move_forward_for_distance(distance_to_Brick);                           // Move to Brick                                     ATTENTION FUNCTION NOT IMPLEMENTED YET
        arm_position_grabbing();                                                // Call Aeschlimans function                         MOVE A LITTLE AFTER GREIFER ON FLOOR IN AESCHLIMANS FUNCTION?
        //movement_to_brick_finished=true;
    }


    
    return 0;
}