Revision 1456
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