root / branches / simulator / projects / simulator / simulator / core / rangefinders.c @ 1090
History | View | Annotate | Download (738 Bytes)
1 | 1064 | bcoltin | #include "rangefinders.h" |
---|---|---|---|
2 | |||
3 | #include <math.h> |
||
4 | |||
5 | #include "robot.h" |
||
6 | #include "world.h" |
||
7 | |||
8 | //TODO: These need to be measured...
|
||
9 | double rf_thetas[5] = {0.0, 2*M_PI/5, 4*M_PI/5, 6*M_PI/5, 8*M_PI/5}; |
||
10 | //double rf_thetas[5] = {0.0, 0.0, M_PI/2, M_PI, 3*M_PI/2};
|
||
11 | |||
12 | void update_rangefinders(Robot *bot)
|
||
13 | { |
||
14 | ray_t rf; |
||
15 | rf.p.x = bot->pose.x; |
||
16 | rf.p.y = bot->pose.y; |
||
17 | rf.d = 0.0; |
||
18 | /* Motion code has +y going down, I don't.
|
||
19 | * So to compensate, just reflect theta up to first quadrant.*/
|
||
20 | double theta = bot->pose.theta;
|
||
21 | int ir;
|
||
22 | double x;
|
||
23 | for (ir = 0; ir < 5; ir++) |
||
24 | { |
||
25 | rf.d = theta + rf_thetas[ir]; |
||
26 | x = collide_world(&rf); |
||
27 | //printf("@(%g) - [%d] = %g --> %d\n",rf.d, ir, x, (short)x);
|
||
28 | bot->shared->ranges.d[ir] = x; |
||
29 | } |
||
30 | } |