root / branches / encoders / code / projects / odometry / odometry.c @ 846
History | View | Annotate | Download (1.98 KB)
1 |
#include "odometry.h" |
---|---|
2 |
|
3 |
|
4 |
#include <math.h> |
5 |
#include "encoders.h" |
6 |
|
7 |
//Odometry resolution, *64 microseconds.
|
8 |
#define ODOMETRY_CLK 80 |
9 |
|
10 |
//Wheel = 2.613 in.
|
11 |
//Circumference = 208.508133 mm
|
12 |
//Distance per encoder click (circumference / 1024) = 203.621224 um.
|
13 |
//Robot width = 5.3745 in. = 136.5123 mm
|
14 |
|
15 |
#define ROBOT_WIDTH_MM 137 //mm |
16 |
#define CLICK_DISTANCE_UM 204 //um |
17 |
|
18 |
//Standard measures will be mm and us
|
19 |
|
20 |
int encoder_ltm1, encoder_rtm1;
|
21 |
|
22 |
int diff_x,diff_y;
|
23 |
double angle;
|
24 |
|
25 |
int odometry_getx(void){ |
26 |
return diff_x;
|
27 |
} |
28 |
|
29 |
int odometry_gety(void){ |
30 |
return diff_y;
|
31 |
} |
32 |
|
33 |
int odometry_get_angle(void){ |
34 |
return angle;
|
35 |
} |
36 |
|
37 |
void odometry_init(void){ |
38 |
encoders_init(); |
39 |
|
40 |
odometry_reset(); |
41 |
|
42 |
} |
43 |
|
44 |
void odometry_reset(void){ |
45 |
diff_x = 0;
|
46 |
diff_y = 0;
|
47 |
angle = 0;
|
48 |
|
49 |
//CTC Mode. CLK / 1024
|
50 |
TCCR3A = 0; // (Fully disconnected) |
51 |
TCCR3B = _BV(WGM32) | _BV(CS32) | _BV(CS30); //CLK/1024 , CTC Mode.
|
52 |
|
53 |
OCR3A = ODOMETRY_CLK; |
54 |
} |
55 |
|
56 |
void odometry_run(void){ |
57 |
int v_l, v_r;
|
58 |
double omega;
|
59 |
int encoder_left, encoder_right;
|
60 |
|
61 |
encoder_left = encoder_read(LEFT); |
62 |
encoder_right = encoder_read(RIGHT); |
63 |
|
64 |
v_l = (CLICK_DISTANCE_UM*(encoder_left - encoder_ltm1)) / (ODOMETRY_CLK * 64); // um / us = m/s |
65 |
v_r = (CLICK_DISTANCE_UM*(encoder_right - encoder_rtm1)) / (ODOMETRY_CLK * 64); // same |
66 |
|
67 |
if(v_l == v_r){
|
68 |
diff_x += v_l * cos(angle); |
69 |
diff_y += v_r * sin(angle); |
70 |
return;
|
71 |
} |
72 |
|
73 |
//Relative to the center of the robot, the center of it's turn.
|
74 |
int turning_radius = ROBOT_WIDTH_MM * ((v_l + v_r) / (v_l - v_r)); // = mm |
75 |
|
76 |
//Here non floating point operations get trickier.
|
77 |
if(v_r > v_l) //CCW positive. |
78 |
omega = ((double)-v_r) / ((double)(turning_radius + ROBOT_WIDTH_MM/2)); |
79 |
|
80 |
if(v_r > v_l) //CW negative. |
81 |
omega = ((double)-v_l) / ((double)(turning_radius - ROBOT_WIDTH_MM/2)); |
82 |
|
83 |
//Omega positive whenever t_r is negative, vica versa.
|
84 |
int vmag = -omega * turning_radius;
|
85 |
|
86 |
angle += omega; // * the timer interval.
|
87 |
diff_x += ((double)vmag) * cos(angle);
|
88 |
diff_y += ((double)vmag) * sin(angle);
|
89 |
} |
90 |
|
91 |
ISR(TIMER3_COMPA_vect){ |
92 |
odometry_run(); |
93 |
} |