root / branches / encoders / code / projects / libdragonfly / encoders.c @ 749
History | View | Annotate | Download (5.03 KB)
| 1 | 674 | justin | #include "encoders.h" |
|---|---|---|---|
| 2 | #include "spi.h" |
||
| 3 | #include <dragonfly_lib.h> |
||
| 4 | 675 | justin | #include "ring_buffer.h" |
| 5 | 495 | kwoo | |
| 6 | 749 | bneuman | unsigned int left_data_buf; |
| 7 | unsigned int right_data_buf; |
||
| 8 | 674 | justin | char buf_index;
|
| 9 | |||
| 10 | 749 | bneuman | unsigned int left_data; |
| 11 | unsigned int right_data; |
||
| 12 | 674 | justin | |
| 13 | 749 | bneuman | unsigned int left_data_array[BUFFER_SIZE]; |
| 14 | unsigned int right_data_array[BUFFER_SIZE]; |
||
| 15 | 677 | jykong | int left_data_idx;
|
| 16 | int right_data_idx;
|
||
| 17 | 675 | justin | |
| 18 | 677 | jykong | int left_dx;
|
| 19 | int right_dx;
|
||
| 20 | long int timecount; |
||
| 21 | |||
| 22 | void encoder_recv(char data); |
||
| 23 | |||
| 24 | //Helper Function Prototypes
|
||
| 25 | inline void left_data_array_put(unsigned short int value); |
||
| 26 | 749 | bneuman | inline unsigned int left_data_array_top(void); |
| 27 | inline unsigned int left_data_array_prev(void); |
||
| 28 | inline unsigned int left_data_array_bottom(void); |
||
| 29 | 677 | jykong | |
| 30 | inline void right_data_array_put(unsigned short int value); |
||
| 31 | 749 | bneuman | inline unsigned int right_data_array_top(void); |
| 32 | inline unsigned int right_data_array_prev(void); |
||
| 33 | inline unsigned int right_data_array_bottom(void); |
||
| 34 | 677 | jykong | |
| 35 | //RING_BUFFER_NEW(enc_buffer, BUFFER_SIZE, short int);
|
||
| 36 | |||
| 37 | 749 | bneuman | void encoder_recv_complete(void); |
| 38 | |||
| 39 | void encoder_recv_complete(){
|
||
| 40 | spi_transfer(5);
|
||
| 41 | } |
||
| 42 | |||
| 43 | |||
| 44 | 674 | justin | void encoders_init(void){ |
| 45 | 677 | jykong | int i;
|
| 46 | |||
| 47 | 749 | bneuman | spi_init(encoder_recv, encoder_recv_complete); |
| 48 | 674 | justin | buf_index = 0;
|
| 49 | left_data_buf = 0;
|
||
| 50 | right_data_buf= 0;
|
||
| 51 | left_data = -1;
|
||
| 52 | right_data = -1;
|
||
| 53 | 677 | jykong | //RING_BUFFER_INIT(enc_buffer,BUFFER_SIZE);
|
| 54 | left_data_idx = 0;
|
||
| 55 | right_data_idx = 0;
|
||
| 56 | for(i = 0; i < BUFFER_SIZE; i++) { |
||
| 57 | left_data_array[i] = 0;
|
||
| 58 | } |
||
| 59 | for(i = 0; i < BUFFER_SIZE; i++) { |
||
| 60 | right_data_array[i] = 0;
|
||
| 61 | } |
||
| 62 | 749 | bneuman | spi_transfer(5);
|
| 63 | 674 | justin | } |
| 64 | |||
| 65 | int encoder_read(char encoder){ |
||
| 66 | if(encoder==LEFT)
|
||
| 67 | return left_data;
|
||
| 68 | else if(encoder==RIGHT) |
||
| 69 | return right_data;
|
||
| 70 | else return -1; |
||
| 71 | } |
||
| 72 | |||
| 73 | int encoder_change(char encoder){ |
||
| 74 | return 0; |
||
| 75 | } |
||
| 76 | |||
| 77 | char encoder_direction(char encoder){ |
||
| 78 | return 0; |
||
| 79 | } |
||
| 80 | |||
| 81 | 677 | jykong | int encoder_get_dx(char encoder) { |
| 82 | if(encoder==LEFT)
|
||
| 83 | return left_dx;
|
||
| 84 | else if(encoder==RIGHT) |
||
| 85 | return right_dx;
|
||
| 86 | else return -1; |
||
| 87 | } |
||
| 88 | |||
| 89 | void encoder_rst_dx(char encoder) { |
||
| 90 | if(encoder==LEFT)
|
||
| 91 | left_dx = 0;
|
||
| 92 | else if(encoder==RIGHT) |
||
| 93 | right_dx = 0;
|
||
| 94 | } |
||
| 95 | |||
| 96 | int encoder_get_tc(void) { |
||
| 97 | return timecount;
|
||
| 98 | } |
||
| 99 | |||
| 100 | void encoder_rst_tc(void) { |
||
| 101 | timecount = 0;
|
||
| 102 | } |
||
| 103 | |||
| 104 | |||
| 105 | 674 | justin | //Full reads occur every 40 microseconds. This function should be called
|
| 106 | //every 8 microseconds.
|
||
| 107 | void encoder_recv(char data){ |
||
| 108 | 749 | bneuman | //usb_puti(buf_index);
|
| 109 | 677 | jykong | short int dx; |
| 110 | |||
| 111 | 674 | justin | if(buf_index < 2) |
| 112 | 676 | justin | right_data_buf |= (((short)data) << ((1-buf_index)*8)) & (0xFF<<(1-buf_index)); |
| 113 | 674 | justin | |
| 114 | else if (buf_index == 2) |
||
| 115 | left_data_buf |= (((short)data) << 9) & (0x7F << 9); |
||
| 116 | |||
| 117 | else if (buf_index == 3) |
||
| 118 | 676 | justin | left_data_buf |= (((short)data) << 1) & (0xFF<<1); |
| 119 | 674 | justin | |
| 120 | else if (buf_index == 4) |
||
| 121 | 676 | justin | left_data_buf |= (((short)data)>>7) & 0x1; |
| 122 | 674 | justin | |
| 123 | |||
| 124 | buf_index = (buf_index + 1) % 5; |
||
| 125 | |||
| 126 | if(buf_index==0){ |
||
| 127 | if(left_data_buf & (OCF | COF | LIN))
|
||
| 128 | left_data = INVALID; |
||
| 129 | |||
| 130 | else if(((left_data_buf & MagINCn) > 0) && ((left_data_buf & MagDECn) > 0)) |
||
| 131 | left_data = MAGNET_FAILURE; |
||
| 132 | |||
| 133 | 749 | bneuman | else
|
| 134 | left_data = (left_data_buf>>5) & 1023; |
||
| 135 | 674 | justin | |
| 136 | if(right_data_buf & (OCF | COF | LIN))
|
||
| 137 | right_data = INVALID; |
||
| 138 | |||
| 139 | else if ( ((left_data_buf & MagINCn) > 0) && ((left_data_buf & MagDECn) > 0)) |
||
| 140 | left_data = MAGNET_FAILURE; |
||
| 141 | |||
| 142 | else right_data = (right_data_buf>>5) & 1023; |
||
| 143 | 675 | justin | |
| 144 | 749 | bneuman | //usb_puts("[");usb_puti(left_data);usb_puts(",");usb_puti(right_data);usb_puts("]\r\n");
|
| 145 | 674 | justin | |
| 146 | left_data_buf = 0;
|
||
| 147 | right_data_buf = 0;
|
||
| 148 | 749 | bneuman | |
| 149 | 674 | justin | } |
| 150 | 677 | jykong | |
| 151 | |||
| 152 | if(left_data < INVALID) {
|
||
| 153 | //Put new data onto data array
|
||
| 154 | left_data_array_put(left_data); |
||
| 155 | |||
| 156 | //Adjust left accumulator
|
||
| 157 | dx = left_data - left_data_array_prev(); |
||
| 158 | if(dx > 5) { //underflow |
||
| 159 | left_dx += dx - 1023;
|
||
| 160 | } |
||
| 161 | else if(dx < -5) { //overflow |
||
| 162 | left_dx += dx + 1023;
|
||
| 163 | } |
||
| 164 | else {
|
||
| 165 | left_dx += dx; |
||
| 166 | } |
||
| 167 | } |
||
| 168 | |||
| 169 | if(right_data < INVALID) {
|
||
| 170 | //Put new data onto data array
|
||
| 171 | right_data_array_put(right_data); |
||
| 172 | |||
| 173 | //Adjust right accumulator
|
||
| 174 | dx = right_data - right_data_array_prev(); |
||
| 175 | if(dx > 5) { //underflow |
||
| 176 | right_dx += dx - 1023;
|
||
| 177 | } |
||
| 178 | else if(dx < -5) { //overflow |
||
| 179 | right_dx += dx + 1023;
|
||
| 180 | } |
||
| 181 | else {
|
||
| 182 | right_dx += dx; |
||
| 183 | } |
||
| 184 | } |
||
| 185 | |||
| 186 | //Increment timecount accumulator
|
||
| 187 | timecount++; |
||
| 188 | 674 | justin | } |
| 189 | 677 | jykong | |
| 190 | //Helper Functions
|
||
| 191 | inline void left_data_array_put(unsigned short int value) { |
||
| 192 | if(left_data_idx == BUFFER_SIZE-1) |
||
| 193 | left_data_idx = 0;
|
||
| 194 | else
|
||
| 195 | left_data_idx++; |
||
| 196 | left_data_array[left_data_idx] = value; |
||
| 197 | } |
||
| 198 | |||
| 199 | 749 | bneuman | inline unsigned int left_data_array_top(void) { |
| 200 | 677 | jykong | return left_data_array[left_data_idx];
|
| 201 | } |
||
| 202 | |||
| 203 | 749 | bneuman | inline unsigned int left_data_array_prev(void) { |
| 204 | 677 | jykong | if(left_data_idx == 0) |
| 205 | return left_data_array[BUFFER_SIZE-1]; |
||
| 206 | else
|
||
| 207 | return left_data_array[left_data_idx - 1]; |
||
| 208 | } |
||
| 209 | |||
| 210 | 749 | bneuman | inline unsigned int left_data_array_bottom(void) { |
| 211 | 677 | jykong | if(left_data_idx == BUFFER_SIZE-1) |
| 212 | return left_data_array[0]; |
||
| 213 | else
|
||
| 214 | return left_data_array[left_data_idx + 1]; |
||
| 215 | } |
||
| 216 | |||
| 217 | inline void right_data_array_put(unsigned short int value) { |
||
| 218 | if(right_data_idx == BUFFER_SIZE-1) |
||
| 219 | right_data_idx = 0;
|
||
| 220 | else
|
||
| 221 | right_data_idx++; |
||
| 222 | right_data_array[right_data_idx] = value; |
||
| 223 | } |
||
| 224 | |||
| 225 | 749 | bneuman | inline unsigned int right_data_array_top(void) { |
| 226 | 677 | jykong | return right_data_array[right_data_idx];
|
| 227 | } |
||
| 228 | |||
| 229 | 749 | bneuman | inline unsigned int right_data_array_prev(void) { |
| 230 | 677 | jykong | if(right_data_idx == 0) |
| 231 | return right_data_array[BUFFER_SIZE-1]; |
||
| 232 | else
|
||
| 233 | return right_data_array[right_data_idx - 1]; |
||
| 234 | } |
||
| 235 | |||
| 236 | 749 | bneuman | inline unsigned int right_data_array_bottom(void) { |
| 237 | 677 | jykong | if(right_data_idx == BUFFER_SIZE-1) |
| 238 | return right_data_array[0]; |
||
| 239 | else
|
||
| 240 | return right_data_array[right_data_idx + 1]; |
||
| 241 | } |