## Revision 1001

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