root / trunk / code / projects / mapping / matlab / testRobot / odometry.c @ 953
History  View  Annotate  Download (3.75 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 
long lround(double d); 
9  
10 
char control_velocity = 0; 
11 
int encoder_ltm1, encoder_rtm1, move_speed, move_angle;

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, target_velocity, target_angular_velocity;

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

17 
double angle;

18  
19 
/**

20 
* Retrieve the estimated x position. [mm]

21 
*/

22 
long odometry_dx(void){ 
23 
return diff_x;

24 
} 
25  
26 
/**

27 
* Retrieve the estimated y position. [mm]

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 
* Retrieve the estimated velocity [mm / s]

41 
*/

42 
long odometry_velocity(void){ 
43 
return velocity;

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

52 
} 
53  
54 
/**

55 
* Initializes odometry to run on timer 2.

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

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

58 
*/

59 
void odometry_init(void){ 
60 

61 
encoders_init(); 
62 

63 
delay_ms(100);

64  
65 
odometry_reset(); 
66  
67 

68 
//CTC Mode. CLK / 1024

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

71  
72 
TIMSK = _BV(OCIE2); 
73  
74 
OCR2 = ODOMETRY_CLK; 
75 
} 
76 
/**

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

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

79 
*/

80 
void odometry_reset(void){ 
81 
diff_x = 0;

82 
diff_y = 0;

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

91 
double theta, rl, dc;

92 
long dr,dl;

93 
char buf[100]; 
94 

95 
//Get the change in wheel positions

96 
{ 
97 
int encoder_right = encoder_read(RIGHT);

98 
int encoder_left = encoder_read(LEFT);

99 

100 
dl = encoder_left  encoder_ltm1; 
101 
dr = encoder_right  encoder_rtm1; 
102 

103 
//No motion.

104 
if(dl == 0 && dr == 0){ 
105 
if(control_velocity) modify_velocity();

106 
return;

107 
} 
108  
109 
encoder_ltm1 = encoder_left; 
110 
encoder_rtm1 = encoder_right; 
111 

112 
// Try to avoid over/underflow.

113 
dl = dl > 512 ? dl  1024 :dl; 
114 
dl = dl < 512 ? dl + 1024 :dl; 
115 
dr = dr > 512 ? dr  1024 :dr; 
116 
dr = dr < 512 ? dr + 1024 :dr; 
117 

118 
//Convert "clicks" to um

119 
dl *= CLICK_DISTANCE_UM; //um

120 
dr *= CLICK_DISTANCE_UM; 
121 
} 
122 

123  
124 
if(dl == dr){

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

129 
return;

130 
} 
131 

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

133 
rl = ((double)(ROBOT_WIDTH_UM*dl))/((double)(dl  dr)); //um 
134 
theta = ((double)(dl  dr))/((double)ROBOT_WIDTH_UM); //rad 
135 

136 
//Distance the center has traveled.

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

139 

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

141 
angle = ANGLE_SCALE * theta; 
142 

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

144 
diff_x += lround((DISTANCE_SCALE * dc * cos(angle))/1000.0); //mm 
145 
diff_y += lround((DISTANCE_SCALE * dc * sin(angle))/1000.0); //mm 
146  
147 
if(control_velocity) modify_velocity();

148 
} 
149  
150 
void modify_velocity(){

151 
int diff = target_velocity  velocity;

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

168 
return (long)(d  f > 0.5 ? f + 1 : f); 
169 
} 
170  
171 