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

2 
#include <encoders.h> 
3 
#include <move.h> 
4 
#include <math.h> 
5 
#include <avr/interrupt.h> 
6 
#include <dragonfly_lib.h> 
7  
8 
void modify_velocity(void); 
9  
10  
11 
long lround(double d); 
12  
13 
char control_velocity = 0; 
14 
int encoder_ltm1, encoder_rtm1, move_speed, move_angle;

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

17 
long diff_x,diff_y, velocity, target_velocity, target_angular_velocity;

18  
19 
//Measures radian angle displacement from starting position. (initially 0)

20 
double angle;

21  
22 
/**

23 
* Retrieve the estimated x position. [mm]

24 
*/

25 
long odometry_dx(void){ 
26 
return diff_x;

27 
} 
28  
29 
/**

30 
* Retrieve the estimated y position. [mm]

31 
*/

32 
long odometry_dy(void){ 
33 
return diff_y;

34 
} 
35  
36 
/**

37 
* Retrieve the estimated angle [radians]

38 
*/

39 
double odometry_angle(void){ 
40 
return angle;

41 
} 
42 
/**

43 
* Retrieve the estimated velocity [mm / s]

44 
*/

45 
long odometry_velocity(void){ 
46 
return velocity;

47 
} 
48  
49 
void odometry_set_velocity(long t_velocity, double t_angular_velocity, int start_speed, int start_angle){ 
50 
target_velocity = t_velocity; 
51 
target_angular_velocity = t_angular_velocity; 
52 
move_angle = start_angle; 
53 
move_speed = start_speed; 
54 
control_velocity = 1;

55 
} 
56  
57 
/**

58 
* Initializes odometry to run on timer 2.

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

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

61 
*/

62 
void odometry_init(void){ 
63 

64 
encoders_init(); 
65 

66 
delay_ms(100);

67  
68 
odometry_reset(); 
69  
70 

71 
//CTC Mode. CLK / 1024

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

74  
75 
TIMSK = _BV(OCIE2); 
76  
77 
OCR2 = ODOMETRY_CLK; 
78 
} 
79 
/**

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

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

82 
*/

83 
void odometry_reset(void){ 
84 
diff_x = 0;

85 
diff_y = 0;

86 
encoder_ltm1 = encoder_read(LEFT); 
87 
encoder_rtm1 = encoder_read(RIGHT); 
88 
angle = 0.0; 
89 
} 
90  
91  
92 
void odometry_run(void){ 
93 
//Angle swept through in a time step CCW

94 
double theta, rl, dc;

95 
long dr,dl;

96 
char buf[100]; 
97 

98 
//Get the change in wheel positions

99 
{ 
100 
int encoder_right = encoder_read(RIGHT);

101 
int encoder_left = encoder_read(LEFT);

102 

103 
dl = encoder_left  encoder_ltm1; 
104 
dr = encoder_right  encoder_rtm1; 
105 

106 
//No motion.

107 
if(dl == 0 && dr == 0){ 
108 
if(control_velocity) modify_velocity();

109 
return;

110 
} 
111  
112 
encoder_ltm1 = encoder_left; 
113 
encoder_rtm1 = encoder_right; 
114 

115 
// Try to avoid over/underflow.

116 
dl = dl > 512 ? dl  1024 :dl; 
117 
dl = dl < 512 ? dl + 1024 :dl; 
118 
dr = dr > 512 ? dr  1024 :dr; 
119 
dr = dr < 512 ? dr + 1024 :dr; 
120 

121 
//Convert "clicks" to um

122 
dl *= CLICK_DISTANCE_UM; //um

123 
dr *= CLICK_DISTANCE_UM; 
124 
} 
125 

126  
127 
if(dl == dr){

128 
diff_x += lround(dl*cos(angle)/1000.0); //mm 
129 
diff_y += lround(dl*sin(angle)/1000.0); //mm 
130 
velocity = lround((dl * 1000.0)/(ODOMETRY_CLK*TIME_SCALE)); //um / ms = mm/s 
131 
if(control_velocity) modify_velocity();

132 
return;

133 
} 
134 

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

136 
rl = ((double)(ROBOT_WIDTH_UM*dl))/((double)(dl  dr)); //um 
137 
theta = ((double)(dl  dr))/((double)ROBOT_WIDTH_UM); //rad 
138 

139 
//Distance the center has traveled.

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

142 

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

144 
angle = ANGLE_SCALE * theta; 
145 

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

147 
diff_x += lround((DISTANCE_SCALE * dc * cos(angle))/1000.0); //mm 
148 
diff_y += lround((DISTANCE_SCALE * dc * sin(angle))/1000.0); //mm 
149  
150 
if(control_velocity) modify_velocity();

151 
} 
152  
153 
void modify_velocity(){

154 
usb_puts("modify_velocity\n");

155 
int diff = target_velocity  velocity;

156 
if(diff > 0){ 
157 
move_speed++; 
158 
move(move_speed,move_angle); 
159 
} 
160 
else if(diff < 0){ 
161 
move_speed; 
162 
move(move_speed,move_angle); 
163 
} 
164 
} 
165  
166 
ISR(TIMER2_COMP_vect){ 
167 
odometry_run(); 
168 
} 
169  
170 
long lround(double d){ 
171 
double f = floor(d);

172 
return (long)(d  f > 0.5 ? f + 1 : f); 
173 
} 
174  
175 