Revision 1461
updated all the library code to have sensible _init behavior.
Almost all of the library components have a global variable which gets set after init and the functions inside will fail with an error code if init has not been called. Also, the init functions themselves check this variable and will bail out without doing any damage if that init has already been called
| encoders.c | ||
|---|---|---|
| 3 | 3 |
#include "ring_buffer.h" |
| 4 | 4 |
#include <avr/io.h> |
| 5 | 5 |
|
| 6 |
unsigned char encoders_initd=0; |
|
| 7 |
|
|
| 6 | 8 |
unsigned int left_data_buf; |
| 7 | 9 |
unsigned int right_data_buf; |
| 8 | 10 |
char encoder_buf_index; |
| ... | ... | |
| 21 | 23 |
|
| 22 | 24 |
volatile short int data_ready; |
| 23 | 25 |
|
| 24 |
void encoder_recv(char data);
|
|
| 26 |
int encoder_recv(char data);
|
|
| 25 | 27 |
|
| 26 | 28 |
//Helper Function Prototypes |
| 27 | 29 |
inline void left_data_array_put(unsigned short int value); |
| ... | ... | |
| 49 | 51 |
/** |
| 50 | 52 |
* @brief Initializes encoder variables and the hardware interface. |
| 51 | 53 |
*/ |
| 52 |
void encoders_init(void){
|
|
| 53 |
int i;
|
|
| 54 |
int encoders_init(void){
|
|
| 55 |
int i;
|
|
| 54 | 56 |
|
| 55 |
data_ready=0; |
|
| 57 |
if(encoders_initd) |
|
| 58 |
return ERROR_INIT_ALREADY_INITD; |
|
| 56 | 59 |
|
| 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); |
|
| 60 |
data_ready=0; |
|
| 61 |
|
|
| 62 |
if(spi_init(encoder_recv, encoder_recv_complete)) |
|
| 63 |
return -1; //if spi was inited for something else, bail out |
|
| 64 |
encoder_buf_index = 0; |
|
| 65 |
left_data_buf = 0; |
|
| 66 |
right_data_buf= 0; |
|
| 67 |
left_data = -1; |
|
| 68 |
right_data = -1; |
|
| 69 |
|
|
| 70 |
//RING_BUFFER_INIT(enc_buffer,BUFFER_SIZE); |
|
| 71 |
left_data_idx = 0; |
|
| 72 |
right_data_idx = 0; |
|
| 73 |
|
|
| 74 |
for(i = 0; i < BUFFER_SIZE; i++) {
|
|
| 75 |
left_data_array[i] = 0; |
|
| 76 |
} |
|
| 77 |
for(i = 0; i < BUFFER_SIZE; i++) {
|
|
| 78 |
right_data_array[i] = 0; |
|
| 79 |
} |
|
| 80 |
|
|
| 81 |
spi_transfer(5); |
|
| 82 |
|
|
| 83 |
encoders_initd=1; |
|
| 84 |
return 0; |
|
| 74 | 85 |
} |
| 75 | 86 |
|
| 76 | 87 |
/** |
| ... | ... | |
| 81 | 92 |
* |
| 82 | 93 |
* @return the value of the specified encoder. |
| 83 | 94 |
* -1 usually means low battery. |
| 95 |
* -2 means the library was not properly initialized |
|
| 84 | 96 |
* values above ENCODER_MAX usually means phyiscal problems with |
| 85 | 97 |
* the encoder. |
| 86 | 98 |
**/ |
| 87 | 99 |
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; |
|
| 100 |
if(!encoders_initd) |
|
| 101 |
return -2; |
|
| 102 |
|
|
| 103 |
if(encoder==LEFT) {
|
|
| 104 |
return left_data; |
|
| 105 |
} |
|
| 106 |
else if(encoder==RIGHT){
|
|
| 107 |
return right_data; |
|
| 108 |
} |
|
| 109 |
else{
|
|
| 110 |
return -1; |
|
| 111 |
} |
|
| 92 | 112 |
} |
| 93 | 113 |
|
| 94 | 114 |
/** |
| ... | ... | |
| 109 | 129 |
* |
| 110 | 130 |
* @param encoder the encoder that you want to read, use LEFT or RIGHT |
| 111 | 131 |
* |
| 112 |
* @return The distance covered by the specified encoder.
|
|
| 132 |
* @return The distance covered by the specified encoder, -2 if the library is not initialized
|
|
| 113 | 133 |
**/ |
| 114 | 134 |
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; |
|
| 135 |
if(!encoders_initd) |
|
| 136 |
return -2; |
|
| 137 |
|
|
| 138 |
if(encoder==LEFT) |
|
| 139 |
return left_dx; |
|
| 140 |
else if(encoder==RIGHT) |
|
| 141 |
return right_dx; |
|
| 142 |
else |
|
| 143 |
return -1; |
|
| 119 | 144 |
} |
| 120 | 145 |
|
| 121 | 146 |
/** |
| ... | ... | |
| 132 | 157 |
*/ |
| 133 | 158 |
int encoder_get_v(char encoder){
|
| 134 | 159 |
int vel1, vel2; |
| 160 |
|
|
| 135 | 161 |
vel1 = get_dx(encoder, 0); |
| 136 | 162 |
vel2 = get_dx(encoder, 1); |
| 137 | 163 |
|
| ... | ... | |
| 151 | 177 |
* |
| 152 | 178 |
* @param encoder the encoder that you want to reset distance for |
| 153 | 179 |
**/ |
| 154 |
void encoder_rst_dx(char encoder) {
|
|
| 155 |
|
|
| 156 |
if(encoder==LEFT) left_dx = 0; |
|
| 157 |
else if(encoder==RIGHT) right_dx = 0; |
|
| 180 |
int encoder_rst_dx(char encoder) {
|
|
| 181 |
|
|
| 182 |
if(!encoders_initd) |
|
| 183 |
return ERROR_LIBRARY_NOT_INITD; |
|
| 184 |
|
|
| 185 |
if(encoder==LEFT) |
|
| 186 |
left_dx = 0; |
|
| 187 |
else if(encoder==RIGHT) |
|
| 188 |
right_dx = 0; |
|
| 189 |
|
|
| 190 |
return 0; |
|
| 158 | 191 |
} |
| 159 | 192 |
|
| 160 | 193 |
/** |
| ... | ... | |
| 168 | 201 |
|
| 169 | 202 |
/** |
| 170 | 203 |
* @brief Resets the encoder read counter. |
| 204 |
* @return 0 if init succesfull, an error code otherwise |
|
| 171 | 205 |
*/ |
| 172 |
void encoder_rst_tc(void) {
|
|
| 173 |
timecount = 0; |
|
| 206 |
int encoder_rst_tc(void) {
|
|
| 207 |
if(!encoders_initd) |
|
| 208 |
return ERROR_LIBRARY_NOT_INITD; |
|
| 209 |
|
|
| 210 |
timecount = 0; |
|
| 211 |
|
|
| 212 |
return 0; |
|
| 174 | 213 |
} |
| 175 | 214 |
|
| 176 | 215 |
/** |
| ... | ... | |
| 178 | 217 |
* Counter is reset on functions exit. |
| 179 | 218 |
* |
| 180 | 219 |
* @param n |
| 220 |
* |
|
| 221 |
* @return 0 if init succesfull, an error code otherwise |
|
| 181 | 222 |
*/ |
| 182 |
void encoder_wait(int n){
|
|
| 183 |
while(data_ready<n); |
|
| 184 |
data_ready=0; |
|
| 223 |
int encoder_wait(int n){
|
|
| 224 |
if(!encoders_initd) |
|
| 225 |
return ERROR_LIBRARY_NOT_INITD; |
|
| 226 |
|
|
| 227 |
while(data_ready<n); |
|
| 228 |
data_ready=0; |
|
| 229 |
|
|
| 230 |
return 0; |
|
| 185 | 231 |
} |
| 186 | 232 |
|
| 233 |
/** |
|
| 234 |
* @brief This functions recieves data from the encoders directly |
|
| 235 |
* |
|
| 236 |
* @note Full reads occur every 40 microseconds. This function should be called every 8 microseconds. |
|
| 237 |
* |
|
| 238 |
* @return 0 if init succesfull, an error code otherwise |
|
| 239 |
*/ |
|
| 240 |
int encoder_recv(char data){
|
|
| 241 |
short int dx; |
|
| 187 | 242 |
|
| 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 |
} |
|
| 243 |
if(!encoders_initd) |
|
| 244 |
return ERROR_LIBRARY_NOT_INITD; |
|
| 245 |
|
|
| 246 |
//Parse the encoder data, comes in over 5 bytes 16 bits per encoder, |
|
| 247 |
// second is offset by 1 bit. |
|
| 248 |
switch(encoder_buf_index){
|
|
| 249 |
case 0: |
|
| 250 |
right_data_buf |= ((short)data)<<8 & 0xff00; |
|
| 251 |
break; |
|
| 252 |
case 1: |
|
| 253 |
right_data_buf |= ((short)data) & 0xff; |
|
| 254 |
break; |
|
| 255 |
case 2: |
|
| 256 |
left_data_buf |= (((short)data) << 9) & (0x7F << 9); |
|
| 257 |
break; |
|
| 258 |
case 3: |
|
| 259 |
left_data_buf |= (((short)data) << 1) & (0xFF<<1); |
|
| 260 |
break; |
|
| 261 |
case 4: left_data_buf |= (((short)data)>>7) & 0x1; |
|
| 262 |
} |
|
| 210 | 263 |
|
| 211 |
encoder_buf_index = (encoder_buf_index + 1) % 5;
|
|
| 264 |
encoder_buf_index = (encoder_buf_index + 1) % 5;
|
|
| 212 | 265 |
|
| 213 |
if(encoder_buf_index==0) {
|
|
| 266 |
if(encoder_buf_index==0) {
|
|
| 214 | 267 |
|
| 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;
|
|
| 268 |
/*Error handling for the left encoder*/
|
|
| 269 |
if(!(left_data_buf & OCF))
|
|
| 270 |
left_data = ENCODER_DATA_NOT_READY;
|
|
| 271 |
if(left_data_buf & (COF | LIN))
|
|
| 272 |
left_data = ENCODER_MISALIGNED;
|
|
| 273 |
else if((left_data_buf & MagINCn) && (left_data_buf & MagDECn))
|
|
| 274 |
left_data = ENCODER_MAGNET_FAILURE;
|
|
| 275 |
else left_data = (left_data_buf>>5) & 1023;
|
|
| 223 | 276 |
|
| 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;
|
|
| 277 |
/*Error handling for the right encoder*/
|
|
| 278 |
if(!(right_data_buf & OCF))
|
|
| 279 |
right_data = ENCODER_DATA_NOT_READY;
|
|
| 280 |
if(right_data_buf & (COF | LIN))
|
|
| 281 |
right_data = ENCODER_MISALIGNED;
|
|
| 282 |
else if ((right_data_buf & MagINCn) && (right_data_buf & MagDECn))
|
|
| 283 |
right_data = ENCODER_MAGNET_FAILURE;
|
|
| 284 |
else right_data = (right_data_buf>>5) & 1023;
|
|
| 232 | 285 |
|
| 233 |
left_data_buf = 0;
|
|
| 234 |
right_data_buf = 0;
|
|
| 286 |
left_data_buf = 0;
|
|
| 287 |
right_data_buf = 0;
|
|
| 235 | 288 |
|
| 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);
|
|
| 289 |
/*Note: Above 1023 is invalid data*/
|
|
| 290 |
if(!(left_data > 1023)) {
|
|
| 291 |
//Reverse the left wheel since encoders are necessarily mounted backwards.
|
|
| 292 |
left_data = 1023 - left_data;
|
|
| 293 |
left_data_array_put(left_data);
|
|
| 241 | 294 |
|
| 242 |
//Adjust left accumulator
|
|
| 243 |
dx = left_data - left_data_array_prev();
|
|
| 295 |
//Adjust left accumulator
|
|
| 296 |
dx = left_data - left_data_array_prev();
|
|
| 244 | 297 |
|
| 245 |
if(left_data_array_prev()==0) dx=0;
|
|
| 298 |
if(left_data_array_prev()==0) dx=0;
|
|
| 246 | 299 |
|
| 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 |
}
|
|
| 300 |
if(dx > 512) left_dx += dx - 1023; //Underflow
|
|
| 301 |
else if(dx < -512) left_dx += dx + 1023; //Overflow
|
|
| 302 |
else left_dx += dx;
|
|
| 303 |
}
|
|
| 251 | 304 |
|
| 252 |
/*Above 1023 is invalid data*/
|
|
| 253 |
if(!(right_data > 1023)) {
|
|
| 254 |
right_data_array_put(right_data);
|
|
| 305 |
/*Above 1023 is invalid data*/
|
|
| 306 |
if(!(right_data > 1023)) {
|
|
| 307 |
right_data_array_put(right_data);
|
|
| 255 | 308 |
|
| 256 |
//Adjust right accumulator
|
|
| 257 |
dx = right_data - right_data_array_prev();
|
|
| 309 |
//Adjust right accumulator
|
|
| 310 |
dx = right_data - right_data_array_prev();
|
|
| 258 | 311 |
|
| 259 |
if(right_data_array_prev()==0) dx=0;
|
|
| 312 |
if(right_data_array_prev()==0) dx=0;
|
|
| 260 | 313 |
|
| 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 |
}
|
|
| 314 |
if(dx > 512) right_dx += dx - 1023; //underflow
|
|
| 315 |
else if(dx < -512) right_dx += dx + 1023; //overflow
|
|
| 316 |
else right_dx += dx;
|
|
| 317 |
}
|
|
| 318 |
}
|
|
| 266 | 319 |
|
| 267 |
//Increment timecount accumulator |
|
| 268 |
timecount++; |
|
| 320 |
//Increment timecount accumulator |
|
| 321 |
timecount++; |
|
| 322 |
|
|
| 323 |
return 0; |
|
| 269 | 324 |
} |
| 270 | 325 |
|
| 271 | 326 |
|
Also available in: Unified diff