Colony

#include "rangefinders.h"

#include <math.h>

#include "robot.h"

#include "world.h"

//TODO: These need to be measured...

double rf_thetas[5] = {8*M_PI/5, 0.0, 2*M_PI/5, 4*M_PI/5, 6*M_PI/5};

//double rf_thetas[5] = {0.0, 0.0, M_PI/2, M_PI, 3*M_PI/2};

void update_rangefinders(Robot *bot)

{

    ray_t rf;

        rf.p.x = bot->pose.x;

        rf.p.y = bot->pose.y;

        rf.d = 0.0;

    /* Motion code has +y going down, I don't.

     * So to compensate, just reflect theta up to first quadrant.*/

    double theta = bot->pose.theta;

    int ir;

    double x;

    for (ir = 0; ir < 5; ir++)

    {

        rf.d = theta + rf_thetas[ir];

        x = collide_world(&rf);

        // printf("@(%g) - [%d] = %g --> %d\n",rf.d, ir, x, (short)x);

        bot->shared->ranges.d[ir] = x;

    }

}