File content as of revision 54:36c290715769:

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

#include "Movement.h"

int moving()
{

    return 0;
}

int move_forward_for_distance(float distance)
{

    return 0;
}

int move_backward_for_distance()
{

    return 0;
}

int turn_for_deg(float deg)
{

    return 0;
}


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