root / branches / colonetmk2 / code / projects / libdragonfly / odometry.c @ 1456
History  View  Annotate  Download (3.24 KB)
1 
#include "odometry.h" 

2 
#include "encoders.h" 
3 
#include <math.h> 
4 
#include <avr/interrupt.h> 
5 
#include "time.h" 
6 
#include "serial.h" 
7  
8 
long lround(double d); 
9  
10  
11 
int encoder_ltm1, encoder_rtm1;

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

14 
long diff_x,diff_y, velocity;

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

17 
double angle;

18  
19 
/**

20 
* Retrieve the estimated x position. [millimeters]

21 
*/

22 
long odometry_dx(void){ 
23 
return diff_x;

24 
} 
25  
26 
/**

27 
* Retrieve the estimated y position. [millimeters]

28 
*/

29 
long odometry_dy(void){ 
30 
return diff_y;

31 
} 
32  
33 
/**

34 
* Retrieve the estimated angle [radians]

35 
*/

36 
double odometry_angle(void){ 
37 
return angle;

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

42 
} 
43  
44 
/**

45 
* Initializes odometry to run on timer 2.

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

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

48 
*/

49 
void odometry_init(void){ 
50 

51 
encoders_init(); 
52 

53 
delay_ms(100);

54  
55 
odometry_reset(); 
56  
57 

58 
//CTC Mode. CLK / 1024

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

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

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

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

69 
*/

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

72 
diff_y = 0;

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

81 
double theta, rl, dc;

82 
long dr,dl;

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 
usb_puts("dc: ");

115 
usb_puti(dl); 
116 
usb_puts("um, velocity: ");

117 
usb_puti(velocity); 
118 
usb_puts("\r\n mm/s");

119 
return;

120 
} 
121 

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

123 
rl = ((double)(ROBOT_WIDTH_UM*dl))/((double)(dl  dr)); //um 
124 
theta = ((double)(dl  dr))/((double)ROBOT_WIDTH_UM); //rad 
125 

126 
//Distance the center has traveled.

127 
dc = (theta * (rl  ROBOT_WIDTH_UM)) / 2.0; //um 
128 
velocity = lround( dc * 1000 /(ODOMETRY_CLK*TIME_SCALE));

129 
usb_puts("dc: ");

130 
usb_puti(dl); 
131 
usb_puts("um, velocity: ");

132 
usb_puti(velocity); 
133 
usb_puts("\r\n mm/s");

134 

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

136 
angle = ANGLE_SCALE * theta; 
137 

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

139 
diff_x += lround((DISTANCE_SCALE * dc * cos(angle))/1000.0); //mm 
140 
diff_y += lround((DISTANCE_SCALE * dc * sin(angle))/1000.0); //mm 
141 
} 
142  
143 
ISR(TIMER2_COMP_vect){ 
144 
odometry_run(); 
145 
} 
146  
147 
long lround(double d){ 
148 
double f = floor(d);

149 
return (long)(d  f > 0.5 ? f + 1 : f); 
150 
} 
151  
152 