Project

General

Profile

Revision 926

Added by Justin Scheiner over 15 years ago

Odometry committed. New version is more general, but still drifts to zero.

View differences:

trunk/code/projects/mapping/odometry/odometry.c
25 25 
void odometry_init(void){
26 26
27 27 
	encoders_init();
28
29 
	delay_ms(100);
28 30 

  		




  29
  31
  
    
	odometry_reset();


  		




  30
  32
  
    

  		




  ......




  44
  46
  
    
	diff_y = 0;


  		




  45
  47
  
    
	encoder_ltm1 = encoder_read(LEFT);


  		




  46
  48
  
    
	encoder_rtm1 = encoder_read(RIGHT);


  		




  47
  
  
    
	sprintf(buf,"(en_right,en_left) in um: (%d,%d)\n\r",encoder_ltm1,encoder_rtm1);


  		




  48
  
  
    
	usb_puts(buf);


  		




  
  49
  
    
	//sprintf(buf,"(en_right,en_left) in um: (%d,%d)\n\r",encoder_ltm1,encoder_rtm1);


  		




  
  50
  
    
	//usb_puts(buf);


  		




  49
  51
  
    
	angle = 0.0;


  		




  50
  52
  
    
}


  		




  51
  53
  
    

  		




  ......




  112
  114
  
    
	//Angle swept through in a time step CCW-


  		




  113
  115
  
    
	double theta, rl, dc;


  		




  114
  116
  
    
	


  		




  115
  
  
    
	long dr,dl;


  		




  
  117
  
    
	double dr,dl;


  		




  116
  118
  
    
	


  		




  117
  119
  
    
	char buf[40];	


  		




  118
  120
  
    
	//Relative to the inner wheel, turn radius.


  		




  ......




  122
  124
  
    
	int encoder_right = encoder_read(RIGHT);	


  		




  123
  125
  
    
	int encoder_left = encoder_read(LEFT);	


  		




  124
  126
  
    
	


  		




  125
  
  
    
	sprintf(buf,"(en_right,en_left) in um: (%d,%d)\n\r",encoder_left,encoder_right);


  		




  126
  
  
    
	usb_puts(buf);


  		




  127
  
  
    
	


  		




  128
  127
  
    
	dl = encoder_left - encoder_ltm1;


  		




  129
  128
  
    
	dr = encoder_right - encoder_rtm1;


  		




  130
  129
  
    
	


  		




  131
  
  
    
	if(dl >=-1 && dl <= 1  && dr >= -1 && dr <= 1) return;


  		




  
  130
  
    
	//if(dl >=-1 && dl <= 1  && dr >= -1 && dr <= 1) return;


  		




  132
  131
  
    
	


  		




  133
  132
  
    
	encoder_ltm1 = encoder_left;


  		




  134
  133
  
    
	encoder_rtm1 = encoder_right;


  		




  135
  134
  
    
	


  		




  136
  
  
    
	//sprintf(buf,"(dl,dr) in clicks: (%ld,%ld)\n\r",dl,dr);


  		




  137
  
  
    
	//usb_puts(buf);


  		




  138
  
  
    
	


  		




  139
  135
  
    
	// Try to avoid over/underflow.


  		




  140
  136
  
    
	dl = dl > 512 ? dl - 1024 :dl;


  		




  141
  137
  
    
	dl = dl < -512 ? dl + 1024 :dl;


  		




  142
  138
  
    
	dr = dr > 512 ? dr - 1024 :dr;


  		




  143
  139
  
    
	dr = dr < -512 ? dr + 1024 :dr;


  		




  144
  140
  
    
	


  		




  145
  
  
    
	sprintf(buf,"(dl,dr) in um: (%ld,%ld)\n\r",dl,dr);


  		




  146
  
  
    
	usb_puts(buf);


  		




  147
  141
  
    
	//Convert "clicks" to um


  		




  148
  142
  
    
	dl *= CLICK_DISTANCE_UM; //um


  		




  149
  143
  
    
	dr *= CLICK_DISTANCE_UM;


  		




  ......




  152
  146
  
    
		//usb_putc('#');


  		




  153
  147
  
    
		diff_x += dl*cos(angle)/1000; 


  		




  154
  148
  
    
		diff_y += dl*sin(angle)/1000; 


  		




  155
  
  
    
		//sprintf(buf,"Distance in um: %ld\n\r",dl);


  		




  156
  
  
    
		//usb_puts(buf);


  		




  157
  149
  
    
		return;


  		




  158
  150
  
    
	}


  		




  159
  
  
    
	//usb_putc('*');	


  		




  160
  
  
    
	//sprintf(buf,"(dl,dr) in um: (%ld,%ld)\n\r",dl,dr);


  		




  161
  
  
    
	//usb_puts(buf);


  		




  162
  151
  
    
	


  		




  163
  152
  
    
	//Call the left wheel 0, clockwise positive.


  		




  164
  
  
    

  		




  165
  153
  
    
	rl = ((double)(ROBOT_WIDTH_UM*dl))/((double)(dl - dr)); //um


  		




  166
  154
  
    
	


  		




  167
  
  
    
	//sprintf(buf,"Radius from left wheel: %ld\n\r",rl);


  		




  168
  
  
    
	//usb_puts(buf);


  		




  169
  
  
    
	


  		




  170
  155
  
    
	theta = ((double)(dl - dr))/((double)ROBOT_WIDTH_UM); //rad


  		




  171
  156
  
    

  		




  172
  157
  
    
	dc = (rl - ROBOT_WIDTH_UM/2)*theta; //um


  		




  173
  158
  
    

  		




  174
  
  
    
	//sprintf(buf,"Distance in um: %ld\n\r",dc);


  		




  175
  
  
    
	//usb_puts(buf);


  		




  176
  
  
    
	


  		




  177
  159
  
    
	diff_x += ((dc * cos(angle))/1000); //mm


  		




  178
  160
  
    
	diff_y += ((dc * sin(angle))/1000); //mm


  		




  179
  161
  
    
		


  		




  180
  
  
    
	//sprintf(buf,"(dx, dy): (%ld, %ld)\n\r",diff_x, diff_y);


  		




  181
  
  
    
	//usb_puts(buf);


  		




  182
  
  
    

  		




  183
  162
  
    
	//Change state variables.	


  		




  184
  163
  
    
	


  		




  185
  164
  
    
	//angle is necessarily CCW+, so subtract.


  		












  

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





  7
  7
  
    
	odometry_init();


  		




  8
  8
  
    
	char buf[100];


  		




  9
  9
  
    
	while(1){


  		




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


  		




  11
  
  
    
		//usb_puts(buf);


  		




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


  		




  
  11
  
    
		usb_puts(buf);


  		




  12
  12
  
    
		delay_ms(500);


  		




  13
  13
  
    
	}


  		




  14
  14
  
    
	return 0;


  		












Also available in:  Unified diff