File content as of revision 46:8b52c7b34d34:

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

#include "Mapping.h"
//#include "Pathfinding.h"

uint8_t obstacle_list_superpos[7][7] = {{0,0,2,2,2,0,0},
                                        {0,2,2,2,2,2,0},
                                        {2,2,2,2,2,2,2},
                                        {2,2,2,1,2,2,2},
                                        {2,2,2,2,2,2,2},
                                        {0,2,2,2,2,2,0},
                                        {0,0,2,2,2,0,0}};
 
position old_pos = {0};
position mapping_pos_right = {0};
position mapping_pos_left = {0};

float old_heading = {0};
float distance_left = 0;
float distance_right = 0;

void mapping()
{
    position new_pos = get_current_pos();
    float new_heading = get_current_heading();
    if(old_heading != new_heading || old_pos.x != new_pos.x || old_pos.y != new_pos.y){
    //pseudo code
    //check_sensor(left);
    distance_left = getDistanceIR(0);
    if(distance_left < 0.75f && distance_left > 0.1f){
        mapping_pos_left = position_calculation(distance_left, 50, 0.12, 0.12, new_heading);
        draw_to_map(mapping_pos_left);
    }
    //check_sensor(right);
    distance_right = getDistanceIR(4);
    if(distance_right < 0.75f && distance_right > 0.1f){
        mapping_pos_right = position_calculation(distance_right, -50, -0.12, 0.12, new_heading);
        draw_to_map(mapping_pos_right);
    }
    
    old_pos = new_pos;
    old_heading = new_heading;
    }   
}

void draw_to_map(position pos){
    
    }
    
position position_calculation(float distance,float degree,float ofsettx,float ofsetty, float heading){
    
    
    position pos = {0};
    return pos;
}

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 remove_target()
{
    obstacle_list[target.y][target.x] = 0;
    return 0;
}

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





/*
void chack_sensor(sensor)
{
    get_distance();
    if(sensor < 80cm || sensor > 10cm) {
        calculate_position(sensor, distance);
        check_map(position.this, obstacle);
        if(map.no-entry){
            check_if_obstacle(distance);
            update_map();
        }
    }
}

void get_distance(sensor)
{
    sensor.value;
    return value;
}

void calcualte_position(sensor, distance)
{
    do math;
    return position;
}

void check_if_obstacle(distance)
{
    move_scanner();
    distance2 = get_distance(sensor);
    reset_scanner();
    if(distance -5cm < distance2 && distance +5cm > distance2) {
        return 1;
    } else {
        return 0;
    }
}*/