Revision 1452 trunk/code/projects/libdragonfly/odometry.c

odometry.c (revision 1452)
1 1 
#include "odometry.h"
2 
#include <encoders.h>
2 
#include "encoders.h"
3 3 
#include <math.h>
4 4 
#include <avr/interrupt.h>
5 
#include <dragonfly_lib.h>
5 
#include "time.h"
6 
#include "serial.h"
6 7 

  		
  




  7
  8
    
    
long lround(double d);


  		
  




  8
  9
    
    

  		
  




......




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


  		
  




  80
  81
    
    
	double theta, rl, dc;


  		
  




  81
  82
    
    
	long dr,dl;


  		
  




  82
  
    
    
	char buf[100];


  		
  




  83
  83
    
    
	


  		
  




  84
  84
    
    
	//Get the change in wheel positions


  		
  




  85
  85
    
    
	{	


  		
  




......




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


  		
  




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


  		
  




  113
  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);


  		
  




  
  114
    
    
    usb_puts("dc: ");


  		
  




  
  115
    
    
    usb_puti(dl);


  		
  




  
  116
    
    
    usb_puts("um, velocity: ");


  		
  




  
  117
    
    
    usb_puti(velocity);


  		
  




  
  118
    
    
		usb_puts("\r\n mm/s");


  		
  




  116
  119
    
    
		return;


  		
  




  117
  120
    
    
	}


  		
  




  118
  121
    
    
	


  		
  




......




  123
  126
    
    
	//Distance the center has traveled.


  		
  




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


  		
  




  125
  128
    
    
	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);


  		
  




  
  129
    
    
	usb_puts("dc: ");


  		
  




  
  130
    
    
  usb_puti(dl);


  		
  




  
  131
    
    
  usb_puts("um, velocity: ");


  		
  




  
  132
    
    
  usb_puti(velocity);


  		
  




  
  133
    
    
  usb_puts("\r\n mm/s");


  		
  




  128
  134
    
    
	


  		
  




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


  		
  




  130
  136
    
    
	angle -= ANGLE_SCALE * theta; 


  		
  












Also available in:
	Unified diff