File content as of revision 71:ddf4eb5c3081:

/**
 * Mapping function library
 * Handels Mapping of the arena and LEGO-stones
**/

#include "Mapping.h"

#define list_time_value         60
#define list_target_value       1
#define list_boundry_value      255
#define servo_angle             50


uint8_t obstacle_list[row][col] = { 0 };
uint8_t target_list[row][col] = {0};

uint8_t a = list_time_value; //Substitution um Matrix übersichtlich zu halten
uint8_t superpos[7][7] = {{0,0,a,a,a,0,0},
    {0,a,a,a,a,a,0},
    {a,a,a,a,a,a,a},
    {a,a,a,a,a,a,a},
    {a,a,a,a,a,a,a},
    {0,a,a,a,a,a,0},
    {0,0,a,a,a,0,0}
};

position old_pos = {0};
float old_heading = 0;

//******************************************************************************
void mapping()
{
   printf("mapping...\r\n"); 
    
    position current_pos = get_current_pos();
    float current_heading = get_current_heading();
    if(old_heading != current_heading || old_pos.x != current_pos.x || old_pos.y != current_pos.y) {
        //check_sensor(left);
        float distance0 = getDistanceIR(0);
        if(distance0 < 0.75f && distance0 > 0.1f) {
            set_servo_position(0, servo_angle);
            float distance = getDistanceIR(1);
            int object = 0;
            if(distance < 0.75f && distance > 0.1f && (distance -0.05f < distance0 || distance + 0.05f > distance0)) {
                object = 0;
            } else {
                object = 1;
            }
            position mapping_pos = position_calculation(distance0, servo_angle, 0.12, -0.12, current_heading, current_pos);
            draw_to_map(mapping_pos, object);
        }
        //check_sensor(right);
        float distance4 = getDistanceIR(4);
        if(distance4 < 0.75f && distance4 > 0.1f) {
            set_servo_position(1, -servo_angle);
            float distance = getDistanceIR(5);
            int object = 0;
            if(distance < 0.75f && distance > 0.1f && (distance -0.05f < distance4 || distance + 0.05f > distance4)) {
                object = 0;
            } else {
                object = 1;
            }
            position mapping_pos = position_calculation(distance4, -servo_angle, -0.12, -0.12, current_heading, current_pos);
            draw_to_map(mapping_pos, object);
        }
        //check_sensor(center);
        float distance2 = getDistanceIR(2);
        if(distance2 < 0.75f && distance2 > 0.1f) {
            float distance = getDistanceIR(3);
            int object = 0;
            if(distance < 0.75f && distance > 0.1f && (distance -0.05f < distance2 || distance + 0.05f > distance2)) {
                object = 0;
            } else {
                object = 1;
            }
            position mapping_pos = position_calculation(distance2, 0, -0.12, -0.12, current_heading, current_pos);
            draw_to_map(mapping_pos, object);
        }

        old_pos = current_pos;
        old_heading = current_heading;
    }
}

//******************************************************************************
void draw_to_map(position pos, int object)
{
    uint8_t x_negative_offset = 0;
    uint8_t x_positive_offset = 0;
    uint8_t y_negative_offset = 0;
    uint8_t y_positive_offset = 0;

    if (pos.x < 5) {
        x_negative_offset = 3 - pos.x;
    }
    if (pos.x > (col-5)) {
        x_positive_offset = pos.x - (col - 4);
    }

    if (pos.y < 5) {
        y_negative_offset = 3 - pos.y;
    }
    if (pos.y > (row-5)) {
        y_positive_offset = pos.y - (row - 4);
    }

    for (int y = 0 + y_negative_offset; y < 7 - y_positive_offset; y++) {
        for (int x = 0 + x_negative_offset; x < 7 - x_positive_offset; x++) {
            switch(object) {
                case 0:
                    a = list_time_value;
                    obstacle_list[pos.y - 3 + y][pos.x - 3 + x] = superpos[y][x];
                    break;
                case 1:
                    a = list_target_value;
                    target_list[pos.y - 3 + y][pos.x - 3 + x] = superpos[y][x];
                    break;
                case 2:
                    a = list_boundry_value;
                    target_list[pos.y - 3 + y][pos.x - 3 + x] = superpos[y][x];
                    break;
                default:
                    break;
            }
        }
    }
}

