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