Revision 749
added another function pointer to encoders which gets called after all bytes are recv'd.
working on a behavior to drive in a straight line, but encoders are giving invalid on robot 5
| encoders.c | ||
|---|---|---|
| 3 | 3 |
#include <dragonfly_lib.h> |
| 4 | 4 |
#include "ring_buffer.h" |
| 5 | 5 |
|
| 6 |
unsigned short int left_data_buf;
|
|
| 7 |
unsigned short int right_data_buf;
|
|
| 6 |
unsigned int left_data_buf; |
|
| 7 |
unsigned int right_data_buf; |
|
| 8 | 8 |
char buf_index; |
| 9 | 9 |
|
| 10 |
unsigned short int left_data;
|
|
| 11 |
unsigned short int right_data;
|
|
| 10 |
unsigned int left_data; |
|
| 11 |
unsigned int right_data; |
|
| 12 | 12 |
|
| 13 |
unsigned short int left_data_array[BUFFER_SIZE];
|
|
| 14 |
unsigned short int right_data_array[BUFFER_SIZE];
|
|
| 13 |
unsigned int left_data_array[BUFFER_SIZE]; |
|
| 14 |
unsigned int right_data_array[BUFFER_SIZE]; |
|
| 15 | 15 |
int left_data_idx; |
| 16 | 16 |
int right_data_idx; |
| 17 | 17 |
|
| ... | ... | |
| 23 | 23 |
|
| 24 | 24 |
//Helper Function Prototypes |
| 25 | 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);
|
|
| 26 |
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 | 29 |
|
| 30 | 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);
|
|
| 31 |
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 | 34 |
|
| 35 | 35 |
//RING_BUFFER_NEW(enc_buffer, BUFFER_SIZE, short int); |
| 36 | 36 |
|
| 37 |
void encoder_recv_complete(void); |
|
| 38 |
|
|
| 39 |
void encoder_recv_complete(){
|
|
| 40 |
spi_transfer(5); |
|
| 41 |
} |
|
| 42 |
|
|
| 43 |
|
|
| 37 | 44 |
void encoders_init(void){
|
| 38 | 45 |
int i; |
| 39 | 46 |
|
| 40 |
spi_init(encoder_recv); |
|
| 47 |
spi_init(encoder_recv, encoder_recv_complete);
|
|
| 41 | 48 |
buf_index = 0; |
| 42 | 49 |
left_data_buf = 0; |
| 43 | 50 |
right_data_buf= 0; |
| ... | ... | |
| 52 | 59 |
for(i = 0; i < BUFFER_SIZE; i++) {
|
| 53 | 60 |
right_data_array[i] = 0; |
| 54 | 61 |
} |
| 62 |
spi_transfer(5); |
|
| 55 | 63 |
} |
| 56 | 64 |
|
| 57 | 65 |
int encoder_read(char encoder){
|
| ... | ... | |
| 94 | 102 |
} |
| 95 | 103 |
|
| 96 | 104 |
|
| 97 |
|
|
| 98 | 105 |
//Full reads occur every 40 microseconds. This function should be called |
| 99 | 106 |
//every 8 microseconds. |
| 100 | 107 |
void encoder_recv(char data){
|
| 108 |
//usb_puti(buf_index); |
|
| 101 | 109 |
short int dx; |
| 102 | 110 |
|
| 103 | 111 |
if(buf_index < 2) |
| ... | ... | |
| 122 | 130 |
else if(((left_data_buf & MagINCn) > 0) && ((left_data_buf & MagDECn) > 0)) |
| 123 | 131 |
left_data = MAGNET_FAILURE; |
| 124 | 132 |
|
| 125 |
else left_data = (left_data_buf>>5) & 1023; |
|
| 133 |
else |
|
| 134 |
left_data = (left_data_buf>>5) & 1023; |
|
| 126 | 135 |
|
| 127 | 136 |
if(right_data_buf & (OCF | COF | LIN)) |
| 128 | 137 |
right_data = INVALID; |
| ... | ... | |
| 132 | 141 |
|
| 133 | 142 |
else right_data = (right_data_buf>>5) & 1023; |
| 134 | 143 |
|
| 135 |
|
|
| 144 |
//usb_puts("[");usb_puti(left_data);usb_puts(",");usb_puti(right_data);usb_puts("]\r\n");
|
|
| 136 | 145 |
|
| 137 | 146 |
left_data_buf = 0; |
| 138 | 147 |
right_data_buf = 0; |
| 148 |
|
|
| 139 | 149 |
} |
| 140 | 150 |
|
| 141 | 151 |
|
| ... | ... | |
| 186 | 196 |
left_data_array[left_data_idx] = value; |
| 187 | 197 |
} |
| 188 | 198 |
|
| 189 |
inline unsigned short int left_data_array_top(void) {
|
|
| 199 |
inline unsigned int left_data_array_top(void) {
|
|
| 190 | 200 |
return left_data_array[left_data_idx]; |
| 191 | 201 |
} |
| 192 | 202 |
|
| 193 |
inline unsigned short int left_data_array_prev(void) {
|
|
| 203 |
inline unsigned int left_data_array_prev(void) {
|
|
| 194 | 204 |
if(left_data_idx == 0) |
| 195 | 205 |
return left_data_array[BUFFER_SIZE-1]; |
| 196 | 206 |
else |
| 197 | 207 |
return left_data_array[left_data_idx - 1]; |
| 198 | 208 |
} |
| 199 | 209 |
|
| 200 |
inline unsigned short int left_data_array_bottom(void) {
|
|
| 210 |
inline unsigned int left_data_array_bottom(void) {
|
|
| 201 | 211 |
if(left_data_idx == BUFFER_SIZE-1) |
| 202 | 212 |
return left_data_array[0]; |
| 203 | 213 |
else |
| ... | ... | |
| 212 | 222 |
right_data_array[right_data_idx] = value; |
| 213 | 223 |
} |
| 214 | 224 |
|
| 215 |
inline unsigned short int right_data_array_top(void) {
|
|
| 225 |
inline unsigned int right_data_array_top(void) {
|
|
| 216 | 226 |
return right_data_array[right_data_idx]; |
| 217 | 227 |
} |
| 218 | 228 |
|
| 219 |
inline unsigned short int right_data_array_prev(void) {
|
|
| 229 |
inline unsigned int right_data_array_prev(void) {
|
|
| 220 | 230 |
if(right_data_idx == 0) |
| 221 | 231 |
return right_data_array[BUFFER_SIZE-1]; |
| 222 | 232 |
else |
| 223 | 233 |
return right_data_array[right_data_idx - 1]; |
| 224 | 234 |
} |
| 225 | 235 |
|
| 226 |
inline unsigned short int right_data_array_bottom(void) {
|
|
| 236 |
inline unsigned int right_data_array_bottom(void) {
|
|
| 227 | 237 |
if(right_data_idx == BUFFER_SIZE-1) |
| 228 | 238 |
return right_data_array[0]; |
| 229 | 239 |
else |
Also available in: Unified diff