Revision 1092
slowed down motion and hacked more crap
branches/simulator/projects/simulator/simulator/core/motion.c | ||
---|---|---|
10 | 10 |
#define TIME 1 /*sec*/ |
11 | 11 |
#define ROBOT_WIDTH 131 /*mm*/ |
12 | 12 |
|
13 |
#define MOTOR_CONVERSION_FACTOR 10.0 |
|
13 |
#define MOTOR_CONVERSION_FACTOR 1000.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"); |
|
26 | 27 |
Pose old_pose = r->pose; |
27 | 28 |
|
28 | 29 |
short speed1 = r->shared->motor1; |
... | ... | |
42 | 43 |
} |
43 | 44 |
|
44 | 45 |
double radius; |
46 |
int divide; |
|
45 | 47 |
if (speed1 == speed2) { |
46 | 48 |
/* go straight */ |
47 | 49 |
r->pose.x += cos(theta) * speed1 / MOTOR_CONVERSION_FACTOR; |
48 | 50 |
r->pose.y += sin(theta) * speed1 / MOTOR_CONVERSION_FACTOR; |
51 |
for (divide = 0; divide < 5; divide++) { |
|
52 |
/* Lets just call this a collision... */ |
|
53 |
printf("%d: %d\n",divide,r->shared->ranges.d[divide]); |
|
54 |
if (r->shared->ranges.d[divide] < FUDGE) { |
|
55 |
/* Restore x,y, but allow rotation */ |
|
56 |
r->pose.x = old_pose.x; |
|
57 |
r->pose.y = old_pose.y; |
|
58 |
|
|
59 |
/* Rotated robot, need to recalculate */ |
|
60 |
update_rangefinders(r); |
|
61 |
return 0; |
|
62 |
} |
|
63 |
} |
|
49 | 64 |
return 0; |
50 | 65 |
} |
51 | 66 |
radius = ROBOT_WIDTH * speed1 / (speed1 - speed2); |
... | ... | |
61 | 76 |
r->pose.x += newy * - sin(theta); |
62 | 77 |
r->pose.y += newy * cos(theta); |
63 | 78 |
|
64 |
int divide = (t+r->pose.theta)/(2 * M_PI);
|
|
79 |
divide = (t+r->pose.theta)/(2 * M_PI); |
|
65 | 80 |
r->pose.theta = (t+r->pose.theta) - (2 * M_PI * divide); |
66 | 81 |
if (r->pose.theta<0) r->pose.theta += 2 * M_PI; |
67 | 82 |
|
... | ... | |
69 | 84 |
update_rangefinders(r); |
70 | 85 |
for (divide = 0; divide < 5; divide++) { |
71 | 86 |
/* Lets just call this a collision... */ |
87 |
printf("%d: %d\n",divide,r->shared->ranges.d[divide]); |
|
72 | 88 |
if (r->shared->ranges.d[divide] < FUDGE) { |
73 | 89 |
/* Restore x,y, but allow rotation */ |
74 | 90 |
r->pose.x = old_pose.x; |
Also available in: Unified diff