Project

General

Profile

Revision 985

Added by Rich Hong about 12 years ago

ticket #159:
main() is just for testing
no working code yet just comments, still need to figure out the math

View differences:

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) += 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
}

Also available in: Unified diff