Project

General

Profile

Revision 949

Added by Justin Scheiner over 15 years ago

Added a odometry_velocity function that gives approximate speed of the robot in mm/s.

View differences:

trunk/code/projects/mapping/odometry/odometry.c
10 10 
int encoder_ltm1, encoder_rtm1;
11 11 

  		




  12
  12
  
    
//Measure mm displacement in x and y from starting position. (+x:theta=0 +y:theta=M_PI/2)


  		




  13
  
  
    
long diff_x,diff_y;


  		




  
  13
  
    
long diff_x,diff_y, velocity;


  		




  14
  14
  
    

  		




  15
  15
  
    
//Measures radian angle displacement from starting position. (initially 0)


  		




  16
  16
  
    
double angle;


  		




  ......




  36
  36
  
    
	return angle;


  		




  37
  37
  
    
}


  		




  38
  38
  
    

  		




  
  39
  
    
long odometry_velocity(void){


  		




  
  40
  
    
	return velocity;


  		




  
  41
  
    
}


  		




  
  42
  
    

  		




  39
  43
  
    
/**


  		




  40
  44
  
    
 * Initializes odometry to run on timer 2.


  		




  41
  45
  
    
 * Also resets all values so that the center of the robot is 


  		




  ......




  75
  79
  
    
	//Angle swept through in a time step CCW-


  		




  76
  80
  
    
	double theta, rl, dc;


  		




  77
  81
  
    
	long dr,dl;


  		




  
  82
  
    
	char buf[100];


  		




  78
  83
  
    
	


  		




  79
  84
  
    
	//Get the change in wheel positions


  		




  80
  85
  
    
	{	


  		




  ......




  105
  110
  
    
	if(dl == dr){


  		




  106
  111
  
    
		diff_x += lround(dl*cos(angle)/1000.0); //mm


  		




  107
  112
  
    
		diff_y += lround(dl*sin(angle)/1000.0); //mm


  		




  
  113
  
    
		velocity = lround((dl * 1000.0)/(ODOMETRY_CLK*TIME_SCALE)); //um / ms = mm/s


  		




  
  114
  
    
		sprintf(buf,"dc: %ld, velocity: %ld\r\n mm/s",dl,velocity);


  		




  
  115
  
    
		usb_puts(buf);


  		




  108
  116
  
    
		return;


  		




  109
  117
  
    
	}


  		




  110
  118
  
    
	


  		




  ......




  113
  121
  
    
	theta = ((double)(dl - dr))/((double)ROBOT_WIDTH_UM);   //rad


  		




  114
  122
  
    
	


  		




  115
  123
  
    
	//Distance the center has traveled.


  		




  116
  
  
    
	dc = lround( (theta * (rl - ROBOT_WIDTH_UM)) / 2.0 ); //um


  		




  
  124
  
    
	dc = (theta * (rl - ROBOT_WIDTH_UM)) / 2.0; //um


  		




  
  125
  
    
	velocity = lround( dc * 1000 /(ODOMETRY_CLK*TIME_SCALE));


  		




  
  126
  
    
	sprintf(buf,"dc: %lfum, velocity: %ld\r\n mm/s",dc,velocity);


  		




  
  127
  
    
	usb_puts(buf);


  		




  117
  128
  
    
	


  		




  118
  129
  
    
	//angle is necessarily CCW+, so subtract.


  		




  119
  130
  
    
	angle -= ANGLE_SCALE * theta; 


  		




  120
  131
  
    
	


  		




  121
  
  
    
	//Change state variables.	


  		




  
  132
  
    
	//Change state variables. Probably lose all measurements less then a mm.


  		




  122
  133
  
    
	diff_x += lround((DISTANCE_SCALE * dc * cos(angle))/1000.0); //mm


  		




  123
  134
  
    
	diff_y += lround((DISTANCE_SCALE * dc * sin(angle))/1000.0); //mm


  		




  124
  135
  
    
}


  		












  

    
      trunk/code/projects/mapping/odometry/main.c
    
  





  8
  8
  
    
	odometry_init();


  		




  9
  9
  
    
	char buf[100];


  		




  10
  10
  
    
	while(1){


  		




  11
  
  
    
		sprintf(buf,"X: %ld, Y: %ld, Theta: %lf\n\r", odometry_dx(), odometry_dy(), odometry_angle());


  		




  12
  
  
    
		usb_puts(buf);


  		




  
  11
  
    
		//sprintf(buf,"X: %ld mm, Y: %ld mm, Theta: %lf rad Velocity: %ld mm/s\n\r", 


  		




  
  12
  
    
				//odometry_dx(), odometry_dy(), odometry_angle(), odometry_velocity());


  		




  
  13
  
    
		//usb_puts(buf);


  		




  13
  14
  
    
		delay_ms(500);


  		




  14
  15
  
    
	}


  		




  15
  16
  
    
	return 0;


  		












  

    
      trunk/code/projects/mapping/odometry/odometry.h
    
  





  11
  11
  
    
#ifndef __ODOMETRY_C__


  		




  12
  12
  
    
#define __ODOMETRY_C__


  		




  13
  13
  
    

  		




  
  14
  
    
/**


  		




  
  15
  
    
 * @addtogroup odometry 


  		




  
  16
  
    
 * @{


  		




  
  17
  
    
 **/


  		




  
  18
  
    

  		




  14
  19
  
    
