root / branches / simulator / projects / simulator / simulator / core / motion.c @ 1006
History  View  Annotate  Download (1.52 KB)
1 
#include <stdio.h> 

2 
#include <math.h> 
3  
4 
#include "motion.h" 
5  
6 
#define CUTOFF 120 
7 
#define ABS(x) (x<0?x:x) 
8 
#define TIME 1 /*sec*/ 
9 
#define ROBOT_WIDTH 131 /*mm*/ 
10  
11 
/** move will move a robot from its initial position, (x,y), and theta (in radians) to a new position given speed.

12 
* (x,y) and theta will be updated by the move function instead of returning a value

13 
* (x,y) is some kind of absolute position in the "world", let's make (0,0) the top left of the "world"

14 
* theta will an angle be between 0  2*Pi (0 being faces east)

15 
* speed is between 0  255, there is some magical cutoff point before the motors actually starts running

16 
* move will return 0 if successful

17 
**/

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; 
22  
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;

40  
41 
double newx = radius * sin(t);

42 
double newy = radius  radius * cos(t);

43  
44 
*x += newx * cos(*theta); 
45 
*y += newx * sin(*theta); 
46  
47 
*x += newy *  sin(*theta); 
48 
*y += newy * cos(*theta); 
49  
50 
*theta = fmod((t + *theta), (2 * M_PI));

51 
if (*theta<0) *theta += 2 * M_PI; 
52  
53 
return 0; 
54 
} 
55 