Revision 924
Odometry will temporarily not work. Working code commented out. This commit is so the blight on the world that is
colony math.h will be eradicated for all time.
odometry.c | ||
---|---|---|
4 | 4 |
#include <avr/interrupt.h> |
5 | 5 |
#include <dragonfly_lib.h> |
6 | 6 |
|
7 |
long encoder_ltm1, encoder_rtm1;
|
|
7 |
int encoder_ltm1, encoder_rtm1;
|
|
8 | 8 |
|
9 | 9 |
long diff_x,diff_y; |
10 | 10 |
double angle; |
11 | 11 |
|
12 | 12 |
|
13 |
int odometry_dx(void){
|
|
13 |
long odometry_dx(void){
|
|
14 | 14 |
return diff_x; |
15 | 15 |
} |
16 | 16 |
|
17 |
int odometry_dy(void){
|
|
17 |
long odometry_dy(void){
|
|
18 | 18 |
return diff_y; |
19 | 19 |
} |
20 | 20 |
|
... | ... | |
23 | 23 |
} |
24 | 24 |
|
25 | 25 |
void odometry_init(void){ |
26 |
|
|
26 | 27 |
encoders_init(); |
28 |
|
|
27 | 29 |
odometry_reset(); |
30 |
|
|
28 | 31 |
|
29 | 32 |
//CTC Mode. CLK / 1024 |
30 | 33 |
TCCR2 = 0; // (Fully disconnected) |
... | ... | |
36 | 39 |
} |
37 | 40 |
|
38 | 41 |
void odometry_reset(void){ |
42 |
char buf[40]; |
|
39 | 43 |
diff_x = 0; |
40 | 44 |
diff_y = 0; |
45 |
encoder_ltm1 = encoder_read(LEFT); |
|
46 |
encoder_rtm1 = encoder_read(RIGHT); |
|
47 |
sprintf(buf,"(en_right,en_left) in um: (%d,%d)\n\r",encoder_ltm1,encoder_rtm1); |
|
48 |
usb_puts(buf); |
|
41 | 49 |
angle = 0.0; |
42 | 50 |
} |
43 | 51 |
|
44 |
/*Currently assumes robot only goes forward.*/
|
|
52 |
/*Currently assumes robot only goes forward. |
|
45 | 53 |
void odometry_run(void){ |
46 | 54 |
double theta; |
47 | 55 |
int encoder_left, encoder_right,dl,dr; |
... | ... | |
97 | 105 |
|
98 | 106 |
encoder_ltm1 = encoder_left; |
99 | 107 |
encoder_rtm1 = encoder_right; |
108 |
}*/ |
|
109 |
|
|
110 |
/*Generalized version of odometry.*/ |
|
111 |
void odometry_run(void){ |
|
112 |
//Angle swept through in a time step CCW- |
|
113 |
double theta, rl, dc; |
|
114 |
|
|
115 |
long dr,dl; |
|
116 |
|
|
117 |
char buf[40]; |
|
118 |
//Relative to the inner wheel, turn radius. |
|
119 |
|
|
120 |
//Get the change in wheel positions |
|
121 |
|
|
122 |
int encoder_right = encoder_read(RIGHT); |
|
123 |
int encoder_left = encoder_read(LEFT); |
|
124 |
|
|
125 |
sprintf(buf,"(en_right,en_left) in um: (%d,%d)\n\r",encoder_left,encoder_right); |
|
126 |
usb_puts(buf); |
|
127 |
|
|
128 |
dl = encoder_left - encoder_ltm1; |
|
129 |
dr = encoder_right - encoder_rtm1; |
|
130 |
|
|
131 |
if(dl >=-1 && dl <= 1 && dr >= -1 && dr <= 1) return; |
|
132 |
|
|
133 |
encoder_ltm1 = encoder_left; |
|
134 |
encoder_rtm1 = encoder_right; |
|
135 |
|
|
136 |
//sprintf(buf,"(dl,dr) in clicks: (%ld,%ld)\n\r",dl,dr); |
|
137 |
//usb_puts(buf); |
|
138 |
|
|
139 |
// Try to avoid over/underflow. |
|
140 |
dl = dl > 512 ? dl - 1024 :dl; |
|
141 |
dl = dl < -512 ? dl + 1024 :dl; |
|
142 |
dr = dr > 512 ? dr - 1024 :dr; |
|
143 |
dr = dr < -512 ? dr + 1024 :dr; |
|
144 |
|
|
145 |
sprintf(buf,"(dl,dr) in um: (%ld,%ld)\n\r",dl,dr); |
|
146 |
usb_puts(buf); |
|
147 |
//Convert "clicks" to um |
|
148 |
dl *= CLICK_DISTANCE_UM; //um |
|
149 |
dr *= CLICK_DISTANCE_UM; |
|
150 |
|
|
151 |
if(dl == dr){ |
|
152 |
//usb_putc('#'); |
|
153 |
diff_x += dl*cos(angle)/1000; |
|
154 |
diff_y += dl*sin(angle)/1000; |
|
155 |
//sprintf(buf,"Distance in um: %ld\n\r",dl); |
|
156 |
//usb_puts(buf); |
|
157 |
return; |
|
158 |
} |
|
159 |
//usb_putc('*'); |
|
160 |
//sprintf(buf,"(dl,dr) in um: (%ld,%ld)\n\r",dl,dr); |
|
161 |
//usb_puts(buf); |
|
162 |
|
|
163 |
//Call the left wheel 0, clockwise positive. |
|
164 |
|
|
165 |
rl = ((double)(ROBOT_WIDTH_UM*dl))/((double)(dl - dr)); //um |
|
166 |
|
|
167 |
//sprintf(buf,"Radius from left wheel: %ld\n\r",rl); |
|
168 |
//usb_puts(buf); |
|
169 |
|
|
170 |
theta = ((double)(dl - dr))/((double)ROBOT_WIDTH_UM); //rad |
|
171 |
|
|
172 |
dc = (rl - ROBOT_WIDTH_UM/2)*theta; //um |
|
173 |
|
|
174 |
//sprintf(buf,"Distance in um: %ld\n\r",dc); |
|
175 |
//usb_puts(buf); |
|
176 |
|
|
177 |
diff_x += ((dc * cos(angle))/1000); //mm |
|
178 |
diff_y += ((dc * sin(angle))/1000); //mm |
|
179 |
|
|
180 |
//sprintf(buf,"(dx, dy): (%ld, %ld)\n\r",diff_x, diff_y); |
|
181 |
//usb_puts(buf); |
|
182 |
|
|
183 |
//Change state variables. |
|
184 |
|
|
185 |
//angle is necessarily CCW+, so subtract. |
|
186 |
angle -= theta; |
|
187 |
|
|
100 | 188 |
} |
101 | 189 |
|
102 | 190 |
ISR(TIMER2_COMP_vect){ |
103 |
odometry_run(); |
|
191 |
odometry_run();
|
|
104 | 192 |
} |
105 | 193 |
|
106 | 194 |
|
Also available in: Unified diff