//Odometry resolution, *64 microseconds.


  		




  15
  
  
    
//= approx. 100 ms


  		




  16
  20
  
    
#define ODOMETRY_CLK 255u 


  		




  
  21
  
    
#define TIME_SCALE 64


  		




  17
  22
  
    

  		




  18
  23
  
    
//Wheel = 2.613 in.  


  		




  19
  24
  
    
//Circumference = 208.508133 mm


  		




  ......




  23
  28
  
    
#define ROBOT_WIDTH_UM 137000  //um


  		




  24
  29
  
    
#define CLICK_DISTANCE_UM 204 //um


  		




  25
  30
  
    

  		




  26
  
  
    
#define DISTANCE_SCALE 2.10526316


  		




  27
  
  
    
#define ANGLE_SCALE 1.12823207


  		




  
  31
  
    
#define DISTANCE_SCALE 2.10526316 //Magic constant.


  		




  
  32
  
    
#define ANGLE_SCALE 1.12823207 //Magic constant.


  		




  28
  33
  
    

  		




  29
  
  
    
//Standard measures will be mm and us


  		




  30
  
  
    

  		




  31
  34
  
    
/** @brief Retrieve the robots estimated x position*/


  		




  32
  35
  
    
long odometry_dx(void);


  		




  
  36
  
    

  		




  33
  37
  
    
/** @brief Retrieve the robots estimated y position*/


  		




  34
  38
  
    
long odometry_dy(void);


  		




  
  39
  
    

  		




  35
  40
  
    
/** @brief Retrieve the robots estimated orientation*/


  		




  36
  41
  
    
double odometry_angle(void);


  		




  
  42
  
    

  		




  37
  43
  
    
/** @brief Initialize odometry. MUST be called before 


  		




  38
  44
  
    
 * the other functions work.**/


  		




  39
  45
  
    
void odometry_init(void);


  		




  
  46
  
    

  		




  40
  47
  
    
/** @brief Reset position and orientation to the origin facing


  		




  41
  48
  
    
 * the x axis.*/


  		




  42
  49
  
    
void odometry_reset(void);


  		




  43
  50
  
    

  		




  
  51
  
    
/** @brief Report estimated velocity [mm/s].*/


  		




  
  52
  
    
long odometry_velocity(void);


  		




  
  53
  
    

  		




  
  54
  
    
/**@}**/ //end group


  		




  
  55
  
    

  		




  44
  56
  
    
#endif


  		












  

    
      trunk/code/projects/libdragonfly/odometry.c
    
  





  10
  10
  
    
int encoder_ltm1, encoder_rtm1;


  		




  11
  11
  
    

  		




  12
  12
  
    
//Measure mm displacement in x and y from starting position. (+x:theta=0 +y:theta=M_PI/2)


  		




  13
  
  
    
long diff_x,diff_y;


  		




  
  13
  
    
long diff_x,diff_y, velocity;


  		




  14
  14
  
    

  		




  15
  15
  
    
//Measures radian angle displacement from starting position. (initially 0)


  		




  16
  16
  
    
double angle;


  		




  ......




  36
  36
  
    
	return angle;


  		




  37
  37
  
    
}


  		




  38
  38
  
    

  		




  
  39
  
    
long odometry_velocity(void){


  		




  
  40
  
    
	return velocity;


  		




  
  41
  
    
}


  		




  
  42
  
    

  		




  39
  43
  
    
