File content as of revision 121:af16ffc05593:

/**
 * 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 += 3;
                set_servo_position(2,deg_r);
                
            } else if(current_coord.y == 0) {
                deg_r -= 5; //compensate overturning
                //deg_r -= 3; // compensate not straight looking
                dist_r *= 100;
                float offsetx = 11.5;
                float offsety = 11;

                current_coord.y = dist_r + sqrt(offsetx*offsetx+offsety*offsety)*cos((90-deg_r-atan(offsety/offsetx))/180.0f*(float)M_PI);


            }

            //left
            if(dist_l < last_dist_l) {
                last_dist_l = dist_l;
                deg_l -= 5;
                set_servo_position(0,deg_l);
                
            } else if(current_coord.x == 0) {
                deg_l += 3; //compensate overturning
                deg_l -= 3; //compensate not straight looking
                dist_r *= 100;
                float offsetx = 11.5;
                float offsety = 11;

                current_coord.x = dist_r + sqrt(offsetx*offsetx+offsety*offsety)*cos((90-fabsf(deg_l)-atan(offsetx/offsety))/180.0f*(float)M_PI);


            }

            if(current_coord.x != 0 && current_coord.y != 0) {
                
                printf("degs: %d || %d \r\n", deg_l , deg_r);
                printf("coords : %f || %f \r\n", current_coord.x, current_coord.y);
                current_heading = (360-deg_r + 270-deg_l)/2;

                clamp_heading();
                printf("heading: %f \r\n", current_heading);
                
                current_coord.x = 20;
                current_coord.y = 20;

                //finished
                return 11;

            }
        }
    }
    // not finished
    return 17;
}