Project

General

Profile

Revision 1001

Added by Rich Hong about 15 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