Revision 581
wireless library now times out instead of infinitely looping in waiting for ok
| colonet_dragonfly.c | ||
|---|---|---|
| 172 | 172 |
//usb_puts("after cur_robot_x/y = robot_x/y.\n");
|
| 173 | 173 |
|
| 174 | 174 |
|
| 175 |
int r_x = target_x - cur_robot_x;
|
|
| 176 |
int r_y = target_y - cur_robot_y;
|
|
| 175 |
int e_x = target_x - cur_robot_x;
|
|
| 176 |
int e_y = target_y - cur_robot_y;
|
|
| 177 | 177 |
|
| 178 | 178 |
int v_x = cur_robot_x - last_x; |
| 179 | 179 |
int v_y = cur_robot_y - last_y; |
| 180 | 180 |
|
| 181 |
int e_x = r_x - v_x; |
|
| 182 |
int e_y = r_y - v_y; |
|
| 183 | 181 |
|
| 184 |
sprintf(buf, "vx:%d vy:%d rx:%d ry:%d ex:%d ey:%d\n", v_x, v_y, r_x, r_y, e_x, e_y); |
|
| 182 |
|
|
| 183 |
sprintf(buf, "vx:%d vy:%d rx:%d ry:%d ex:%d ey:%d\n", v_x, v_y, e_x, e_y, e_x, e_y); |
|
| 185 | 184 |
usb_puts(buf); |
| 186 | 185 |
|
| 187 | 186 |
int e_mag = e_x*e_x + e_y*e_y; |
| 188 | 187 |
|
| 189 |
int motor_differential = e_mag;// / 100;
|
|
| 188 |
int motor_differential = e_mag >> 7;// / 128;
|
|
| 190 | 189 |
|
| 191 | 190 |
/* |
| 192 | 191 |
sprintf(buf, "Current position: %d %d\n", cur_robot_x, cur_robot_y); |
| ... | ... | |
| 222 | 221 |
int p_x = v_y; |
| 223 | 222 |
int p_y = -v_x; |
| 224 | 223 |
|
| 225 |
int mat[2][2]; |
|
| 226 |
mat[0][0] = v_y; |
|
| 227 |
mat[0][1] = -v_x; |
|
| 224 |
int e_trans_x = p_x * e_x + p_y * e_y; |
|
| 228 | 225 |
|
| 229 |
int e_trans_x = mat[0][0] * e_x + mat[0][1] * e_y; |
|
| 230 |
|
|
| 231 | 226 |
/* |
| 232 | 227 |
// Determine left or right by transforming error vector to robot axes |
| 233 | 228 |
// Perpendicular |
Also available in: Unified diff