//******************************************************************************
void print_map()
{
    // Debug function for printing the obstacle matrix on putty. 
    for (int y = 0; y < col; y++) {
        for (int x = 0; x < row; x++) {
            printf("%d ", obstacle_list[y][x]);
        }
        printf("\r\n");
    }
    printf("\r\n");
}

//******************************************************************************
position position_calculation(float distance, float degree, float offsetx, float offsety, float heading, position current_pos)
{
    distance *= 100;
    offsetx *= 100;
    offsety *= 100;

    position pos = { 0 };
    float direction = 0;

    float x = (offsetx + sin(degree/180*(float)M_PI)*distance)/4;
    float y = (-offsety - cos(degree/180*(float)M_PI)*distance)/4;
    float hyp = sqrt(x*x+y*y);
    direction = asin(x/hyp)/(float)M_PI*180;
    direction += heading;

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

    printf("%f || %f || %f || %f\r\n", x, y, hyp, degree);

    if ((0 <= direction && direction < 90) || (180 <= direction && direction < 270)) {
        pos.x = current_pos.x + rint(sin(direction / 180 * (float)M_PI)*hyp);
        pos.y = current_pos.y - rint(cos(direction / 180 * (float)M_PI)*hyp);
        printf("%d || %d\r\n", pos.x,pos.y);
    }
    if ((90 <= direction && direction < 180) || (270 <= direction && direction < 360)) {
        pos.x = current_pos.x + rint(sin((180-direction) / 180 * (float)M_PI)*hyp);
        pos.y = current_pos.y + rint(cos((180-direction) / 180 * (float)M_PI)*hyp);
        printf("%d || %d\r\n", pos.x, pos.y);
    }

    return pos;
}


coordinates coordinates_calculation(float distance, float degree, float offsetx, float offsety, float heading, coordinates current_coord)
{
    distance *= 100;
    offsetx *= 100;
    offsety *= 100;

    coordinates coord = { 0 };
    float direction = 0;

    float x = (offsetx + sin(degree/180*(float)M_PI)*distance)/4;
    float y = (-offsety - cos(degree/180*(float)M_PI)*distance)/4;
    float hyp = sqrt(x*x+y*y);
    direction = asin(x/hyp)/(float)M_PI*180;
    direction += heading;

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

    printf("%f || %f || %f || %f\n", x, y, hyp, degree);

    if ((0 <= direction && direction < 90) || (180 <= direction && direction < 270)) {
        coord.x = current_coord.x + sin(direction / 180 * (float)M_PI)*hyp;
        coord.y = current_coord.y - cos(direction / 180 * (float)M_PI)*hyp;
        printf("%f || %f\n", coord.x,coord.y);
    }
    if ((90 <= direction && direction < 180) || (270 <= direction && direction < 360)) {
        coord.x = current_coord.x + sin((180-direction) / 180 * (float)M_PI)*hyp;
        coord.y = current_coord.y + cos((180-direction) / 180 * (float)M_PI)*hyp;
        printf("%f || %f\n", coord.x, coord.y);
    }

    return coord;
}
//******************************************************************************
int select_target()
{
    position myPos = get_current_pos();
    position diff = {0};
    target.x = 0;
    int closest_dist = 10000;
    int current_dist = 0;

    for(int i = 0; i < row; i++) {
        for(int j = 0; j < col; j++) {
            if(obstacle_list[i][j] == 2) {
                diff.x = abs(myPos.x - j);
                diff.y = abs(myPos.y - i);
                current_dist = diff.x * diff.y;
                if(current_dist < closest_dist) {
                    closest_dist = current_dist;
                    target.x = j;
                    target.y = i;
                }
            }
        }
    }
    if(target.x == 0) {
        return 47; // No Target found
    } else {
        return 36; // Target found
    }
}

//******************************************************************************
int switch_target_red()
{
    obstacle_list[target.y][target.x] = 0;
    return 0;
}