Colony » Colony Scout » Colony Scout OS

<<<<<<< HEAD

    // matrices for going from robot's frame to base frame

    float rightMat[2][2] = {{0, 1}, {-1, 0}};

    float downMat[2][2] = {{-1, 0}, {0, -1}};

    float leftMat[2][2] = {{0, 1}, {1, 0}};

    // Look to the left (and update map).

    float left_distance = readings[0]; // w.r.t. robot

    if(left_distance == 0)

        return false;

    int left_idx = -mm_to_idx(left_distance); // w.r.t. map

    // plot to map if indices are valid

    if (0 <= left_idx && left_idx < MAP_LENGTH)

        map[row][col+left_idx] = SEEN;

    // Look in the other directions (and update map).

    for (int i = 24; i < 48; i++) {

        float distance = readings[i]; // w.r.t. robot

        if(distance == 0)

            return false;

        if(distance >= 500)

            break; // too far to be accurate

        float theta = (M_PI/24)*i - M_PI;

        float xDist = distance*cos(theta); // w.r.t. robot

        float yDist = distance*sin(theta); // w.r.t. robot

        float inputs[2] = {xDist, yDist};

        float *ans;

        // re-orient x and y distances based on direction

        switch(dir) {

            case UP:

                ans[0] = xDist;

                ans[1] = yDist;

                break;

            case RIGHT:

                ans = matrix_mult(inputs, rightMat);

                break;

            case DOWN:

                ans = matrix_mult(inputs, downMat);

                break;

            case LEFT:

                ans = matrix_mult(inputs, leftMat);

                break;

        }

        // indices into the map

        int pixDistX = row + mm_to_idx(ans[0]);

        int pixDistY = col + mm_to_idx(ans[1]);

        // plot to map if indices are valid

        if (0 <= pixDistX && pixDistX < MAP_LENGTH

            && 0 <= pixDistY && pixDistY < MAP_LENGTH)

                map[pixDistX][pixDistY] = SEEN;

=======

    int left_idx = m_to_idx(left_distance);

    int front_idx = m_to_idx(front_distance);

    int right_idx = m_to_idx(right_distance);

>>>>>>> dbbad8ed5cba08f5500e7d333beab2d089f9956c