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:

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
  
    
}


  		












Also available in:  Unified diff