Revision 1091
allow rotation while stuck
| branches/simulator/projects/simulator/simulator/core/motion.c | ||
|---|---|---|
| 3 | 3 |
#include <math.h> |
| 4 | 4 |
|
| 5 | 5 |
#include "motion.h" |
| 6 |
|
|
| 6 |
#include "rangefinders.h" |
|
| 7 | 7 |
#include "robot.h" |
| 8 | 8 |
|
| 9 | 9 |
#define CUTOFF 120 |
| ... | ... | |
| 68 | 68 |
/* XXX: this is a terrible hack */ |
| 69 | 69 |
update_rangefinders(r); |
| 70 | 70 |
for (divide = 0; divide < 5; divide++) {
|
| 71 |
/* Lets just call this a collision... */ |
|
| 71 | 72 |
if (r->shared->ranges.d[divide] < FUDGE) {
|
| 72 |
r->pose = old_pose; |
|
| 73 |
/* Restore x,y, but allow rotation */ |
|
| 74 |
r->pose.x = old_pose.x; |
|
| 75 |
r->pose.y = old_pose.y; |
|
| 76 |
|
|
| 77 |
/* Rotated robot, need to recalculate */ |
|
| 78 |
update_rangefinders(r); |
|
| 73 | 79 |
return 0; |
| 74 | 80 |
} |
| 75 | 81 |
} |
| 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 | 82 |
|
| 80 | 83 |
return 0; |
| 81 | 84 |
} |
Also available in: Unified diff