root / branches / encoders / code / projects / libdragonfly / odometry.c @ 1345
History  View  Annotate  Download (3.17 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 
long lround(double d); 
8  
9  
10 
int encoder_ltm1, encoder_rtm1;

11  
12 
//Measure mm displacement in x and y from starting position. (+x:theta=0 +y:theta=M_PI/2)

13 
long diff_x,diff_y, velocity;

14  
15 
//Measures radian angle displacement from starting position. (initially 0)

16 
double angle;

17  
18 
/**

19 
* Retrieve the estimated x position. [millimeters]

20 
*/

21 
long odometry_dx(void){ 
22 
return diff_x;

23 
} 
24  
25 
/**

26 
* Retrieve the estimated y position. [millimeters]

27 
*/

28 
long odometry_dy(void){ 
29 
return diff_y;

30 
} 
31  
32 
/**

33 
* Retrieve the estimated angle [radians]

34 
*/

35 
double odometry_angle(void){ 
36 
return angle;

37 
} 
38  
39 
long odometry_velocity(void){ 
40 
return velocity;

41 
} 
42  
43 
/**

44 
* Initializes odometry to run on timer 2.

45 
* Also resets all values so that the center of the robot is

46 
* considered to be at the origin facing the x direction.

47 
*/

48 
void odometry_init(void){ 
49 

50 
encoders_init(); 
51 

52 
delay_ms(100);

53  
54 
odometry_reset(); 
55  
56 

57 
//CTC Mode. CLK / 1024

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

60  
61 
TIMSK = _BV(OCIE2); 
62  
63 
OCR2 = ODOMETRY_CLK; 
64 
} 
65 
/**

66 
* Resets all values so that the center of the robot is

67 
* considered to be at the origin facing the x direction.

68 
*/

69 
void odometry_reset(void){ 
70 
diff_x = 0;

71 
diff_y = 0;

72 
encoder_ltm1 = encoder_read(LEFT); 
73 
encoder_rtm1 = encoder_read(RIGHT); 
74 
angle = 0.0; 
75 
} 
76  
77  
78 
void odometry_run(void){ 
79 
//Angle swept through in a time step CCW

80 
double theta, rl, dc;

81 
long dr,dl;

82 
char buf[100]; 
83 

84 
//Get the change in wheel positions

85 
{ 
86 
int encoder_right = encoder_read(RIGHT);

87 
int encoder_left = encoder_read(LEFT);

88 

89 
dl = encoder_left  encoder_ltm1; 
90 
dr = encoder_right  encoder_rtm1; 
91 

92 
//No motion.

93 
if(dl == 0 && dr == 0) return; 
94  
95 
encoder_ltm1 = encoder_left; 
96 
encoder_rtm1 = encoder_right; 
97 

98 
// Try to avoid over/underflow.

99 
dl = dl > 512 ? dl  1024 :dl; 
100 
dl = dl < 512 ? dl + 1024 :dl; 
101 
dr = dr > 512 ? dr  1024 :dr; 
102 
dr = dr < 512 ? dr + 1024 :dr; 
103 

104 
//Convert "clicks" to um

105 
dl *= CLICK_DISTANCE_UM; //um

106 
dr *= CLICK_DISTANCE_UM; 
107 
} 
108 

109  
110 
if(dl == dr){

111 
diff_x += lround(dl*cos(angle)/1000.0); //mm 
112 
diff_y += lround(dl*sin(angle)/1000.0); //mm 
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); 
116 
return;

117 
} 
118 

119 
//Call the left wheel 0, clockwise positive.

120 
rl = ((double)(ROBOT_WIDTH_UM*dl))/((double)(dl  dr)); //um 
121 
theta = ((double)(dl  dr))/((double)ROBOT_WIDTH_UM); //rad 
122 

123 
//Distance the center has traveled.

124 
dc = (theta * (rl  ROBOT_WIDTH_UM)) / 2.0; //um 
125 
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); 
128 

129 
//angle is necessarily CCW+, so subtract.

130 
angle = ANGLE_SCALE * theta; 
131 

132 
//Change state variables. Probably lose all measurements less then a mm.

133 
diff_x += lround((DISTANCE_SCALE * dc * cos(angle))/1000.0); //mm 
134 
diff_y += lround((DISTANCE_SCALE * dc * sin(angle))/1000.0); //mm 
135 
} 
136  
137 
ISR(TIMER2_COMP_vect){ 
138 
odometry_run(); 
139 
} 
140  
141 
long lround(double d){ 
142 
double f = floor(d);

143 
return (long)(d  f > 0.5 ? f + 1 : f); 
144 
} 
145  
146 