/**


  		




  40
  44
  
    
 * Initializes odometry to run on timer 2.


  		




  41
  45
  
    
 * Also resets all values so that the center of the robot is 


  		




  ......




  75
  79
  
    
	//Angle swept through in a time step CCW-


  		




  76
  80
  
    
	double theta, rl, dc;


  		




  77
  81
  
    
	long dr,dl;


  		




  
  82
  
    
	char buf[100];


  		




  78
  83
  
    
	


  		




  79
  84
  
    
	//Get the change in wheel positions


  		




  80
  85
  
    
	{	


  		




  ......




  105
  110
  
    
	if(dl == dr){


  		




  106
  111
  
    
		diff_x += lround(dl*cos(angle)/1000.0); //mm


  		




  107
  112
  
    
		diff_y += lround(dl*sin(angle)/1000.0); //mm


  		




  
  113
  
    
		velocity = lround((dl * 1000.0)/(ODOMETRY_CLK*TIME_SCALE)); //um / ms = mm/s


  		




  
  114
  
    
		sprintf(buf,"dc: %ld, velocity: %ld\r\n mm/s",dl,velocity);


  		




  
  115
  
    
		usb_puts(buf);


  		




  108
  116
  
    
		return;


  		




  109
  117
  
    
	}


  		




  110
  118
  
    
	


  		




  ......




  113
  121
  
    
	theta = ((double)(dl - dr))/((double)ROBOT_WIDTH_UM);   //rad


  		




  114
  122
  
    
	


  		




  115
  123
  
    
	//Distance the center has traveled.


  		




  116
  
  
    
	dc = lround( (theta * (rl - ROBOT_WIDTH_UM)) / 2.0 ); //um


  		




  
  124
  
    
	dc = (theta * (rl - ROBOT_WIDTH_UM)) / 2.0; //um


  		




  
  125
  
    
	velocity = lround( dc * 1000 /(ODOMETRY_CLK*TIME_SCALE));


  		




  
  126
  
    
	sprintf(buf,"dc: %lfum, velocity: %ld\r\n mm/s",dc,velocity);


  		




  
  127
  
    
	usb_puts(buf);


  		




  117
  128
  
    
	


  		




  118
  129
  
    
	//angle is necessarily CCW+, so subtract.


  		




  119
  130
  
    
	angle -= ANGLE_SCALE * theta; 


  		




  120
  131
  
    
	


  		




  121
  
  
    
	//Change state variables.	


  		




  
  132
  
    
	//Change state variables. Probably lose all measurements less then a mm.


  		




  122
  133
  
    
	diff_x += lround((DISTANCE_SCALE * dc * cos(angle))/1000.0); //mm


  		




  123
  134
  
    
	diff_y += lround((DISTANCE_SCALE * dc * sin(angle))/1000.0); //mm


  		




  124
  135
  
    
}


  		












  

    
      trunk/code/projects/libdragonfly/odometry.h
    
  





  11
  11
  
    
#ifndef __ODOMETRY_C__


  		




  12
  12
  
    
#define __ODOMETRY_C__


  		




  13
  13
  
    

  		




  
  14
  
    
/**


  		




  
  15
  
    
 * @addtogroup odometry 


  		




  
  16
  
    
 * @{


  		




  
  17
  
    
 **/


  		




  
  18
  
    

  		




  14
  19
  
    
//Odometry resolution, *64 microseconds.


  		




  15
  
  
    
//= approx. 100 ms


  		




  16
  20
  
    
#define ODOMETRY_CLK 255u 


  		




  
  21
  
    
#define TIME_SCALE 64


  		




  17
  22
  
    

  		




  18
  23
  
    
//Wheel = 2.613 in.  


  		




  19
  24
  
    
//Circumference = 208.508133 mm


  		




  ......




  23
  28
  
    
#define ROBOT_WIDTH_UM 137000  //um


  		




  24
  29
  
    
#define CLICK_DISTANCE_UM 204 //um


  		




  25
  30
  
    

  		




  26
  
  
    
#define DISTANCE_SCALE 2.10526316


  		




  27
  
  
    
#define ANGLE_SCALE 1.12823207


  		




  
  31
  
    
#define DISTANCE_SCALE 2.10526316 //Magic constant.


  		




  
  32
  
    
#define ANGLE_SCALE 1.12823207 //Magic constant.


  		




  28
  33
  
    

  		




  29
  
  
    
//Standard measures will be mm and us


  		




  30
  
  
    

  		




  31
  34
  
    
/** @brief Retrieve the robots estimated x position*/


  		




  32
  35
  
    
long odometry_dx(void);


  		




  
  36
  
    

  		




  33
  37
  
    
/** @brief Retrieve the robots estimated y position*/


  		




  34
  38
  
    
long odometry_dy(void);


  		




  
  39
  
    

  		




  35
  40
  
    
/** @brief Retrieve the robots estimated orientation*/


  		




  36
  41
  
    
double odometry_angle(void);


  		




  
  42
  
    

  		




  37
  43
  
    
/** @brief Initialize odometry. MUST be called before 


  		




  38
  44
  
    
 * the other functions work.**/


  		




  39
  45
  
    
void odometry_init(void);


  		




  
  46
  
    

  		




  40
  47
  
    
/** @brief Reset position and orientation to the origin facing


  		




  41
  48
  
    
 * the x axis.*/


  		




  42
  49
  
    
void odometry_reset(void);


  		




  43
  50
  
    

  		




  
  51
  
    
/** @brief Report estimated velocity [mm/s].*/


  		




  
  52
  
    
long odometry_velocity(void);


  		




  
  53
  
    

  		




  
  54
  
    
/**@}**/ //end group


  		




  
  55
  
    

  		




  44
  56
  
    
#endif


  		












Also available in:  Unified diff