Revision 1090
awful hack for collisions that appears to work
branches/simulator/projects/simulator/simulator/core/motion.c | ||
---|---|---|
12 | 12 |
|
13 | 13 |
#define MOTOR_CONVERSION_FACTOR 10.0 |
14 | 14 |
|
15 |
#define FUDGE 10 /* minimum rangefinder distance until collision */ |
|
16 |
|
|
15 | 17 |
/** move_robot will move a robot from its initial position, (x,y), and theta (in radians) to a new position given speed. |
16 | 18 |
* (x,y) and theta will be updated by the move_robot function instead of returning a value |
17 | 19 |
* (x,y) is some kind of absolute position in the "world", (0,0) is the top left of the "world" |
... | ... | |
21 | 23 |
**/ |
22 | 24 |
int move_robot(Robot* r) |
23 | 25 |
{ |
26 |
Pose old_pose = r->pose; |
|
27 |
|
|
24 | 28 |
short speed1 = r->shared->motor1; |
25 | 29 |
short speed2 = r->shared->motor2; |
26 | 30 |
float theta = r->pose.theta; |
... | ... | |
61 | 65 |
r->pose.theta = (t+r->pose.theta) - (2 * M_PI * divide); |
62 | 66 |
if (r->pose.theta<0) r->pose.theta += 2 * M_PI; |
63 | 67 |
|
68 |
/* XXX: this is a terrible hack */ |
|
69 |
update_rangefinders(r); |
|
70 |
for (divide = 0; divide < 5; divide++) { |
|
71 |
if (r->shared->ranges.d[divide] < FUDGE) { |
|
72 |
r->pose = old_pose; |
|
73 |
return 0; |
|
74 |
} |
|
75 |
} |
|
76 |
/* XXX: should store values from before we alter rangefinders |
|
77 |
* and restore here, but this is all temporary...right? */ |
|
78 |
update_rangefinders(r); |
|
79 |
|
|
64 | 80 |
return 0; |
65 | 81 |
} |
66 | 82 |
|
Also available in: Unified diff