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