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