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