root / branches / simulator / projects / simulator / core / test_motors.c @ 985
History | View | Annotate | Download (1.82 KB)
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) += speed1-CUTOFF; |
34 |
(*y) += speed1-CUTOFF; |
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/(v1-v2).
|
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 |
} |