File content as of revision 118:fbd6d41f6ce8:

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


#include "Positioning.h"
coordinates current_coord;  // in degree // Updated
position current_pos;       // in degree // 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;
float direction_r = 0;
float direction_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 clamp_heading()
{

    while (current_heading >= 360) current_heading -= 360;
    while (current_heading < 0) current_heading += 360;

}

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

    // get wheel_speed
    float speed_left = get_speed_left();
    float speed_right = get_speed_right();

    // calc movement and turning speed
    float movement_speed = (speed_left + speed_right) /2.0f ;
    float turning_speed = speed_left - speed_right;

    // calc heading and coverd distance
    current_heading += 360/(2*circle_r*M_PI) * ((2*(float)wheel_r*(float)M_PI)/(2*M_PI) * t2.read() * turning_speed*0.1f);
    float distance = (2*(float)wheel_r*(float)M_PI)/(2*M_PI) * t2.read() * movement_speed * 0.1f;
    clamp_heading();
    t2.reset();
    t2.start();
    
    // calc new position
    current_coord.x += sin(current_heading/180.0f*(float)M_PI)* distance;
    current_coord.y -= cos(current_heading/180.0f*(float)M_PI) * distance;

     current_pos = coord_to_pos(current_coord);
}



int initial_positioning()
{
    if(init == false) {
        printf("initial_positioning init\r\n");
        last_dist_r = 100;
        last_dist_l = 100;

        //      deg_r = -50;
        //    deg_l = 50;

        deg_r = 0;
        deg_l = 0;

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

        if(t2 > 0.2f) {

            t2.reset();
            float dist_r = getDistanceIR(0);
            float dist_l = getDistanceIR(4);
            printf("distances %f || %f\r\n",dist_l, dist_r);

            //right
            if(dist_r < last_dist_r) {
                last_dist_r = dist_r;


                deg_r += 5;
                set_servo_position(2,-deg_r);
            } else if(direction_r == 0) {

                dist_r *= 100;
                float offsetx = 12;
                float offsety = 12;

                float x = (offsetx + sin(deg_r/180*(float)M_PI)*dist_r)/4;
                float y = (-offsety - cos(deg_r/180*(float)M_PI)*dist_r)/4;
                float hyp = sqrt(x*x+y*y);                     // y pos
                direction_r = asin(x/hyp)/(float)M_PI*180;     // winkel

                current_coord.y = hyp;

                while (direction_r >= 360) direction_r -= 360;
                while (direction_r < 0) direction_r += 360;
            }

            //left
            if(dist_l < last_dist_l) {
                last_dist_l = dist_l;
                deg_l -= 5;
                set_servo_position(0,-deg_l);
            } else if(direction_l == 0) {

                dist_r *= 100;
                float offsetx = -12;
                float offsety = 12;

                float x = (offsetx + sin(deg_l/180*(float)M_PI)*dist_l)/4;
                float y = (-offsety - cos(deg_l/180*(float)M_PI)*dist_l)/4;
                float hyp = sqrt(x*x+y*y);                     // y pos
                direction_l = asin(x/hyp)/(float)M_PI*180;     // winkel

                current_coord.x = hyp;

                while (direction_l >= 360) direction_l -= 360;
                while (direction_l < 0) direction_l += 360;
            }

            if(current_coord.x != 0 && current_coord.y != 0) {
                printf("directions: %f || %f \r\n", direction_l, direction_r);
                current_heading = (360-direction_r + 270-direction_l)/2;

                clamp_heading();
                printf("heading: %f \r\n", current_heading);

                //finished
                return 17;

            }
        }
    }
    // not finished
    return 11;
}