Project

General

Profile

Revision 1027

Added by Rich Hong about 15 years ago

use abs in stdlib instead of a crappy macro

View differences:

branches/simulator/projects/simulator/simulator/core/motion.c
1 1 
#include <stdio.h>
2 
#include <stdlib.h>
2 3 
#include <math.h>
3 4 

  		




  4
  5
  
    
#include "motion.h"


  		




  5
  6
  
    

  		




  6
  7
  
    
#include "robot.h"


  		




  7
  8
  
    

  		




  8
  
  
    
#define CUTOFF		120


  		




  9
  
  
    
#define ABS(x) (x<0?-x:x)


  		




  10
  
  
    
#define TIME			1 /*sec*/


  		




  
  9
  
    
#define CUTOFF 120


  		




  
  10
  
    
#define TIME 1 /*sec*/


  		




  11
  11
  
    
#define ROBOT_WIDTH 131 /*mm*/


  		




  12
  12
  
    

  		




  13
  13
  
    
#define MOTOR_CONVERSION_FACTOR 10.0


  		




  14
  14
  
    

  		




  15
  
  
    
/** move will move a robot from its initial position, (x,y), and theta (in radians) to a new position given speed.


  		




  16
  
  
    
 * (x,y) and theta will be updated by the move function instead of returning a value


  		




  17
  
  
    
 * (x,y) is some kind of absolute position in the "world", let's make (0,0) the top left of the "world"


  		




  18
  
  
    
 * theta will an angle be between 0 - 2*Pi (0 being faces east)


  		




  19
  
  
    
 * speed is between 0 - 255, there is some magical cutoff point before the motors actually starts running


  		




  
  15
  
    
/** move_robot will move a robot from its initial position, (x,y), and theta (in radians) to a new position given speed.


  		




  
  16
  
    
 * (x,y) and theta will be updated by the move_robot function instead of returning a value


  		




  
  17
  
    
 * (x,y) is some kind of absolute position in the "world", (0,0) is the top left of the "world"


  		




  
  18
  
    
 * theta will an angle be between 0 and 2*Pi (0 being faces east and goes clockwise)


  		




  
  19
  
    
 * speed is between 0 and 255, there is some magical cutoff point before the motors actually starts running


  		




  20
  20
  
    
 * move will return 0 if successful


  		




  21
  21
  
    
 **/


  		




  22
  22
  
    
int move_robot(Robot* r)


  		




  ......




  30
  30
  
    
	if (speed2 < 0 || speed2 > 255) return 1;


  		




  31
  31
  
    

  		




  32
  32
  
    
	/* if speed is lower than the cut off, don't move */


  		




  33
  
  
    
	if (ABS(speed1) < CUTOFF) {


  		




  
  33
  
    
	if (abs(speed1) < CUTOFF) {


  		




  34
  34
  
    
		speed1 = 0;


  		




  35
  35
  
    
	}


  		




  36
  
  
    
	if (ABS(speed2) < CUTOFF) {


  		




  
  36
  
    
	if (abs(speed2) < CUTOFF) {


  		




  37
  37
  
    
		speed2 = 0;


  		




  38
  38
  
    
	}


  		




  39
  39
  
    
	speed1 = (speed1 - CUTOFF);


  		




  40
  40
  
    
	speed2 = (speed2 - CUTOFF);


  		




  41
  41
  
    
	double radius;


  		




  42
  42
  
    
	if (speed1 == speed2) {


  		




  43
  
  
    
		/* go straight */


  		




  44
  
  
    
	r->pose.x += cos(theta) * speed1 / MOTOR_CONVERSION_FACTOR;


  		




  45
  
  
    
	r->pose.y += sin(theta) * speed1 / MOTOR_CONVERSION_FACTOR;


  		




  46
  
  
    
		return 0;


  		




  
  43
  
    
	  /* go straight */


  		




  
  44
  
    
	  r->pose.x += cos(theta) * speed1 / MOTOR_CONVERSION_FACTOR;


  		




  
  45
  
    
	  r->pose.y += sin(theta) * speed1 / MOTOR_CONVERSION_FACTOR;


  		




  
  46
  
    
	  return 0;


  		




  47
  47
  
    
	}


  		




  48
  48
  
    
	radius = ROBOT_WIDTH * speed1 / (speed1 - speed2);


  		




  49
  49
  
    
	


  		












Also available in:  Unified diff