Revision 1456 branches/colonetmk2/code/projects/libdragonfly/odometry.c

View differences:

odometry.c
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