Revision 1345
Final spline code for master/slave
updated outdated libdragonfly and libwireless
encoders.c | ||
---|---|---|
19 | 19 |
int right_dx; |
20 | 20 |
long int timecount; |
21 | 21 |
|
22 |
int left_v; |
|
23 |
int right_v; |
|
24 |
|
|
25 |
|
|
26 | 22 |
volatile short int data_ready; |
27 | 23 |
|
28 | 24 |
void encoder_recv(char data); |
... | ... | |
38 | 34 |
inline unsigned int right_data_array_prev(void); |
39 | 35 |
inline unsigned int right_data_array_bottom(void); |
40 | 36 |
|
41 |
//RING_BUFFER_NEW(enc_buffer, BUFFER_SIZE, short int); |
|
42 | 37 |
|
43 |
void encoder_recv_complete(void); |
|
44 |
|
|
45 | 38 |
void encoder_recv_complete(){ |
46 |
encoder_buf_index = 0; |
|
47 |
// usb_puts("[");usb_puti(left_dx);usb_puts(",");usb_puti(right_dx);usb_puts("]\r\n"); |
|
48 |
// usb_puts("\r\n"); |
|
49 |
data_ready++; |
|
39 |
encoder_buf_index = 0; |
|
40 |
data_ready++; |
|
50 | 41 |
|
51 |
//delay_ms(ENCODER_DELAY); |
|
52 |
|
|
53 |
spi_transfer(5); |
|
42 |
spi_transfer(5); |
|
54 | 43 |
} |
55 | 44 |
|
56 |
void put_bin(char data){ |
|
57 |
int i; |
|
58 |
|
|
59 |
for(i=7;i>=0;i--) |
|
60 |
usb_puti((data>>i)&1); |
|
61 |
usb_puts(" "); |
|
62 |
} |
|
63 |
|
|
64 |
/** |
|
65 |
* Initializes the encoders variables |
|
66 |
**/ |
|
45 |
/** |
|
46 |
* @brief Initializes encoder variables and the hardware interface. |
|
47 |
*/ |
|
67 | 48 |
void encoders_init(void){ |
68 | 49 |
int i; |
69 | 50 |
|
70 | 51 |
data_ready=0; |
71 | 52 |
|
72 |
spi_init(encoder_recv/*put_bin*/, encoder_recv_complete);
|
|
53 |
spi_init(encoder_recv, encoder_recv_complete); |
|
73 | 54 |
encoder_buf_index = 0; |
74 | 55 |
left_data_buf = 0; |
75 | 56 |
right_data_buf= 0; |
76 | 57 |
left_data = -1; |
77 | 58 |
right_data = -1; |
78 | 59 |
|
79 |
|
|
80 |
left_v=0; |
|
81 |
right_v=0; |
|
82 |
|
|
83 | 60 |
//RING_BUFFER_INIT(enc_buffer,BUFFER_SIZE); |
84 | 61 |
left_data_idx = 0; |
85 | 62 |
right_data_idx = 0; |
... | ... | |
90 | 67 |
right_data_array[i] = 0; |
91 | 68 |
} |
92 | 69 |
spi_transfer(5); |
93 |
usb_puts("\tencoders.c Debug: ENCODERS INITIALIZED\n\r"); |
|
94 | 70 |
} |
95 | 71 |
|
96 | 72 |
/** |
97 |
* Returns the specified encoders value |
|
73 |
* @brief Returns the specified encoders value
|
|
98 | 74 |
* |
99 | 75 |
* @param encoder this is the encoder that you want to read. Valid arguments |
100 | 76 |
* are LEFT and RIGHT |
... | ... | |
102 | 78 |
* @return the value of the specified encoder |
103 | 79 |
**/ |
104 | 80 |
int encoder_read(char encoder){ |
105 |
if(encoder==LEFT){ |
|
106 |
usb_puts("\tencoders.c Debug: PRINTING LEFT ENCODER VALUE\n\r"); |
|
107 |
return left_data; |
|
108 |
} |
|
109 |
else if(encoder==RIGHT){ |
|
110 |
usb_puts("\tencoders.c Debug: PRINTING RIGHT ENCODER VALUE\n\r"); |
|
111 |
return right_data; |
|
112 |
} |
|
113 |
else{ |
|
114 |
usb_puts("\tencoders.c Debug: INVALID ENCODER - USE LEFT/RIGHT\n\r"); |
|
115 |
return -1; |
|
116 |
} |
|
81 |
|
|
82 |
if(encoder==LEFT) return left_data; |
|
83 |
else if(encoder==RIGHT) return right_data; |
|
84 |
else return -1; |
|
117 | 85 |
} |
118 | 86 |
|
119 |
int encoder_change(char encoder){ |
|
120 |
return 0; |
|
121 |
} |
|
122 | 87 |
|
88 |
/** |
|
89 |
* @brief Outputs encoder direction as FORWARD OR BACK |
|
90 |
* A STUB! DO NOT use. |
|
91 |
* |
|
92 |
* @param encoder The encoder you want the direction of. |
|
93 |
* Valid arguments are right and left. |
|
94 |
* |
|
95 |
* @return FORWARD or BACK (the constants) |
|
96 |
*/ |
|
123 | 97 |
char encoder_direction(char encoder){ |
124 | 98 |
return 0; |
125 | 99 |
} |
100 |
|
|
126 | 101 |
/** |
127 |
* Gets the velocity of the specified encoder.
|
|
102 |
* Gets the total distance covered by the specified encoder (in encoder clicks)
|
|
128 | 103 |
* |
129 | 104 |
* @param encoder the encoder that you want to read, use LEFT or RIGHT |
130 | 105 |
* |
131 |
* @return The velocity of the specified encoder.
|
|
106 |
* @return The distance covered by the specified encoder.
|
|
132 | 107 |
**/ |
133 | 108 |
int encoder_get_dx(char encoder) { |
134 |
if(encoder==LEFT) |
|
135 |
return left_dx; |
|
136 |
else if(encoder==RIGHT) |
|
137 |
return right_dx; |
|
109 |
|
|
110 |
if(encoder==LEFT) return left_dx; |
|
111 |
else if(encoder==RIGHT) return right_dx; |
|
138 | 112 |
else return -1; |
139 | 113 |
} |
140 | 114 |
|
141 | 115 |
/** |
142 |
* Resets the value of the velocity global variable for the specified
|
|
116 |
* Resets the distance accumulator for the specified
|
|
143 | 117 |
* encoder. |
144 | 118 |
* |
145 |
* @param encoder the encoder that you want to modify
|
|
119 |
* @param encoder the encoder that you want to reset distance for
|
|
146 | 120 |
**/ |
147 | 121 |
void encoder_rst_dx(char encoder) { |
148 |
if(encoder==LEFT) |
|
149 |
left_dx = 0; |
|
150 |
else if(encoder==RIGHT) |
|
151 |
right_dx = 0; |
|
122 |
|
|
123 |
if(encoder==LEFT) left_dx = 0; |
|
124 |
else if(encoder==RIGHT) right_dx = 0; |
|
152 | 125 |
} |
153 | 126 |
|
154 |
/** |
|
155 |
* Returns the current time count for the encoders. |
|
156 |
**/ |
|
127 |
/** |
|
128 |
* @brief Returns the number of encoder reads that have occurred. |
|
129 |
* |
|
130 |
* @return The time count. |
|
131 |
*/ |
|
157 | 132 |
int encoder_get_tc(void) { |
158 | 133 |
return timecount; |
159 | 134 |
} |
160 | 135 |
|
161 |
/** |
|
162 |
* Resets the time count for the encoders.
|
|
163 |
**/
|
|
136 |
/**
|
|
137 |
* @brief Resets the encoder read counter.
|
|
138 |
*/ |
|
164 | 139 |
void encoder_rst_tc(void) { |
165 | 140 |
timecount = 0; |
166 | 141 |
} |
167 | 142 |
|
143 |
/////////////////////////////////////////////////////////////////////// |
|
144 |
int left_data_at(int index) { |
|
145 |
int tmp_idx = left_data_idx - index; |
|
146 |
if (tmp_idx < 0) |
|
147 |
tmp_idx += BUFFER_SIZE; |
|
148 |
return left_data_array[tmp_idx]; |
|
149 |
} |
|
168 | 150 |
|
151 |
int right_data_at(int index) { |
|
152 |
int tmp_idx = right_data_idx - index; |
|
153 |
if (tmp_idx < 0) |
|
154 |
tmp_idx += BUFFER_SIZE; |
|
155 |
return right_data_array[tmp_idx]; |
|
156 |
} |
|
157 |
|
|
158 |
|
|
159 |
int get_dx(char encoder, int index) { |
|
160 |
int dx, ctr; |
|
161 |
ctr = 0; |
|
162 |
dx = 1024; |
|
163 |
do { |
|
164 |
if (encoder == LEFT) |
|
165 |
dx = left_data_at(index+ctr+38) - left_data_at(index+ctr); |
|
166 |
else |
|
167 |
dx = right_data_at(index+ctr) - right_data_at(index+ctr+38); |
|
168 |
ctr++; |
|
169 |
} while ((dx > 30 || dx < -30) && ctr < 3); |
|
170 |
if (dx > 30 || dx < -30) |
|
171 |
return ERR_VEL; |
|
172 |
return dx; |
|
173 |
} |
|
174 |
/** |
|
175 |
* @brief Returns the approximated instantaneous velocity of the robot |
|
176 |
* in terms of encoder clicks. |
|
177 |
* |
|
178 |
* @param encoder RIGHT or LEFT - the wheel you want the velocity for. |
|
179 |
* |
|
180 |
* @return The instantaneous velocity for the given wheel. |
|
181 |
*/ |
|
169 | 182 |
int encoder_get_v(char encoder){ |
170 |
int last, res=0; |
|
171 |
|
|
172 |
cli(); |
|
173 |
|
|
174 |
if(encoder==LEFT){ |
|
175 |
if(left_data_idx==0) |
|
176 |
last = BUFFER_SIZE - 1; |
|
177 |
else |
|
178 |
last = left_data_idx - 1; |
|
179 |
res = ((int)left_data_array[last]) - ((int)left_data_array[left_data_idx]); |
|
180 |
} |
|
181 |
if(encoder==RIGHT){ |
|
182 |
if(right_data_idx==0) |
|
183 |
last = BUFFER_SIZE - 1; |
|
184 |
else |
|
185 |
last = right_data_idx - 1; |
|
186 |
res = ((int)right_data_array[right_data_idx]) - ((int)right_data_array[last]); |
|
187 |
} |
|
188 |
|
|
189 |
sei(); |
|
183 |
/* |
|
184 |
if (encoder == LEFT) |
|
185 |
return left_data_array_bottom() - left_data_array_top(); |
|
190 | 186 |
|
191 |
while(res<MIN_V)//underflow |
|
192 |
res+=1024; |
|
193 |
while(res>MAX_V)//overflow |
|
194 |
res-=1024; |
|
187 |
if (encoder == RIGHT) |
|
188 |
return right_data_array_top() - right_data_array_bottom(); |
|
195 | 189 |
|
196 |
return res; |
|
190 |
return -1; |
|
191 |
*/ |
|
192 |
int vel1, vel2, tmp; |
|
193 |
vel1 = get_dx(encoder, 0); |
|
194 |
vel2 = get_dx(encoder, 1); |
|
195 |
|
|
196 |
if (vel1 == ERR_VEL && vel2 == ERR_VEL) |
|
197 |
return ERR_VEL << 1; |
|
198 |
else if (vel2 == ERR_VEL) |
|
199 |
return vel1 << 1; |
|
200 |
else if (vel1 == ERR_VEL) |
|
201 |
return vel2 << 1; |
|
202 |
else |
|
203 |
return vel1 + vel2; |
|
197 | 204 |
} |
205 |
///////////////////////////////////////////////////////////////////////// |
|
198 | 206 |
|
207 |
/** |
|
208 |
* @brief Waits until n encoder reads have occurred. |
|
209 |
* Counter is reset on functions exit. |
|
210 |
* |
|
211 |
* @param n |
|
212 |
*/ |
|
199 | 213 |
void encoder_wait(int n){ |
200 | 214 |
while(data_ready<n); |
201 | 215 |
data_ready=0; |
... | ... | |
228 | 242 |
encoder_buf_index = (encoder_buf_index + 1) % 5; |
229 | 243 |
|
230 | 244 |
if(encoder_buf_index==0) { |
231 |
|
|
232 |
if(left_data_buf & (COF | LIN) || !(left_data_buf & OCF)) left_data = INVALID; |
|
233 |
else if(((left_data_buf & MagINCn) > 0) && ((left_data_buf & MagDECn) > 0)) left_data = MAGNET_FAILURE; |
|
245 |
|
|
246 |
/*Error handling for the left encoder*/ |
|
247 |
if(!(left_data_buf & OCF)) |
|
248 |
left_data = ENCODER_DATA_NOT_READY; |
|
249 |
if(left_data_buf & (COF | LIN)) |
|
250 |
left_data = ENCODER_MISALIGNED; |
|
251 |
else if((left_data_buf & MagINCn) && (left_data_buf & MagDECn)) |
|
252 |
left_data = ENCODER_MAGNET_FAILURE; |
|
234 | 253 |
else left_data = (left_data_buf>>5) & 1023; |
235 | 254 |
|
236 |
if(right_data_buf & (COF | LIN) || !(right_data_buf & OCF)) right_data = INVALID; |
|
237 |
else if ( ((right_data_buf & MagINCn) > 0) && ((right_data_buf & MagDECn) > 0)) right_data = MAGNET_FAILURE; |
|
255 |
/*Error handling for the right encoder*/ |
|
256 |
if(!(right_data_buf & OCF)) |
|
257 |
right_data = ENCODER_DATA_NOT_READY; |
|
258 |
if(right_data_buf & (COF | LIN)) |
|
259 |
right_data = ENCODER_MISALIGNED; |
|
260 |
else if ((right_data_buf & MagINCn) && (right_data_buf & MagDECn)) |
|
261 |
right_data = ENCODER_MAGNET_FAILURE; |
|
238 | 262 |
else right_data = (right_data_buf>>5) & 1023; |
239 | 263 |
|
240 | 264 |
left_data_buf = 0; |
241 | 265 |
right_data_buf = 0; |
242 | 266 |
|
243 |
|
|
244 |
if(left_data != INVALID && left_data != MAGNET_FAILURE) {
|
|
267 |
/*Above 1023 is invalid data*/ |
|
268 |
if(!(left_data > 1023)) {
|
|
245 | 269 |
left_data_array_put(left_data); |
246 | 270 |
|
247 | 271 |
//Adjust left accumulator |
248 |
dx = left_data_array_prev() - left_data;
|
|
272 |
dx = - left_data + left_data_array_prev();
|
|
249 | 273 |
|
250 |
//Adjust velocity: save last dx |
|
251 |
left_v = left_dx; |
|
252 |
|
|
253 | 274 |
if(left_data_array_prev()==0) dx=0; |
254 | 275 |
|
255 | 276 |
if(dx > 512) left_dx += dx - 1023; //Underflow |
256 | 277 |
else if(dx < -512) left_dx += dx + 1023; //Overflow |
257 | 278 |
else left_dx += dx; |
258 |
|
|
259 |
//Adjust velocity: update |
|
260 |
left_v = left_dx - left_v; |
|
261 | 279 |
} |
262 | 280 |
|
263 |
if(right_data != INVALID && right_data != MAGNET_FAILURE) { |
|
281 |
/*Above 1023 is invalid data*/ |
|
282 |
if(!(right_data > 1023)) { |
|
264 | 283 |
right_data_array_put(right_data); |
265 | 284 |
|
266 | 285 |
//Adjust right accumulator |
... | ... | |
279 | 298 |
} |
280 | 299 |
|
281 | 300 |
|
282 |
void encoders_print_data_array(void){ |
|
283 |
int i; |
|
284 |
usb_puts("Left Data: Pointer is "); |
|
285 |
usb_puti(left_data_idx); |
|
286 |
usb_puts("\n\r"); |
|
287 |
for(i=0; i<BUFFER_SIZE; i++){ |
|
288 |
usb_puti(left_data_array[i]); |
|
289 |
usb_putc(' '); |
|
290 |
} |
|
291 |
usb_puts("\r\n"); |
|
292 |
usb_puts("Right Data: Pointer is"); |
|
293 |
usb_puti(right_data_idx); |
|
294 |
usb_puts("\n\r"); |
|
295 |
for(i=0; i<BUFFER_SIZE; i++){ |
|
296 |
usb_puti(right_data_array[i]); |
|
297 |
usb_putc(' '); |
|
298 |
} |
|
299 |
usb_puts("\r\n\r\n"); |
|
300 |
} |
|
301 |
|
|
302 |
|
|
303 | 301 |
//Helper Functions |
304 | 302 |
inline void left_data_array_put(unsigned short int value) { |
305 | 303 |
if(left_data_idx == BUFFER_SIZE-1) |
Also available in: Unified diff