File content as of revision 33:8a98f8b9d859:

/**
 * Positioning function library
 * Handels position of the Robot on the map
**/

#include "mbed.h"
#include "pathfinding.h"
#include "IRsensor.h"

position current_pos;

void get_current_pos()
{
    return current_pos;
}

void initial_positioning()
{
    float last_dist_r = 1.0f;
    float last_dist_f = 1.0f;

    int deg_r = 0;
    int deg_l = 0;

    turn_straight_right();
    turn_straight_left();

    while(last_dist_r > sensors[r]) {
        turn_sensor_right(1); //turn sensor + 1 deg
        wait(0.1f)
        deg_r += 1;
        last_dist_r = sensors[r];
    }

    while(last_dist_l > sensors[l]) {
        turn_sensor_left(-1); //turn sensor - 1 deg
        wait(0.1f)
        deg_l += 1;
        last_dist_l = sensors[l];
    }

    int deg_l_2=0;
    turn_straight_left();
    last_dist_l = 0;

    while(last_dist_l < sensors[l]) {
        turn_sensor_left(1); //turn sensor +1 deg (positiv = uhrzeigersinn)
        wait(0.1f)
        deg_l_2 += 1;
        last_dist_l = sensors[l];
    }
    
    turn_straight_right();
    turn_straight_left();
    
    wait(0.2f);
    
}