root / branches / simulator / projects / simulator / test / test_motors.c @ 1012
History | View | Annotate | Download (1.68 KB)
1 |
#include <stdio.h> |
---|---|
2 |
#include <math.h> |
3 |
|
4 |
#define CUTOFF 120 |
5 |
#define ABS(x) (x<0?-x:x) |
6 |
#define TIME 1 /*sec*/ |
7 |
#define ROBOT_WIDTH 131 /*mm*/ |
8 |
|
9 |
/* 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 |
* (x,y) is some kind of absolute position in the "world", let's make (0,0) the top left of the "world"
|
12 |
* theta will an angle be between 0 - 2*Pi (0 being faces east)
|
13 |
* 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 |
int move (int *x, int *y, double *theta, int speed1, int speed2) { |
17 |
if (*theta < 0 || *theta > 2*M_PI) return 1; |
18 |
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 |
if (ABS(speed1) < CUTOFF) {
|
23 |
speed1 = 0;
|
24 |
} |
25 |
if (ABS(speed2) < CUTOFF) {
|
26 |
speed2 = 0;
|
27 |
} |
28 |
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 |
|
39 |
double newx = radius * sin(t);
|
40 |
double newy = radius - radius * cos(t);
|
41 |
|
42 |
*x += newx * cos(*theta); |
43 |
*y += newx * sin(*theta); |
44 |
|
45 |
*x += newy * - sin(*theta); |
46 |
*y += newy * cos(*theta); |
47 |
|
48 |
int divide = (t+*theta)/(2*M_PI); |
49 |
*theta = (t+*theta) - (2*M_PI)*divide;
|
50 |
if (*theta<0) *theta += 2 * M_PI; |
51 |
|
52 |
return 0; |
53 |
} |
54 |
|
55 |
int main () {
|
56 |
int x = 0; |
57 |
int y = 0; |
58 |
double t = 0.0; |
59 |
printf("%d %d %lf\n",x,y,t);
|
60 |
move(&x,&y,&t,150,100); |
61 |
printf("%d %d %lf\n",x,y,t);
|
62 |
return 0; |
63 |
} |