Revision 1418
Added encoder_get_x and encoder_get_v.
use get_v at your own risk
If encoder_read returns -1, this usually means battery is low.
If encoder_read returns a value greater than ENCODER_MAX, this usually means there are physical problems with the encoder.
| encoders.c | ||
|---|---|---|
| 34 | 34 |
inline unsigned int right_data_array_prev(void); |
| 35 | 35 |
inline unsigned int right_data_array_bottom(void); |
| 36 | 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); |
|
| 37 | 41 |
|
| 38 | 42 |
void encoder_recv_complete(void){
|
| 39 | 43 |
encoder_buf_index = 0; |
| ... | ... | |
| 75 | 79 |
* @param encoder this is the encoder that you want to read. Valid arguments |
| 76 | 80 |
* are LEFT and RIGHT |
| 77 | 81 |
* |
| 78 |
* @return the value of the specified encoder |
|
| 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. |
|
| 79 | 86 |
**/ |
| 80 | 87 |
int encoder_read(char encoder){
|
| 81 | 88 |
|
| ... | ... | |
| 85 | 92 |
} |
| 86 | 93 |
|
| 87 | 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 |
/** |
|
| 88 | 108 |
* Gets the total distance covered by the specified encoder (in encoder clicks) |
| 89 | 109 |
* |
| 90 | 110 |
* @param encoder the encoder that you want to read, use LEFT or RIGHT |
| ... | ... | |
| 98 | 118 |
else return -1; |
| 99 | 119 |
} |
| 100 | 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 |
|
|
| 101 | 148 |
/** |
| 102 | 149 |
* Resets the distance accumulator for the specified |
| 103 | 150 |
* encoder. |
| ... | ... | |
| 275 | 322 |
return right_data_array[right_data_idx + 1]; |
| 276 | 323 |
} |
| 277 | 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 |
} |
|
Also available in: Unified diff