File content as of revision 75:dba260cb5ae4:

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

#include "Movement.h"

bool is_turning = false;
bool direction = false;
float wanted_deg = 0;
Timer t;
float previous_t = 0;

int moving()
{

    return 0;
}

int move_forward_for_distance(float distance)
{

    return 0;
}

int move_backward_for_distance()
{

    return 0;
}

float turn_for_deg(float deg) //if deg not 0 equals initilisation.
{

    if(deg != 0) {

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

        wanted_deg = sqrt(deg*deg);

        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();
        t.start();

    } else {

        float speed_left = get_speed_left();
        wanted_deg -=  360.0f / (2*radius*M_PI) * t.read() * fabsf(speed_left);
        t.reset();
        if(wanted_deg <= 0) {
            set_speed(0,0);
            is_turning = false;
            t.stop();
        }
    }
    return (wanted_deg);
}


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;
}

int move_in_search_for_brick()
{

    return 0;
}