Revision 1044
added update_rangefinders, which should be enough for the demo tomorrow
branches/simulator/projects/simulator/simulator/core/main.c | ||
---|---|---|
28 | 28 |
|
29 | 29 |
|
30 | 30 |
robot_create(argv[1]); |
31 |
/*robot_create(argv[1]); |
|
31 | 32 |
robot_create(argv[1]); |
32 | 33 |
robot_create(argv[1]); |
33 | 34 |
robot_create(argv[1]); |
... | ... | |
46 | 47 |
robot_create(argv[1]); |
47 | 48 |
robot_create(argv[1]); |
48 | 49 |
robot_create(argv[1]); |
49 |
robot_create(argv[1]);
|
|
50 |
*/
|
|
50 | 51 |
|
51 | 52 |
|
52 | 53 |
sigset_t set; |
branches/simulator/projects/simulator/simulator/core/robot.c | ||
---|---|---|
25 | 25 |
|
26 | 26 |
#include "robot.h" |
27 | 27 |
#include "motion.h" |
28 |
#include "world.h" |
|
28 | 29 |
#include "gtk_gui.h" |
29 | 30 |
|
30 | 31 |
void sig_chld_handler(int sig); |
... | ... | |
277 | 278 |
if (r->id == -1) |
278 | 279 |
return; |
279 | 280 |
move_robot(r); |
281 |
update_rangefinders(r); |
|
280 | 282 |
} |
281 | 283 |
|
branches/simulator/projects/simulator/simulator/core/world.c | ||
---|---|---|
18 | 18 |
|
19 | 19 |
int init_world(int max_objs, bbox_t b) |
20 | 20 |
{ |
21 |
double pts[8] = {b.p1.x, b.p1.y, b.p1.x, b.p2.y, b.p2.x,b.p2.y,b.p2.x,b.p1.y}; |
|
21 | 22 |
world.max_objs = max_objs; |
22 | 23 |
world.cur_objs = 0; |
23 | 24 |
world.objs = calloc(max_objs, sizeof(object_t)); |
24 | 25 |
world.win = b; |
25 |
//PBBOX(b); |
|
26 |
/* Add a polygon surrounding the world */ |
|
27 |
create(ID_POLY,4,POLY_CONNECTED,pts); |
|
26 | 28 |
} |
27 | 29 |
int destroy_world() |
28 | 30 |
{ |
... | ... | |
235 | 237 |
double rf_thetas[5] = {0.0, 2*M_PI/5, 4*M_PI/5, 6*M_PI/5, 8*M_PI/5}; |
236 | 238 |
void update_rangefinders(Robot *bot) |
237 | 239 |
{ |
238 |
//RangeFinder r = bot->shared->ranges; |
|
239 |
//ray_t rf = {bot->pose.x, bot->pose.y}; |
|
240 |
ray_t rf = {0,0,0}; |
|
240 |
RangeFinder r = bot->shared->ranges; |
|
241 |
ray_t rf = {bot->pose.x, bot->pose.y, 0}; |
|
242 |
/* Motion code has +y going down, I don't. |
|
243 |
* So to compensate, just reflect theta up to first quadrant.*/ |
|
244 |
double theta = bot->pose.theta; |
|
245 |
theta = 2*M_PI - theta; |
|
246 |
//ray_t rf = {0,0,0}; |
|
241 | 247 |
int ir; |
242 | 248 |
double x; |
243 | 249 |
for (ir = 0; ir < 5; ir++) |
244 | 250 |
{ |
245 | 251 |
rf.d = rf_thetas[ir]; |
246 |
x = collide_poly(&rf, world.objs);
|
|
252 |
x = collide_world(&rf);
|
|
247 | 253 |
printf("[%d] = %g\n", ir, x); |
248 |
//bot->shared->ranges.d[ir] = collide_world(&rf);
|
|
254 |
bot->shared->ranges.d[ir] = x;
|
|
249 | 255 |
} |
250 | 256 |
} |
251 | 257 |
|
Also available in: Unified diff