root / branches / colonetmk2 / code / projects / libdragonfly / encoders.c @ 1456
History | View | Annotate | Download (8.4 KB)
| 1 | #include "encoders.h" |
|---|---|
| 2 | #include "spi.h" |
| 3 | #include "ring_buffer.h" |
| 4 | #include <avr/io.h> |
| 5 | |
| 6 | unsigned int left_data_buf; |
| 7 | unsigned int right_data_buf; |
| 8 | char encoder_buf_index;
|
| 9 | |
| 10 | unsigned int left_data; |
| 11 | unsigned int right_data; |
| 12 | |
| 13 | unsigned int left_data_array[BUFFER_SIZE]; |
| 14 | unsigned int right_data_array[BUFFER_SIZE]; |
| 15 | int left_data_idx;
|
| 16 | int right_data_idx;
|
| 17 | |
| 18 | int left_dx;
|
| 19 | int right_dx;
|
| 20 | long int timecount; |
| 21 | |
| 22 | volatile short int data_ready; |
| 23 | |
| 24 | void encoder_recv(char data); |
| 25 | |
| 26 | //Helper Function Prototypes
|
| 27 | inline void left_data_array_put(unsigned short int value); |
| 28 | inline unsigned int left_data_array_top(void); |
| 29 | inline unsigned int left_data_array_prev(void); |
| 30 | inline unsigned int left_data_array_bottom(void); |
| 31 | |
| 32 | inline void right_data_array_put(unsigned short int value); |
| 33 | inline unsigned int right_data_array_top(void); |
| 34 | inline unsigned int right_data_array_prev(void); |
| 35 | inline unsigned int right_data_array_bottom(void); |
| 36 | |
| 37 | //encoder_get_v helper function prototypes
|
| 38 | int left_data_at(int index); |
| 39 | int right_data_at(int index); |
| 40 | int get_dx(char encoder, int index); |
| 41 | |
| 42 | void encoder_recv_complete(void){ |
| 43 | encoder_buf_index = 0;
|
| 44 | data_ready++; |
| 45 | |
| 46 | spi_transfer(5);
|
| 47 | } |
| 48 | |
| 49 | /**
|
| 50 | * @brief Initializes encoder variables and the hardware interface. |
| 51 | */ |
| 52 | void encoders_init(void){ |
| 53 | int i;
|
| 54 | |
| 55 | data_ready=0;
|
| 56 | |
| 57 | spi_init(encoder_recv, encoder_recv_complete); |
| 58 | encoder_buf_index = 0;
|
| 59 | left_data_buf = 0;
|
| 60 | right_data_buf= 0;
|
| 61 | left_data = -1;
|
| 62 | right_data = -1;
|
| 63 | |
| 64 | //RING_BUFFER_INIT(enc_buffer,BUFFER_SIZE);
|
| 65 | left_data_idx = 0;
|
| 66 | right_data_idx = 0;
|
| 67 | for(i = 0; i < BUFFER_SIZE; i++) { |
| 68 | left_data_array[i] = 0;
|
| 69 | } |
| 70 | for(i = 0; i < BUFFER_SIZE; i++) { |
| 71 | right_data_array[i] = 0;
|
| 72 | } |
| 73 | spi_transfer(5);
|
| 74 | } |
| 75 | |
| 76 | /**
|
| 77 | * @brief Returns the specified encoders value |
| 78 | * |
| 79 | * @param encoder this is the encoder that you want to read. Valid arguments |
| 80 | * are LEFT and RIGHT |
| 81 | * |
| 82 | * @return the value of the specified encoder. |
| 83 | * -1 usually means low battery. |
| 84 | * values above ENCODER_MAX usually means phyiscal problems with |
| 85 | * the encoder. |
| 86 | **/ |
| 87 | int encoder_read(char encoder){ |
| 88 | |
| 89 | if(encoder==LEFT) return left_data; |
| 90 | else if(encoder==RIGHT) return right_data; |
| 91 | else return -1; |
| 92 | } |
| 93 | |
| 94 | /**
|
| 95 | * @brief Get total distance travelled by the specified encoder (in encoder ticks) |
| 96 | * |
| 97 | * @param encoder the encoder that you want to read, either LEFT or RIGHT |
| 98 | * |
| 99 | * @return The distance covered by the specified encoder. |
| 100 | * |
| 101 | * @note Simply calls encoder_get_dx. |
| 102 | **/ |
| 103 | int encoder_get_x(char encoder) { |
| 104 | return encoder_get_dx(encoder);
|
| 105 | } |
| 106 | |
| 107 | /**
|
| 108 | * Gets the total distance covered by the specified encoder (in encoder clicks) |
| 109 | * |
| 110 | * @param encoder the encoder that you want to read, use LEFT or RIGHT |
| 111 | * |
| 112 | * @return The distance covered by the specified encoder. |
| 113 | **/ |
| 114 | int encoder_get_dx(char encoder) { |
| 115 | |
| 116 | if(encoder==LEFT) return left_dx; |
| 117 | else if(encoder==RIGHT) return right_dx; |
| 118 | else return -1; |
| 119 | } |
| 120 | |
| 121 | /**
|
| 122 | * @brief Returns the approximated instantaneous velocity of the robot |
| 123 | * in terms of encoder clicks. |
| 124 | * |
| 125 | * @param encoder RIGHT or LEFT - the wheel you want the velocity for. |
| 126 | * |
| 127 | * @return The instantaneous velocity for the given wheel or twice the ERR_VEL |
| 128 | * if an error occurs (1024 * 2 = 2048) |
| 129 | * |
| 130 | * @bug This uses hard coded values and results are inconsistent. |
| 131 | * Use at your own risk. |
| 132 | */ |
| 133 | int encoder_get_v(char encoder){ |
| 134 | int vel1, vel2;
|
| 135 | vel1 = get_dx(encoder, 0);
|
| 136 | vel2 = get_dx(encoder, 1);
|
| 137 | |
| 138 | if (vel1 == ERR_VEL && vel2 == ERR_VEL)
|
| 139 | return ERR_VEL << 1; |
| 140 | else if (vel2 == ERR_VEL) |
| 141 | return vel1 << 1; |
| 142 | else if (vel1 == ERR_VEL) |
| 143 | return vel2 << 1; |
| 144 | else
|
| 145 | return vel1 + vel2;
|
| 146 | } |
| 147 | |
| 148 | /**
|
| 149 | * Resets the distance accumulator for the specified |
| 150 | * encoder. |
| 151 | * |
| 152 | * @param encoder the encoder that you want to reset distance for |
| 153 | **/ |
| 154 | void encoder_rst_dx(char encoder) { |
| 155 | |
| 156 | if(encoder==LEFT) left_dx = 0; |
| 157 | else if(encoder==RIGHT) right_dx = 0; |
| 158 | } |
| 159 | |
| 160 | /**
|
| 161 | * @brief Returns the number of encoder reads that have occurred. |
| 162 | * |
| 163 | * @return The time count. |
| 164 | */ |
| 165 | int encoder_get_tc(void) { |
| 166 | return timecount;
|
| 167 | } |
| 168 | |
| 169 | /**
|
| 170 | * @brief Resets the encoder read counter. |
| 171 | */ |
| 172 | void encoder_rst_tc(void) { |
| 173 | timecount = 0;
|
| 174 | } |
| 175 | |
| 176 | /**
|
| 177 | * @brief Waits until n encoder reads have occurred. |
| 178 | * Counter is reset on functions exit. |
| 179 | * |
| 180 | * @param n |
| 181 | */ |
| 182 | void encoder_wait(int n){ |
| 183 | while(data_ready<n);
|
| 184 | data_ready=0;
|
| 185 | } |
| 186 | |
| 187 | |
| 188 | //Full reads occur every 40 microseconds. This function should be called
|
| 189 | //every 8 microseconds.
|
| 190 | void encoder_recv(char data){ |
| 191 | short int dx; |
| 192 | |
| 193 | //Parse the encoder data, comes in over 5 bytes 16 bits per encoder,
|
| 194 | // second is offset by 1 bit.
|
| 195 | switch(encoder_buf_index){
|
| 196 | case 0: |
| 197 | right_data_buf |= ((short)data)<<8 & 0xff00; |
| 198 | break;
|
| 199 | case 1: |
| 200 | right_data_buf |= ((short)data) & 0xff; |
| 201 | break;
|
| 202 | case 2: |
| 203 | left_data_buf |= (((short)data) << 9) & (0x7F << 9); |
| 204 | break;
|
| 205 | case 3: |
| 206 | left_data_buf |= (((short)data) << 1) & (0xFF<<1); |
| 207 | break;
|
| 208 | case 4: left_data_buf |= (((short)data)>>7) & 0x1; |
| 209 | } |
| 210 | |
| 211 | encoder_buf_index = (encoder_buf_index + 1) % 5; |
| 212 | |
| 213 | if(encoder_buf_index==0) { |
| 214 | |
| 215 | /*Error handling for the left encoder*/
|
| 216 | if(!(left_data_buf & OCF))
|
| 217 | left_data = ENCODER_DATA_NOT_READY; |
| 218 | if(left_data_buf & (COF | LIN))
|
| 219 | left_data = ENCODER_MISALIGNED; |
| 220 | else if((left_data_buf & MagINCn) && (left_data_buf & MagDECn)) |
| 221 | left_data = ENCODER_MAGNET_FAILURE; |
| 222 | else left_data = (left_data_buf>>5) & 1023; |
| 223 | |
| 224 | /*Error handling for the right encoder*/
|
| 225 | if(!(right_data_buf & OCF))
|
| 226 | right_data = ENCODER_DATA_NOT_READY; |
| 227 | if(right_data_buf & (COF | LIN))
|
| 228 | right_data = ENCODER_MISALIGNED; |
| 229 | else if ((right_data_buf & MagINCn) && (right_data_buf & MagDECn)) |
| 230 | right_data = ENCODER_MAGNET_FAILURE; |
| 231 | else right_data = (right_data_buf>>5) & 1023; |
| 232 | |
| 233 | left_data_buf = 0;
|
| 234 | right_data_buf = 0;
|
| 235 | |
| 236 | /*Note: Above 1023 is invalid data*/
|
| 237 | if(!(left_data > 1023)) { |
| 238 | //Reverse the left wheel since encoders are necessarily mounted backwards.
|
| 239 | left_data = 1023 - left_data;
|
| 240 | left_data_array_put(left_data); |
| 241 | |
| 242 | //Adjust left accumulator
|
| 243 | dx = left_data - left_data_array_prev(); |
| 244 | |
| 245 | if(left_data_array_prev()==0) dx=0; |
| 246 | |
| 247 | if(dx > 512) left_dx += dx - 1023; //Underflow |
| 248 | else if(dx < -512) left_dx += dx + 1023; //Overflow |
| 249 | else left_dx += dx;
|
| 250 | } |
| 251 | |
| 252 | /*Above 1023 is invalid data*/
|
| 253 | if(!(right_data > 1023)) { |
| 254 | right_data_array_put(right_data); |
| 255 | |
| 256 | //Adjust right accumulator
|
| 257 | dx = right_data - right_data_array_prev(); |
| 258 | |
| 259 | if(right_data_array_prev()==0) dx=0; |
| 260 | |
| 261 | if(dx > 512) right_dx += dx - 1023; //underflow |
| 262 | else if(dx < -512) right_dx += dx + 1023; //overflow |
| 263 | else right_dx += dx;
|
| 264 | } |
| 265 | } |
| 266 | |
| 267 | //Increment timecount accumulator
|
| 268 | timecount++; |
| 269 | } |
| 270 | |
| 271 | |
| 272 | //Helper Functions
|
| 273 | inline void left_data_array_put(unsigned short int value) { |
| 274 | if(left_data_idx == BUFFER_SIZE-1) |
| 275 | left_data_idx = 0;
|
| 276 | else
|
| 277 | left_data_idx++; |
| 278 | left_data_array[left_data_idx] = value; |
| 279 | } |
| 280 | |
| 281 | inline unsigned int left_data_array_top(void) { |
| 282 | return left_data_array[left_data_idx];
|
| 283 | } |
| 284 | |
| 285 | inline unsigned int left_data_array_prev(void) { |
| 286 | if(left_data_idx == 0) |
| 287 | return left_data_array[BUFFER_SIZE-1]; |
| 288 | else
|
| 289 | return left_data_array[left_data_idx - 1]; |
| 290 | } |
| 291 | |
| 292 | inline unsigned int left_data_array_bottom(void) { |
| 293 | if(left_data_idx == BUFFER_SIZE-1) |
| 294 | return left_data_array[0]; |
| 295 | else
|
| 296 | return left_data_array[left_data_idx + 1]; |
| 297 | } |
| 298 | |
| 299 | inline void right_data_array_put(unsigned short int value) { |
| 300 | if(right_data_idx == BUFFER_SIZE-1) |
| 301 | right_data_idx = 0;
|
| 302 | else
|
| 303 | right_data_idx++; |
| 304 | right_data_array[right_data_idx] = value; |
| 305 | } |
| 306 | |
| 307 | inline unsigned int right_data_array_top(void) { |
| 308 | return right_data_array[right_data_idx];
|
| 309 | } |
| 310 | |
| 311 | inline unsigned int right_data_array_prev(void) { |
| 312 | if(right_data_idx == 0) |
| 313 | return right_data_array[BUFFER_SIZE-1]; |
| 314 | else
|
| 315 | return right_data_array[right_data_idx - 1]; |
| 316 | } |
| 317 | |
| 318 | inline unsigned int right_data_array_bottom(void) { |
| 319 | if(right_data_idx == BUFFER_SIZE-1) |
| 320 | return right_data_array[0]; |
| 321 | else
|
| 322 | return right_data_array[right_data_idx + 1]; |
| 323 | } |
| 324 | |
| 325 | /* Helper functions for encoder_get_v */
|
| 326 | int left_data_at(int index) { |
| 327 | int tmp_idx = left_data_idx - index;
|
| 328 | if (tmp_idx < 0) |
| 329 | tmp_idx += BUFFER_SIZE; |
| 330 | return left_data_array[tmp_idx];
|
| 331 | } |
| 332 | |
| 333 | int right_data_at(int index) { |
| 334 | int tmp_idx = right_data_idx - index;
|
| 335 | if (tmp_idx < 0) |
| 336 | tmp_idx += BUFFER_SIZE; |
| 337 | return right_data_array[tmp_idx];
|
| 338 | } |
| 339 | |
| 340 | int get_dx(char encoder, int index) { |
| 341 | int dx, ctr;
|
| 342 | ctr = 0;
|
| 343 | dx = 1024;
|
| 344 | do {
|
| 345 | if (encoder == LEFT)
|
| 346 | dx = left_data_at(index+ctr) - left_data_at(index+ctr+38);
|
| 347 | else
|
| 348 | dx = right_data_at(index+ctr) - right_data_at(index+ctr+38);
|
| 349 | ctr++; |
| 350 | } while ((dx > 30 || dx < -30) && ctr < 3); |
| 351 | if (dx > 30 || dx < -30) |
| 352 | return ERR_VEL;
|
| 353 | return dx;
|
| 354 | } |