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