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,10 +202,9 @@ static void conv_a2r(struct position *rel, ...@@ -202,10 +202,9 @@ static void conv_a2r(struct position *rel,
assert(abs_start != NULL); assert(abs_start != NULL);
assert(abs_finish != NULL); assert(abs_finish != NULL);
ct = cos(abs_start->theta); ct = cos(abs_start->theta);
st = sin(abs_start->theta); st = sin(abs_start->theta);
rel->x = ct*(abs_finish->x - abs_start->x) + rel->x = ct*(abs_finish->x - abs_start->x) +
st*(abs_finish->y - abs_start->y); st*(abs_finish->y - abs_start->y);
rel->y = ct*(abs_finish->y - abs_start->y) + rel->y = ct*(abs_finish->y - abs_start->y) +
...@@ -214,6 +213,7 @@ static void conv_a2r(struct position *rel, ...@@ -214,6 +213,7 @@ static void conv_a2r(struct position *rel,
rel->theta = abs_finish->theta - abs_start->theta; rel->theta = abs_finish->theta - abs_start->theta;
rel->timestamp = abs_finish->timestamp; rel->timestamp = abs_finish->timestamp;
info("a2r %f %f %f\n", rel->x, rel->y, rel->theta);
} }
/** /**
...@@ -235,8 +235,9 @@ static void conv_r2a(struct position *abs_finish, ...@@ -235,8 +235,9 @@ static void conv_r2a(struct position *abs_finish,
assert(rel != NULL); assert(rel != NULL);
assert(abs_start != NULL); assert(abs_start != NULL);
assert(abs_finish != 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); ct = cos(abs_start->theta);
st = sin(abs_start->theta); st = sin(abs_start->theta);
...@@ -247,6 +248,7 @@ static void conv_r2a(struct position *abs_finish, ...@@ -247,6 +248,7 @@ static void conv_r2a(struct position *abs_finish,
abs_finish->theta = abs_start->theta + rel->theta; abs_finish->theta = abs_start->theta + rel->theta;
abs_finish->timestamp = rel->timestamp; abs_finish->timestamp = rel->timestamp;
info("r2a %f %f %f\n", abs_finish->x, abs_finish->y, abs_finish->theta);
} }
/** /**
...@@ -701,7 +703,7 @@ int main(int argc, char *argv[]) ...@@ -701,7 +703,7 @@ int main(int argc, char *argv[])
(void) fclose(fp); (void) fclose(fp);
} }
} }
if (emc_hostname != NULL) { if (emc_hostname != NULL) {
struct mtp_packet *mp = NULL, *rmp = NULL; struct mtp_packet *mp = NULL, *rmp = NULL;
struct mtp_control mc; struct mtp_control mc;
......
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