Revision 854
Did a lot of cleaning up and commenting. Made the error codes returned
by encoder_read more explicit - so if you get weird values check the documentation.
A lot of stubs and incomplete code, I made these things explicit in the source and documentation.
encoders.c | ||
---|---|---|
38 | 38 |
inline unsigned int right_data_array_prev(void); |
39 | 39 |
inline unsigned int right_data_array_bottom(void); |
40 | 40 |
|
41 |
//RING_BUFFER_NEW(enc_buffer, BUFFER_SIZE, short int); |
|
42 | 41 |
|
43 |
void encoder_recv_complete(void); |
|
44 |
|
|
45 | 42 |
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++; |
|
43 |
encoder_buf_index = 0; |
|
44 |
data_ready++; |
|
50 | 45 |
|
51 |
//delay_ms(ENCODER_DELAY); |
|
52 |
|
|
53 |
spi_transfer(5); |
|
46 |
spi_transfer(5); |
|
54 | 47 |
} |
55 | 48 |
|
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 |
**/ |
|
49 |
/** |
|
50 |
* @brief Initializes encoder variables and the hardware interface. |
|
51 |
*/ |
|
67 | 52 |
void encoders_init(void){ |
68 | 53 |
int i; |
69 | 54 |
|
70 | 55 |
data_ready=0; |
71 | 56 |
|
72 |
spi_init(encoder_recv/*put_bin*/, encoder_recv_complete);
|
|
57 |
spi_init(encoder_recv, encoder_recv_complete); |
|
73 | 58 |
encoder_buf_index = 0; |
74 | 59 |
left_data_buf = 0; |
75 | 60 |
right_data_buf= 0; |
... | ... | |
90 | 75 |
right_data_array[i] = 0; |
91 | 76 |
} |
92 | 77 |
spi_transfer(5); |
93 |
//usb_puts("\tencoders.c Debug: ENCODERS INITIALIZED\n\r"); |
|
94 | 78 |
} |
95 | 79 |
|
96 | 80 |
/** |
97 |
* Returns the specified encoders value |
|
81 |
* @brief Returns the specified encoders value
|
|
98 | 82 |
* |
99 | 83 |
* @param encoder this is the encoder that you want to read. Valid arguments |
100 | 84 |
* are LEFT and RIGHT |
... | ... | |
102 | 86 |
* @return the value of the specified encoder |
103 | 87 |
**/ |
104 | 88 |
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 |
} |
|
89 |
|
|
90 |
if(encoder==LEFT) return left_data; |
|
91 |
else if(encoder==RIGHT) return right_data; |
|
92 |
else return -1; |
|
117 | 93 |
} |
118 | 94 |
|
119 |
int encoder_change(char encoder){ |
|
120 |
return 0; |
|
121 |
} |
|
122 | 95 |
|
96 |
/** |
|
97 |
* @brief Outputs encoder direction as FORWARD OR BACK |
|
98 |
* A STUB! DO NOT use. |
|
99 |
* |
|
100 |
* @param encoder The encoder you want the direction of. |
|
101 |
* Valid arguments are right and left. |
|
102 |
* |
|
103 |
* @return FORWARD or BACK (the constants) |
|
104 |
*/ |
|
123 | 105 |
char encoder_direction(char encoder){ |
124 | 106 |
return 0; |
125 | 107 |
} |
108 |
|
|
126 | 109 |
/** |
127 |
* Gets the velocity of the specified encoder.
|
|
110 |
* Gets the total distance covered by the specified encoder (in encoder clicks)
|
|
128 | 111 |
* |
129 | 112 |
* @param encoder the encoder that you want to read, use LEFT or RIGHT |
130 | 113 |
* |
131 |
* @return The velocity of the specified encoder.
|
|
114 |
* @return The distance covered by the specified encoder.
|
|
132 | 115 |
**/ |
133 | 116 |
int encoder_get_dx(char encoder) { |
134 |
if(encoder==LEFT) |
|
135 |
return left_dx; |
|
136 |
else if(encoder==RIGHT) |
|
137 |
return right_dx; |
|
117 |
|
|
118 |
if(encoder==LEFT) return left_dx; |
|
119 |
else if(encoder==RIGHT) return right_dx; |
|
138 | 120 |
else return -1; |
139 | 121 |
} |
140 | 122 |
|
141 | 123 |
/** |
142 |
* Resets the value of the velocity global variable for the specified
|
|
124 |
* Resets the distance accumulator for the specified
|
|
143 | 125 |
* encoder. |
144 | 126 |
* |
145 |
* @param encoder the encoder that you want to modify
|
|
127 |
* @param encoder the encoder that you want to reset distance for
|
|
146 | 128 |
**/ |
147 | 129 |
void encoder_rst_dx(char encoder) { |
148 |
if(encoder==LEFT) |
|
149 |
left_dx = 0; |
|
150 |
else if(encoder==RIGHT) |
|
151 |
right_dx = 0; |
|
130 |
|
|
131 |
if(encoder==LEFT) left_dx = 0; |
|
132 |
else if(encoder==RIGHT) right_dx = 0; |
|
152 | 133 |
} |
153 | 134 |
|
154 |
/** |
|
155 |
* Returns the current time count for the encoders. |
|
156 |
**/ |
|
135 |
/** |
|
136 |
* @brief Returns the number of encoder reads that have occurred. |
|
137 |
* |
|
138 |
* @return The time count. |
|
139 |
*/ |
|
157 | 140 |
int encoder_get_tc(void) { |
158 | 141 |
return timecount; |
159 | 142 |
} |
160 | 143 |
|
161 |
/** |
|
162 |
* Resets the time count for the encoders.
|
|
163 |
**/
|
|
144 |
/**
|
|
145 |
* @brief Resets the encoder read counter.
|
|
146 |
*/ |
|
164 | 147 |
void encoder_rst_tc(void) { |
165 | 148 |
timecount = 0; |
166 | 149 |
} |
167 | 150 |
|
168 | 151 |
|
152 |
/** |
|
153 |
* @brief Returns the approximated instantaneous velocity of the robot |
|
154 |
* in terms of encoder clicks. |
|
155 |
* |
|
156 |
* @param encoder RIGHT or LEFT - the wheel you want the velocity for. |
|
157 |
* |
|
158 |
* @return The instantaneous velocity for the given wheel. |
|
159 |
*/ |
|
169 | 160 |
int encoder_get_v(char encoder){ |
170 | 161 |
int last, res=0; |
171 | 162 |
|
... | ... | |
196 | 187 |
return res; |
197 | 188 |
} |
198 | 189 |
|
190 |
|
|
191 |
/** |
|
192 |
* @brief Waits until n encoder reads have occurred. |
|
193 |
* Counter is reset on functions exit. |
|
194 |
* |
|
195 |
* @param n |
|
196 |
*/ |
|
199 | 197 |
void encoder_wait(int n){ |
200 | 198 |
while(data_ready<n); |
201 | 199 |
data_ready=0; |
... | ... | |
228 | 226 |
encoder_buf_index = (encoder_buf_index + 1) % 5; |
229 | 227 |
|
230 | 228 |
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; |
|
229 |
|
|
230 |
/*Error handling for the left encoder*/ |
|
231 |
if(!(left_data_buf & OCF)) |
|
232 |
left_data = ENCODER_DATA_NOT_READY; |
|
233 |
if(left_data_buf & (COF | LIN)) |
|
234 |
left_data = ENCODER_MISALIGNED; |
|
235 |
else if((left_data_buf & MagINCn) && (left_data_buf & MagDECn)) |
|
236 |
left_data = ENCODER_MAGNET_FAILURE; |
|
234 | 237 |
else left_data = (left_data_buf>>5) & 1023; |
235 | 238 |
|
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; |
|
239 |
/*Error handling for the right encoder*/ |
|
240 |
if(!(right_data_buf & OCF)) |
|
241 |
right_data = ENCODER_DATA_NOT_READY; |
|
242 |
if(right_data_buf & (COF | LIN)) |
|
243 |
right_data = ENCODER_MISALIGNED; |
|
244 |
else if ((right_data_buf & MagINCn) && (right_data_buf & MagDECn)) |
|
245 |
right_data = ENCODER_MAGNET_FAILURE; |
|
238 | 246 |
else right_data = (right_data_buf>>5) & 1023; |
239 | 247 |
|
240 | 248 |
left_data_buf = 0; |
241 | 249 |
right_data_buf = 0; |
242 | 250 |
|
243 |
|
|
244 |
if(left_data != INVALID && left_data != MAGNET_FAILURE) {
|
|
251 |
/*Above 1023 is invalid data*/ |
|
252 |
if(!(left_data > 1023)) {
|
|
245 | 253 |
left_data_array_put(left_data); |
246 | 254 |
|
247 | 255 |
//Adjust left accumulator |
248 |
dx = left_data_array_prev() - left_data;
|
|
256 |
dx = left_data - left_data_array_prev();
|
|
249 | 257 |
|
250 | 258 |
//Adjust velocity: save last dx |
251 | 259 |
left_v = left_dx; |
252 |
|
|
260 |
|
|
253 | 261 |
if(left_data_array_prev()==0) dx=0; |
254 | 262 |
|
255 | 263 |
if(dx > 512) left_dx += dx - 1023; //Underflow |
... | ... | |
260 | 268 |
left_v = left_dx - left_v; |
261 | 269 |
} |
262 | 270 |
|
263 |
if(right_data != INVALID && right_data != MAGNET_FAILURE) { |
|
271 |
/*Above 1023 is invalid data*/ |
|
272 |
if(!(right_data > 1023)) { |
|
264 | 273 |
right_data_array_put(right_data); |
265 | 274 |
|
266 | 275 |
//Adjust right accumulator |
... | ... | |
279 | 288 |
} |
280 | 289 |
|
281 | 290 |
|
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 | 291 |
//Helper Functions |
304 | 292 |
inline void left_data_array_put(unsigned short int value) { |
305 | 293 |
if(left_data_idx == BUFFER_SIZE-1) |
Also available in: Unified diff