Revision 1094
simulating rangefinder and smart run around
changed the position of IR sensors defined in libsim
libsim and libdragonfly use different numbering for IRs (#418, #419)
added smart_run_around to template, right now it just hits wall, backs
up then segfaults
| motion.c | ||
|---|---|---|
| 10 | 10 |
#define TIME 1 /*sec*/ |
| 11 | 11 |
#define ROBOT_WIDTH 131 /*mm*/ |
| 12 | 12 |
|
| 13 |
#define MOTOR_CONVERSION_FACTOR 1000.0
|
|
| 13 |
#define MOTOR_CONVERSION_FACTOR 10.0 |
|
| 14 | 14 |
|
| 15 | 15 |
#define FUDGE 10 /* minimum rangefinder distance until collision */ |
| 16 | 16 |
|
| ... | ... | |
| 23 | 23 |
**/ |
| 24 | 24 |
int move_robot(Robot* r) |
| 25 | 25 |
{
|
| 26 |
printf("Called motion\n");
|
|
| 27 | 26 |
Pose old_pose = r->pose; |
| 28 | 27 |
|
| 29 | 28 |
short speed1 = r->shared->motor1; |
| ... | ... | |
| 50 | 49 |
r->pose.y += sin(theta) * speed1 / MOTOR_CONVERSION_FACTOR; |
| 51 | 50 |
for (divide = 0; divide < 5; divide++) {
|
| 52 | 51 |
/* Lets just call this a collision... */ |
| 53 |
printf("%d: %d\n",divide,r->shared->ranges.d[divide]);
|
|
| 52 |
//printf("%d: %d\n",divide,r->shared->ranges.d[divide]);
|
|
| 54 | 53 |
if (r->shared->ranges.d[divide] < FUDGE) {
|
| 55 | 54 |
/* Restore x,y, but allow rotation */ |
| 56 | 55 |
r->pose.x = old_pose.x; |
| ... | ... | |
| 60 | 59 |
update_rangefinders(r); |
| 61 | 60 |
return 0; |
| 62 | 61 |
} |
| 63 |
} |
|
| 62 |
}
|
|
| 64 | 63 |
return 0; |
| 65 | 64 |
} |
| 66 | 65 |
radius = ROBOT_WIDTH * speed1 / (speed1 - speed2); |
| ... | ... | |
| 84 | 83 |
update_rangefinders(r); |
| 85 | 84 |
for (divide = 0; divide < 5; divide++) {
|
| 86 | 85 |
/* Lets just call this a collision... */ |
| 87 |
printf("%d: %d\n",divide,r->shared->ranges.d[divide]);
|
|
| 86 |
//printf("%d: %d\n",divide,r->shared->ranges.d[divide]);
|
|
| 88 | 87 |
if (r->shared->ranges.d[divide] < FUDGE) {
|
| 89 | 88 |
/* Restore x,y, but allow rotation */ |
| 90 | 89 |
r->pose.x = old_pose.x; |
Also available in: Unified diff