Project

General

Profile

Statistics
| Revision:

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
}