root / trunk / code / projects / mapping / odometry / odometry.c @ 890
History  View  Annotate  Download (1.71 KB)
1 
#include "odometry.h" 

2 
#include "encoders.h" 
3 
#include <math.h> 
4 
#include <avr/interrupt.h> 
5 
#include <dragonfly_lib.h> 
6  
7 
int encoder_ltm1, encoder_rtm1;

8  
9 
int diff_x,diff_y;

10 
float angle;

11  
12  
13 
int odometry_dx(void){ 
14 
return diff_x;

15 
} 
16  
17 
int odometry_dy(void){ 
18 
return diff_y;

19 
} 
20  
21 
float odometry_angle(void){ 
22 
return angle;

23 
} 
24  
25 
void odometry_init(void){ 
26 
encoder_init(); 
27 
odometry_reset(); 
28 

29 
//CTC Mode. CLK / 1024

30 
TCCR2 = 0; // (Fully disconnected) 
31 
TCCR2 = _BV(WGM21)  _BV(CS22)  _BV(CS20); //CLK/1024 , CTC Mode.

32  
33 
TIMSK = _BV(OCIE2); 
34  
35 
OCR2 = ODOMETRY_CLK; 
36 
} 
37  
38 
void odometry_reset(void){ 
39 
diff_x = 0;

40 
diff_y = 0;

41 
angle = 0.0; 
42 
} 
43  
44 
void odometry_run(void){ 
45 
int v_l, v_r;

46 
float omega;

47 
int encoder_left, encoder_right;

48 

49 
encoder_left = encoder_read(LEFT); 
50 
encoder_right = encoder_read(RIGHT); 
51 

52 
v_l = (CLICK_DISTANCE_UM*(encoder_left  encoder_ltm1)) / (ODOMETRY_CLK * 64); // um / us = m/s 
53 
v_r = (CLICK_DISTANCE_UM*(encoder_right  encoder_rtm1)) / (ODOMETRY_CLK * 64); // same 
54  
55 
if(v_l == v_r){

56 
diff_x += v_l * cos(angle); 
57 
diff_y += v_r * sin(angle); 
58 
return;

59 
} 
60 

61 
//Relative to the center of the robot, the center of it's turn.

62 
float turning_radius = ROBOT_WIDTH_MM * ((v_l + v_r) / (v_l  v_r)); // = mm 
63  
64 
//TODO I think I may have gotten lazy on the angular momentum units before, hard to tell now.

65 
if(v_r > v_l) //CCW positive. 
66 
omega = ((float)(v_r)) / (turning_radius + (ROBOT_WIDTH_MM>>1)); 
67 

68 
else //CW negative. 
69 
omega = ((float)(v_l)) / (turning_radius  (ROBOT_WIDTH_MM>>1)); 
70 

71 
//Omega positive whenever t_r is negative, vica versa.

72 
float vmag = omega * turning_radius;

73  
74 
angle += omega; // * the timer interval.

75 
diff_x += vmag * cos(angle); 
76 
diff_y += vmag * sin(angle); 
77 
} 
78  
79 
ISR(TIMER2_COMP_vect){ 
80 
odometry_run(); 
81 
} 
82  
83 