Revision 985
ticket #159:
main() is just for testing
no working code yet just comments, still need to figure out the math
branches/simulator/projects/simulator/core/test_motors.c  

1 
#include <stdio.h> 

2  
3 
#define FORWARD 1 

4 
#define BACKWARD 0 

5  
6 
#define CUTOFF 120 

7  
8 
/* move will move a robot from its initial position, (x,y), and theta (in degree) to a new position given dir, and speed. 

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

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

11 
* theta will an angle be between 0  359 (0 degree being faces east) 

12 
* dir is either FORWARD or BACKWARD 

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

14 
* 2 pairs of dir and speed, one for each motor 

15 
* move will return 0 if successful 

16 
*/ 

17 
int move (int *x, int *y, int theta, int dir1, int speed1, int dir2, int speed2) { 

18 
if (theta < 0  theta > 359) return 1; 

19 
if (dir1 != FORWARD && dir1 != BACKWARD) return 1; 

20 
if (dir2 != FORWARD && dir2 != BACKWARD) return 1; 

21 
if (speed1 < 0  speed1 > 255) return 1; 

22 
if (speed2 < 0  speed2 > 255) return 1; 

23  
24 
/* if speed is lower than the cut off, don't move */ 

25 
if (speed1 < CUTOFF) { 

26 
speed1 = 0; 

27 
} 

28 
if (speed2 < CUTOFF) { 

29 
speed2 = 0; 

30 
} 

31  
32 
/* FAKE VALUES */ 

33 
(*x) += speed1CUTOFF; 

34 
(*y) += speed1CUTOFF; 

35  
36 
/* d = distance between the two wheels = 12 cm 

37 
* v1 = speed of the first wheel 

38 
* v2 = speed of the second wheel 

39 
* 

40 
* Unless v1 = v2, if left at that point, the robot will go in a circle at some radius. 

41 
* 

42 
* This radius is = dv1/(v1v2). 

43 
* Since we could measure the displacement of a wheel, we could figure out what the overall angular displacement is using r(theta) = arc length and then use trig after that to figure out x, y coordinate displacement. 

44 
*/ 

45  
46 
return 0; 

47 
} 

48  
49 
int main () { 

50 
int x = 0; 

51 
int y = 0; 

52 
printf("%d %d\n",x,y); 

53 
move(&x,&y,0,FORWARD,150,FORWARD,150); 

54 
printf("%d %d\n",x,y); 

55 
return 0; 

56 
} 
Also available in: Unified diff