Revision 872
Small modifications to odometry. I'm all but committed to using floating point.
trunk/code/projects/mapping/odometry/odometry.c | ||
---|---|---|
1 | 1 |
#include "encoders.h" |
2 |
#include <math.h> |
|
2 | 3 |
|
3 | 4 |
//Odometry resolution, *64 microseconds. |
4 | 5 |
#define ODOMETRY_CLK=80 |
... | ... | |
8 | 9 |
//Distance per encoder click (circumference / 1024) = 203.621224 um. |
9 | 10 |
//Robot width = 5.3745 in. = 136.5123 mm |
10 | 11 |
|
11 |
#define ROBOT_WIDTH=137 //mm |
|
12 |
#define CLICK_DISTANCE=204 //um |
|
12 |
#define ROBOT_WIDTH_MM=137 //mm
|
|
13 |
#define CLICK_DISTANCE_UM=204 //um
|
|
13 | 14 |
|
14 | 15 |
//Standard measures will be mm and us |
15 | 16 |
|
16 | 17 |
int encoder_ltm1, encoder_rtm1; |
17 | 18 |
|
18 |
int diff_x,diff_y,angle; |
|
19 |
int diff_x,diff_y; |
|
20 |
double angle; |
|
19 | 21 |
|
20 | 22 |
void odometry_init(){ |
21 | 23 |
encoder_init(); |
... | ... | |
26 | 28 |
void odometry_reset(){ |
27 | 29 |
diff_x = 0; |
28 | 30 |
diff_y = 0; |
29 |
angle = 0; |
|
31 |
angle = 0.0;
|
|
30 | 32 |
|
31 | 33 |
//CTC Mode. CLK / 1024 |
32 | 34 |
TCCR3A = 0; // (Fully disconnected) |
... | ... | |
36 | 38 |
} |
37 | 39 |
|
38 | 40 |
void odometry_run(){ |
39 |
int v_l, v_r, omega; |
|
41 |
int v_l, v_r; |
|
42 |
double omega; |
|
40 | 43 |
int encoder_left, encoder_right; |
41 | 44 |
|
42 |
//TODO Handle the case where v_l = v_r |
|
43 | 45 |
encoder_left = encoder_read(LEFT); |
44 | 46 |
encoder_right = encoder_read(RIGHT); |
45 | 47 |
|
46 |
v_l = (CLICK_DISTANCE*(encoder_left - encoder_ltm1)) / (ODOMETRY_CLK * 64); // um / us = m/s |
|
47 |
v_r = (CLICK_DISTANCE*(encoder_right - encoder_rtm1)) / (ODOMETRY_CLK * 64); // same |
|
48 |
v_l = (CLICK_DISTANCE_UM*(encoder_left - encoder_ltm1)) / (ODOMETRY_CLK * 64); // um / us = m/s
|
|
49 |
v_r = (CLICK_DISTANCE_UM*(encoder_right - encoder_rtm1)) / (ODOMETRY_CLK * 64); // same
|
|
48 | 50 |
|
49 | 51 |
if(v_l == v_r){ |
50 | 52 |
diff_x += v_l * cos(angle); |
51 | 53 |
diff_y += v_r * sin(angle); |
54 |
return; |
|
52 | 55 |
} |
53 | 56 |
|
54 | 57 |
//Relative to the center of the robot, the center of it's turn. |
55 |
turning_radius = ROBOT_WIDTH * ((v_l + v_r) / (v_l - v_r)); // = mm |
|
58 |
turning_radius = ROBOT_WIDTH_MM * ((v_l + v_r) / (v_l - v_r)); // = mm
|
|
56 | 59 |
|
57 |
//Here non floating point operations get trickier.
|
|
60 |
//TODO I think I may have gotten lazy on the angular momentum units before, hard to tell now.
|
|
58 | 61 |
if(v_r > v_l) //CCW positive. |
59 |
omega = -v_r / (turning_radius + ROBOT_WIDTH/2);
|
|
62 |
omega = ((double)(-v_r)) / (turning_radius + ROBOT_WIDTH_MM>>1);
|
|
60 | 63 |
|
61 | 64 |
if(v_r > v_l) //CW negative. |
62 |
omega = -v_l / (turning_radius - ROBOT_WIDTH/2);
|
|
65 |
omega = ((double)(-v_l)) / (turning_radius - ROBOT_WIDTH_MM>>1);
|
|
63 | 66 |
|
64 | 67 |
//Omega positive whenever t_r is negative, vica versa. |
65 | 68 |
vmag = -omega * turning_radius; |
... | ... | |
67 | 70 |
angle += omega; // * the timer interval. |
68 | 71 |
diff_x += vmag * cos(angle); |
69 | 72 |
diff_y += vmag * sin(angle); |
70 |
|
|
71 |
|
|
72 | 73 |
} |
73 | 74 |
|
74 | 75 |
ISR(TIMER3_COMPA_vect){ |
Also available in: Unified diff