Revision 1345
Final spline code for master/slave
updated outdated libdragonfly and libwireless
| encoders.c | ||
|---|---|---|
| 54 | 54 |
encoder_buf_index = 0; |
| 55 | 55 |
left_data_buf = 0; |
| 56 | 56 |
right_data_buf= 0; |
| 57 |
|
|
| 57 | 58 |
left_data = -1; |
| 58 | 59 |
right_data = -1; |
| 59 | 60 |
|
| ... | ... | |
| 140 | 141 |
timecount = 0; |
| 141 | 142 |
} |
| 142 | 143 |
|
| 144 |
/////////////////////////////////////////////////////////////////////// |
|
| 145 |
int left_data_at(int index) {
|
|
| 146 |
int tmp_idx = left_data_idx - index; |
|
| 147 |
if (tmp_idx < 0) |
|
| 148 |
tmp_idx += BUFFER_SIZE; |
|
| 149 |
return left_data_array[tmp_idx]; |
|
| 150 |
} |
|
| 143 | 151 |
|
| 152 |
int right_data_at(int index) {
|
|
| 153 |
int tmp_idx = right_data_idx - index; |
|
| 154 |
if (tmp_idx < 0) |
|
| 155 |
tmp_idx += BUFFER_SIZE; |
|
| 156 |
return right_data_array[tmp_idx]; |
|
| 157 |
} |
|
| 158 |
|
|
| 159 |
|
|
| 160 |
int get_dx(char encoder, int index) {
|
|
| 161 |
int dx, ctr; |
|
| 162 |
ctr = 0; |
|
| 163 |
dx = 1024; |
|
| 164 |
do {
|
|
| 165 |
if (encoder == LEFT) |
|
| 166 |
dx = left_data_at(index+ctr+38) - left_data_at(index+ctr); |
|
| 167 |
else |
|
| 168 |
dx = right_data_at(index+ctr) - right_data_at(index+ctr+38); |
|
| 169 |
ctr++; |
|
| 170 |
} while ((dx > 30 || dx < -30) && ctr < 3); |
|
| 171 |
if (dx > 30 || dx < -30) |
|
| 172 |
return ERR_VEL; |
|
| 173 |
return dx; |
|
| 174 |
} |
|
| 144 | 175 |
/** |
| 145 | 176 |
* @brief Returns the approximated instantaneous velocity of the robot |
| 146 | 177 |
* in terms of encoder clicks. |
| ... | ... | |
| 150 | 181 |
* @return The instantaneous velocity for the given wheel. |
| 151 | 182 |
*/ |
| 152 | 183 |
int encoder_get_v(char encoder){
|
| 184 |
/* |
|
| 153 | 185 |
if (encoder == LEFT) |
| 154 | 186 |
return left_data_array_bottom() - left_data_array_top(); |
| 155 | 187 |
|
| 156 | 188 |
if (encoder == RIGHT) |
| 157 | 189 |
return right_data_array_top() - right_data_array_bottom(); |
| 158 | 190 |
|
| 159 |
return -1; /* TODO: velocity could be -1, use another value for error */ |
|
| 191 |
return -1; |
|
| 192 |
*/ |
|
| 193 |
int vel1, vel2, tmp; |
|
| 194 |
vel1 = get_dx(encoder, 0); |
|
| 195 |
vel2 = get_dx(encoder, 1); |
|
| 196 |
|
|
| 197 |
if (vel1 == ERR_VEL && vel2 == ERR_VEL) |
|
| 198 |
return ERR_VEL << 1; |
|
| 199 |
else if (vel2 == ERR_VEL) |
|
| 200 |
return vel1 << 1; |
|
| 201 |
else if (vel1 == ERR_VEL) |
|
| 202 |
return vel2 << 1; |
|
| 203 |
else |
|
| 204 |
return vel1 + vel2; |
|
| 160 | 205 |
} |
| 206 |
///////////////////////////////////////////////////////////////////////// |
|
| 161 | 207 |
|
| 162 |
|
|
| 163 | 208 |
/** |
| 164 | 209 |
* @brief Waits until n encoder reads have occurred. |
| 165 | 210 |
* Counter is reset on functions exit. |
Also available in: Unified diff