File content as of revision 69:1fdcef6a7577:

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


#include "Positioning.h"
coordinates current_coord;  // Updated
position current_pos;       // Generated from current_coord
float current_heading;
position next_pos;

bool init = false;
Timer t2;

int deg_r = 0;
int deg_l = 0;
float last_dist_r = 0;
float last_dist_l = 0;


coordinates get_current_coord()
{
    return current_coord;
}

coordinates pos_to_coord(position pos)
{
    coordinates coord = {0};
    coord.x = pos.x + 0.5f;
    coord.y = pos.y + 0.5f;
    return coord;
}

position coord_to_pos(coordinates coord)
{
    position pos = {0};
    pos.x = rint(coord.x -0.5f);    //round values
    pos.y = rint(coord.y -0.5f);
    return pos;
}

position get_current_pos()
{
    current_pos = coord_to_pos(current_coord);
    return current_pos;
}

position get_next_pos()
{
    return next_pos;
}

float get_current_heading()
{
    return current_heading;
}


void positioning()
{
    printf("positioning...\r\n");
}



int initial_positioning()
{
    if(init == false) {

        last_dist_r = 100;
        last_dist_l = 100;

        deg_r = -50;
        deg_l = 50;

        set_servo_position(0,deg_l);    //servo sensor left
        set_servo_position(2,-deg_r);   //servo sensor right
        t2.start();
        init = true;
    } else {

        if(t2 > 0.2f) {
            t2 = 0;

            float dist_r = getDistanceIR(4);
            float dist_l = getDistanceIR(0);

            //right
            if(dist_r < last_dist_r) {
                last_dist_r = dist_r;
                deg_r += 5;
                set_servo_position(2,-deg_r);
            } else {
                  

                //    current_coord.y =
            }


            //left
        }

    }
    //finished
    //return 11

        // not finished*/
    return 16;
}

/*
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);
*/