Project

General

Profile

Revision 1001

Added by Rich Hong almost 14 years ago

the function move will now update the position of the robot and its theta. This can be merged into libsim once tested

View differences:

test_motors.c
1 1
#include <stdio.h>
2
#include <math.h>
2 3

  
3
#define	FORWARD	  1
4
#define	BACKWARD  0
5

  
6 4
#define CUTOFF	  120
5
#define ABS(x) (x<0?-x:x)
6
#define TIME      1 /*sec*/
7
#define ROBOT_WIDTH 131 /*mm*/
7 8

  
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
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
10 11
 * (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
12
 * theta will an angle be between 0 - 2*Pi (0 being faces east)
13 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 14
 * move will return 0 if successful
16 15
 */
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;
16
int move (int *x, int *y, double *theta, int speed1, int speed2) {
17
  if (*theta < 0 || *theta > 2*M_PI) return 1;
21 18
  if (speed1 < 0 || speed1 > 255) return 1;
22 19
  if (speed2 < 0 || speed2 > 255) return 1;
23 20

  
24 21
  /* if speed is lower than the cut off, don't move */
25
  if (speed1 < CUTOFF) {
22
  if (ABS(speed1) < CUTOFF) {
26 23
    speed1 = 0;
27 24
  }
28
  if (speed2 < CUTOFF) {
25
  if (ABS(speed2) < CUTOFF) {
29 26
    speed2 = 0;
30 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;
31 38

  
32
  /* FAKE VALUES */
33
  (*x) += speed1-CUTOFF;
34
  (*y) += speed1-CUTOFF;
39
  double newx = radius * sin(t);
40
  double newy = radius - radius * cos(t);
35 41

  
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
   */
42
  *x += newx * cos(*theta);
43
  *y += newx * sin(*theta);
45 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

  
46 51
  return 0;
47 52
}
48 53

  
49 54
int main () {
50 55
  int x = 0;
51 56
  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);
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);
55 61
  return 0;
56 62
}

Also available in: Unified diff