Revision 1044
| branches/simulator/projects/simulator/simulator/core/main.c (revision 1044) | ||
|---|---|---|
| 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 (revision 1044) | ||
|---|---|---|
| 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 (revision 1044) | ||
|---|---|---|
| 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