root / branches / simulator / projects / simulator / test / test_motors.c @ 1001
History  View  Annotate  Download (1.65 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 
*theta = fmod((t + *theta), (2 * M_PI));

49 
if (*theta<0) *theta += 2 * M_PI; 
50  
51 
return 0; 
52 
} 
53  
54 
int main () {

55 
int x = 0; 
56 
int y = 0; 
57 
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 
return 0; 
62 
} 