Commit 49d70d85 authored by Timothy Stack's avatar Timothy Stack
Browse files

Truncate coords returned from the robots, since it seems to confuse

it and fed back in.
parent fb9fe771
......@@ -202,7 +202,6 @@ static void conv_a2r(struct position *rel,
assert(abs_start != NULL);
assert(abs_finish != NULL);
ct = cos(abs_start->theta);
st = sin(abs_start->theta);
......@@ -214,6 +213,7 @@ static void conv_a2r(struct position *rel,
rel->theta = abs_finish->theta - abs_start->theta;
rel->timestamp = abs_finish->timestamp;
info("a2r %f %f %f\n", rel->x, rel->y, rel->theta);
}
/**
......@@ -236,7 +236,8 @@ static void conv_r2a(struct position *abs_finish,
assert(abs_start != NULL);
assert(abs_finish != NULL);
rel->x = floor(rel->x * 1000.0) / 1000.0;
rel->y = floor(rel->y * 1000.0) / 1000.0;
ct = cos(abs_start->theta);
st = sin(abs_start->theta);
......@@ -247,6 +248,7 @@ static void conv_r2a(struct position *abs_finish,
abs_finish->theta = abs_start->theta + rel->theta;
abs_finish->timestamp = rel->timestamp;
info("r2a %f %f %f\n", abs_finish->x, abs_finish->y, abs_finish->theta);
}
/**
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment