Revision 1010
Motion works, but needs fine tuning.
branches/simulator/projects/simulator/simulator/core/motion.c | ||
---|---|---|
3 | 3 |
|
4 | 4 |
#include "motion.h" |
5 | 5 |
|
6 |
#define CUTOFF 120 |
|
6 |
#include "robot.h" |
|
7 |
|
|
8 |
#define CUTOFF 120 |
|
7 | 9 |
#define ABS(x) (x<0?-x:x) |
8 |
#define TIME 1 /*sec*/
|
|
10 |
#define TIME 1 /*sec*/
|
|
9 | 11 |
#define ROBOT_WIDTH 131 /*mm*/ |
10 | 12 |
|
11 | 13 |
/** move will move a robot from its initial position, (x,y), and theta (in radians) to a new position given speed. |
... | ... | |
15 | 17 |
* speed is between 0 - 255, there is some magical cutoff point before the motors actually starts running |
16 | 18 |
* move will return 0 if successful |
17 | 19 |
**/ |
18 |
int move (int *x, int *y, double *theta, int speed1, int speed2) { |
|
19 |
if (*theta < 0 || *theta > 2*M_PI) return 1; |
|
20 |
if (speed1 < 0 || speed1 > 255) return 1; |
|
21 |
if (speed2 < 0 || speed2 > 255) return 1; |
|
20 |
int move_robot(Robot* r) |
|
21 |
{ |
|
22 |
short speed1 = r->shared->motor1; |
|
23 |
short speed2 = r->shared->motor2; |
|
24 |
float theta = r->pose.theta; |
|
22 | 25 |
|
23 |
/* if speed is lower than the cut off, don't move */ |
|
24 |
if (ABS(speed1) < CUTOFF) { |
|
25 |
speed1 = 0; |
|
26 |
} |
|
27 |
if (ABS(speed2) < CUTOFF) { |
|
28 |
speed2 = 0; |
|
29 |
} |
|
30 |
double radius; |
|
31 |
if (speed1 == speed2) { |
|
32 |
/* go straight */ |
|
33 |
*x += cos(*theta) * speed1; |
|
34 |
*y += sin(*theta) * speed1; |
|
35 |
return 0; |
|
36 |
} |
|
37 |
radius = ROBOT_WIDTH * speed1 / (speed1 - speed2); |
|
38 |
|
|
39 |
double t = speed1 / radius; |
|
26 |
if (theta < 0 || theta > 2*M_PI) return 1; |
|
27 |
if (speed1 < 0 || speed1 > 255) return 1; |
|
28 |
if (speed2 < 0 || speed2 > 255) return 1; |
|
40 | 29 |
|
41 |
double newx = radius * sin(t); |
|
42 |
double newy = radius - radius * cos(t); |
|
30 |
/* if speed is lower than the cut off, don't move */ |
|
31 |
if (ABS(speed1) < CUTOFF) { |
|
32 |
speed1 = 0; |
|
33 |
} |
|
34 |
if (ABS(speed2) < CUTOFF) { |
|
35 |
speed2 = 0; |
|
36 |
} |
|
37 |
double radius; |
|
38 |
if (speed1 == speed2) { |
|
39 |
/* go straight */ |
|
40 |
r->pose.x += cos(theta) * speed1; |
|
41 |
r->pose.y += sin(theta) * speed1; |
|
42 |
return 0; |
|
43 |
} |
|
44 |
radius = ROBOT_WIDTH * speed1 / (speed1 - speed2); |
|
45 |
|
|
46 |
double t = speed1 / radius; |
|
43 | 47 |
|
44 |
*x += newx * cos(*theta);
|
|
45 |
*y += newx * sin(*theta);
|
|
48 |
double newx = radius * sin(t);
|
|
49 |
double newy = radius - radius * cos(t);
|
|
46 | 50 |
|
47 |
*x += newy * - sin(*theta);
|
|
48 |
*y += newy * cos(*theta);
|
|
51 |
r->pose.x += newx * cos(theta);
|
|
52 |
r->pose.y += newx * sin(theta);
|
|
49 | 53 |
|
50 |
*theta = fmod((t + *theta), (2 * M_PI));
|
|
51 |
if (*theta<0) *theta += 2 * M_PI;
|
|
54 |
r->pose.x += newy * - sin(theta);
|
|
55 |
r->pose.y += newy * cos(theta);
|
|
52 | 56 |
|
53 |
return 0; |
|
57 |
r->pose.theta = fmod((t + theta), (2 * M_PI)); |
|
58 |
if (r->pose.theta<0) r->pose.theta += 2 * M_PI; |
|
59 |
|
|
60 |
return 0; |
|
54 | 61 |
} |
55 | 62 |
|
branches/simulator/projects/simulator/simulator/core/robot.c | ||
---|---|---|
23 | 23 |
#include <pthread.h> |
24 | 24 |
|
25 | 25 |
#include "robot.h" |
26 |
#include "motion.h" |
|
26 | 27 |
#include "gtk_gui.h" |
27 | 28 |
|
28 | 29 |
void sig_chld_handler(int sig); |
30 |
void robot_update(int i); |
|
29 | 31 |
|
30 | 32 |
// global variables |
31 | 33 |
int first_available_id = 0; |
... | ... | |
236 | 238 |
finished = 0; |
237 | 239 |
pthread_mutex_unlock(&all_finished_mutex); |
238 | 240 |
for (i = 0; i < robots_size; i++) |
241 |
if (robots[i].id != -1) |
|
242 |
robot_update(i); |
|
243 |
for (i = 0; i < robots_size; i++) |
|
239 | 244 |
{ |
240 | 245 |
if (robots[i].id == -1) |
241 | 246 |
continue; |
... | ... | |
246 | 251 |
} |
247 | 252 |
} |
248 | 253 |
|
254 |
void robot_update(int i) |
|
255 |
{ |
|
256 |
Robot* r = &(robots[i]); |
|
257 |
if (r->id == -1) |
|
258 |
return; |
|
259 |
move_robot(r); |
|
260 |
} |
|
261 |
|
branches/simulator/projects/simulator/simulator/Makefile | ||
---|---|---|
1 | 1 |
# Add new files here |
2 | 2 |
COMMON_SRCS := |
3 |
CORE_SRCS := main.c robot.c |
|
3 |
CORE_SRCS := main.c robot.c motion.c
|
|
4 | 4 |
GUI_SRCS := gtk_gui.c gtk_environment_view.c draw_funcs.c |
5 | 5 |
|
6 | 6 |
CORE_DIR := core |
Also available in: Unified diff