root / branches / simulator / projects / simulator / test / test_motors.c @ 1001
History | View | Annotate | Download (1.65 KB)
1 | 985 | chihsiuh | #include <stdio.h> |
---|---|---|---|
2 | 1001 | chihsiuh | #include <math.h> |
3 | 985 | chihsiuh | |
4 | #define CUTOFF 120 |
||
5 | 1001 | chihsiuh | #define ABS(x) (x<0?-x:x) |
6 | #define TIME 1 /*sec*/ |
||
7 | #define ROBOT_WIDTH 131 /*mm*/ |
||
8 | 985 | chihsiuh | |
9 | 1001 | chihsiuh | /* move will move a robot from its initial position, (x,y), and theta (in radians) to a new position given speed.
|
10 | * (x,y) and theta will be updated by the move function instead of returning a value
|
||
11 | 985 | chihsiuh | * (x,y) is some kind of absolute position in the "world", let's make (0,0) the top left of the "world"
|
12 | 1001 | chihsiuh | * theta will an angle be between 0 - 2*Pi (0 being faces east)
|
13 | 985 | chihsiuh | * speed is between 0 - 255, there is some magical cutoff point before the motors actually starts running
|
14 | * move will return 0 if successful
|
||
15 | */
|
||
16 | 1001 | chihsiuh | int move (int *x, int *y, double *theta, int speed1, int speed2) { |
17 | if (*theta < 0 || *theta > 2*M_PI) return 1; |
||
18 | 985 | chihsiuh | if (speed1 < 0 || speed1 > 255) return 1; |
19 | if (speed2 < 0 || speed2 > 255) return 1; |
||
20 | |||
21 | /* if speed is lower than the cut off, don't move */
|
||
22 | 1001 | chihsiuh | if (ABS(speed1) < CUTOFF) {
|
23 | 985 | chihsiuh | speed1 = 0;
|
24 | } |
||
25 | 1001 | chihsiuh | if (ABS(speed2) < CUTOFF) {
|
26 | 985 | chihsiuh | speed2 = 0;
|
27 | } |
||
28 | 1001 | chihsiuh | double radius;
|
29 | if (speed1 == speed2) {
|
||
30 | /* go straight */
|
||
31 | *x += cos(*theta) * speed1; |
||
32 | *y += sin(*theta) * speed1; |
||
33 | return 0; |
||
34 | } |
||
35 | radius = ROBOT_WIDTH * speed1 / (speed1 - speed2); |
||
36 | |||
37 | double t = speed1 / radius;
|
||
38 | 985 | chihsiuh | |
39 | 1001 | chihsiuh | double newx = radius * sin(t);
|
40 | double newy = radius - radius * cos(t);
|
||
41 | 985 | chihsiuh | |
42 | 1001 | chihsiuh | *x += newx * cos(*theta); |
43 | *y += newx * sin(*theta); |
||
44 | 985 | chihsiuh | |
45 | 1001 | chihsiuh | *x += newy * - sin(*theta); |
46 | *y += newy * cos(*theta); |
||
47 | |||
48 | *theta = fmod((t + *theta), (2 * M_PI));
|
||
49 | if (*theta<0) *theta += 2 * M_PI; |
||
50 | |||
51 | 985 | chihsiuh | return 0; |
52 | } |
||
53 | |||
54 | int main () {
|
||
55 | int x = 0; |
||
56 | int y = 0; |
||
57 | 1001 | chihsiuh | double t = 0.0; |
58 | printf("%d %d %lf\n",x,y,t);
|
||
59 | move(&x,&y,&t,150,100); |
||
60 | printf("%d %d %lf\n",x,y,t);
|
||
61 | 985 | chihsiuh | return 0; |
62 | } |