Commit cff84df7 authored by Timothy Stack's avatar Timothy Stack

Robot-related tweaks and tuning

parent 3353e69f
...@@ -44,16 +44,16 @@ mtp_dump.o: mtp.h ...@@ -44,16 +44,16 @@ mtp_dump.o: mtp.h
test_mtp.sh: mtp_send mtp_recv test_mtp.sh: mtp_send mtp_recv
mtp_test: mtp_test.o $(MTPLIBS) mtp_test: mtp_test.o $(MTPLIBS)
$(CC) $(CFLAGS) $(LDFLAGS) -o $@ mtp_test.o -L. -lmtp $(CC) $(CFLAGS) $(LDFLAGS) -o $@ mtp_test.o -L. -lmtp -lm
mtp_send: mtp_send.o $(MTPLIBS) mtp_send: mtp_send.o $(MTPLIBS)
$(CC) $(CFLAGS) $(LDFLAGS) -o $@ mtp_send.o -L. -lmtp $(CC) $(CFLAGS) $(LDFLAGS) -o $@ mtp_send.o -L. -lmtp -lm
mtp_recv: mtp_recv.o $(MTPLIBS) mtp_recv: mtp_recv.o $(MTPLIBS)
$(CC) $(CFLAGS) $(LDFLAGS) -o $@ mtp_recv.o -L. -lmtp $(CC) $(CFLAGS) $(LDFLAGS) -o $@ mtp_recv.o -L. -lmtp -lm
mtp_dump: mtp_dump.o $(MTPLIBS) mtp_dump: mtp_dump.o $(MTPLIBS)
$(CC) $(CFLAGS) $(LDFLAGS) -o $@ mtp_dump.o -L. -lmtp $(CC) $(CFLAGS) $(LDFLAGS) -o $@ mtp_dump.o -L. -lmtp -lm
CLASSES_SRC = $(wildcard $(SRCDIR)/*.java) CLASSES_SRC = $(wildcard $(SRCDIR)/*.java)
......
...@@ -59,8 +59,10 @@ static int mtp_read(char *handle, char *data, int len) ...@@ -59,8 +59,10 @@ static int mtp_read(char *handle, char *data, int len)
assert(handle != NULL); assert(handle != NULL);
assert(data != NULL); assert(data != NULL);
retval = read(mh->mh_fd, data, len); do {
retval = read(mh->mh_fd, data, len);
} while ((retval == -1) && (errno == EINTR));
if (retval == 0) { if (retval == 0) {
mh->mh_flags |= MHF_EOF; mh->mh_flags |= MHF_EOF;
...@@ -69,6 +71,9 @@ static int mtp_read(char *handle, char *data, int len) ...@@ -69,6 +71,9 @@ static int mtp_read(char *handle, char *data, int len)
else if (retval > 0) { else if (retval > 0) {
mh->mh_remaining += retval; mh->mh_remaining += retval;
} }
else {
printf(" mtp_read %d %s\n", retval, strerror(errno));
}
// printf("%d read(%p(%d), %p, %d)\n", retval, mh, mh->mh_fd, data, len); // printf("%d read(%p(%d), %p, %d)\n", retval, mh, mh->mh_fd, data, len);
...@@ -621,6 +626,55 @@ float mtp_theta(float theta) ...@@ -621,6 +626,55 @@ float mtp_theta(float theta)
return retval; return retval;
} }
void mtp_polar(struct robot_position *current,
struct robot_position *dest,
float *r_out,
float *theta_out)
{
assert(current != NULL);
assert(dest != NULL);
assert(r_out != NULL);
assert(theta_out != NULL);
*r_out = hypotf(current->x - dest->x, current->y - dest->y);
*theta_out = atan2f(current->y - dest->y, dest->x - current->x);
}
void mtp_cartesian(struct robot_position *current,
float r,
float theta,
struct robot_position *dest_out)
{
assert(current != NULL);
assert(dest_out != NULL);
dest_out->x = current->x + cos(theta) * r;
dest_out->y = current->y - sin(theta) * r;
}
int mtp_compass(float theta)
{
int retval = 0;
#define DEGREES_30 (0.52f)
theta = mtp_theta(theta);
if ((theta >= DEGREES_30) && (theta <= (M_PI - DEGREES_30)))
retval |= MCF_NORTH;
if ((theta <= (M_PI_2 - DEGREES_30)) && (theta >= (-M_PI_2 + DEGREES_30)))
retval |= MCF_EAST;
if ((theta <= -DEGREES_30) && (theta >= (-M_PI + DEGREES_30)))
retval |= MCF_SOUTH;
if ((theta >= (M_PI_2 + DEGREES_30) || (theta <= (-M_PI + DEGREES_30))))
retval |= MCF_WEST;
return retval;
}
void mtp_print_packet(FILE *file, struct mtp_packet *mp) void mtp_print_packet(FILE *file, struct mtp_packet *mp)
{ {
struct mtp_garcia_telemetry *mgt; struct mtp_garcia_telemetry *mgt;
......
...@@ -128,8 +128,6 @@ typedef enum { ...@@ -128,8 +128,6 @@ typedef enum {
MA_ID, /*< (int) */ MA_ID, /*< (int) */
MA_Code, /*< (int) */ MA_Code, /*< (int) */
MA_Message, /*< (char *) */ MA_Message, /*< (char *) */
MA_Horizontal, /*< (double) */
MA_Vertical, /*< (double) */
MA_RobotLen, /*< (int) */ MA_RobotLen, /*< (int) */
MA_RobotVal, /*< (robot_config *) */ MA_RobotVal, /*< (robot_config *) */
MA_CameraLen, /*< (int) */ MA_CameraLen, /*< (int) */
...@@ -184,6 +182,42 @@ void mtp_free_packet(struct mtp_packet *mp); ...@@ -184,6 +182,42 @@ void mtp_free_packet(struct mtp_packet *mp);
float mtp_theta(float theta); float mtp_theta(float theta);
void mtp_polar(struct robot_position *current,
struct robot_position *dest,
float *r_out,
float *theta_out);
void mtp_cartesian(struct robot_position *current,
float r,
float theta,
struct robot_position *dest_out);
enum {
MCB_NORTH,
MCB_EAST,
MCB_WEST,
MCB_SOUTH
};
enum {
MCF_NORTH = (1L << MCB_NORTH),
MCF_EAST = (1L << MCB_EAST),
MCF_WEST = (1L << MCB_WEST),
MCF_SOUTH = (1L << MCB_SOUTH),
};
#define MTP_COMPASS_STRING(x) ( \
(x) == (MCF_NORTH|MCF_WEST) ? "nw" : \
(x) == (MCF_NORTH) ? "n" : \
(x) == (MCF_NORTH|MCF_EAST) ? "ne" : \
(x) == (MCF_EAST) ? "e" : \
(x) == (MCF_SOUTH|MCF_EAST) ? "se" : \
(x) == (MCF_SOUTH) ? "s" : \
(x) == (MCF_SOUTH|MCF_WEST) ? "sw" : \
(x) == (MCF_WEST) ? "w" : "u")
int mtp_compass(float theta);
/** /**
* Print the contents of the given packet to the given FILE object. * Print the contents of the given packet to the given FILE object.
* *
......
...@@ -34,6 +34,9 @@ struct pilot_connection_data pc_data; ...@@ -34,6 +34,9 @@ struct pilot_connection_data pc_data;
#define cmp_fuzzy(x1, x2, tol) \ #define cmp_fuzzy(x1, x2, tol) \
((((x1) - (tol)) < (x2)) && (x2 < ((x1) + (tol)))) ((((x1) - (tol)) < (x2)) && (x2 < ((x1) + (tol))))
#define min(x, y) ((x) < (y)) ? (x) : (y)
#define max(x, y) ((x) > (y)) ? (x) : (y)
/** /**
* Compare two position objects to see if they are the "same", where "same" is * Compare two position objects to see if they are the "same", where "same" is
* within some tolerance. * within some tolerance.
...@@ -179,6 +182,44 @@ struct pilot_connection *pc_add_robot(struct robot_config *rc) ...@@ -179,6 +182,44 @@ struct pilot_connection *pc_add_robot(struct robot_config *rc)
return retval; return retval;
} }
void pc_dump_info(void)
{
int lpc;
info("info: pilot list\n");
for (lpc = 0; lpc < pc_data.pcd_connection_count; lpc++) {
struct pilot_connection *pc;
int lpc2;
pc = &pc_data.pcd_connections[lpc];
info(" %s: flags=0x%x; state=%s; tries=%d\n"
" actual: %.2f %.2f %.2f\tlast: %.2f %.2f %.2f\n"
" waypt: %.2f %.2f %.2f %s\n"
" goal: %.2f %.2f %.2f\n",
pc->pc_robot->hostname,
pc->pc_flags,
state_strings[pc->pc_state],
pc->pc_tries_remaining,
pc->pc_last_pos.x, pc->pc_last_pos.y, pc->pc_last_pos.theta,
pc->pc_actual_pos.x, pc->pc_actual_pos.y, pc->pc_actual_pos.theta,
pc->pc_waypoint.x, pc->pc_waypoint.y, pc->pc_waypoint.theta,
(pc->pc_flags & PCF_WAYPOINT) ? "(active)" : "(inactive)",
pc->pc_goal_pos.x, pc->pc_goal_pos.y, pc->pc_goal_pos.theta);
for (lpc2 = 0; lpc2 < pc->pc_obstacle_count; lpc2++) {
struct obstacle_config *oc;
oc = &pc->pc_obstacles[lpc];
info(" obstacle[%d] = %d -- %.2f %.2f %.2f %.2f\n",
lpc,
oc->id,
oc->xmin,
oc->ymin,
oc->xmax,
oc->ymax);
}
}
}
struct pilot_connection *pc_find_pilot(int robot_id) struct pilot_connection *pc_find_pilot(int robot_id)
{ {
struct pilot_connection *retval = NULL; struct pilot_connection *retval = NULL;
...@@ -219,6 +260,7 @@ void pc_plot_waypoint(struct pilot_connection *pc) ...@@ -219,6 +260,7 @@ void pc_plot_waypoint(struct pilot_connection *pc)
pc->pc_flags &= ~PCF_WAYPOINT; pc->pc_flags &= ~PCF_WAYPOINT;
pc->pc_obstacle_count = 0; pc->pc_obstacle_count = 0;
pc->pc_waypoint_tries = 0;
} }
else { else {
int lpc; int lpc;
...@@ -239,11 +281,35 @@ void pc_plot_waypoint(struct pilot_connection *pc) ...@@ -239,11 +281,35 @@ void pc_plot_waypoint(struct pilot_connection *pc)
rl.y1 = pc->pc_goal_pos.y; rl.y1 = pc->pc_goal_pos.y;
pc->pc_tries_remaining = MAX_REFINE_RETRIES; pc->pc_tries_remaining = MAX_REFINE_RETRIES;
#if 0
if ((pc->pc_waypoint_tries > 0) &&
(pc->pc_waypoint_tries % 3) == 0) {
info("*** expanding dynamic obstacle!");
oc->xmin -= 0.20;
oc->ymin -= 0.20;
oc->xmax += 0.20;
oc->ymax += 0.20;
}
#endif
if ((rc_clip_line(&rl, oc) == 0) || if (rc_compute_code(pc->pc_goal_pos.x,
(hypotf(rl.x0 - rl.x1, rl.y0 - rl.y1) < 0.20)) { pc->pc_goal_pos.y,
oc) == 0) {
if (debug) { if (debug) {
info("debug: %s cleared obstacle\n", \ info("debug: %s has a goal in an obstacle\n",
pc->pc_robot->hostname);
}
pc->pc_tries_remaining = 0;
pc->pc_obstacle_count = 0;
}
else if ((rc_clip_line(&rl, oc) == 0) ||
(hypotf(rl.x0 - rl.x1, rl.y0 - rl.y1) < 0.20) ||
((oc->id == -1000) && (pc->pc_waypoint_tries > 20))) {
if (debug) {
info("debug: %s cleared obstacle\n",
pc->pc_robot->hostname); pc->pc_robot->hostname);
} }
...@@ -252,18 +318,18 @@ void pc_plot_waypoint(struct pilot_connection *pc) ...@@ -252,18 +318,18 @@ void pc_plot_waypoint(struct pilot_connection *pc)
sizeof(struct obstacle_config) * (32 - lpc)); sizeof(struct obstacle_config) * (32 - lpc));
pc->pc_obstacle_count -= 1; pc->pc_obstacle_count -= 1;
lpc -= 1; lpc -= 1;
pc->pc_waypoint_tries = 0;
} }
else { else {
float bearing; float bearing;
rc_code_t rc; rc_code_t rc;
int in_vision = 0; int in_vision = 0;
int flipped_bearing = 0; int flipped_bearing = 0;
int compass;
pc->pc_flags |= PCF_WAYPOINT;
printf("clip %f %f %f %f\n", printf("clip %f %f %f %f\n",
rl.x0, rl.y0, rl.x1, rl.y1); rl.x0, rl.y0, rl.x1, rl.y1);
switch (rc_compute_closest(rl.x0, rl.y0, oc)) { switch ((rc = rc_compute_closest(rl.x0, rl.y0, oc))) {
case RCF_LEFT: case RCF_LEFT:
rl.x0 = oc->xmin; rl.x0 = oc->xmin;
break; break;
...@@ -293,10 +359,32 @@ void pc_plot_waypoint(struct pilot_connection *pc) ...@@ -293,10 +359,32 @@ void pc_plot_waypoint(struct pilot_connection *pc)
rl.y0 = oc->ymax; rl.y0 = oc->ymax;
break; break;
} }
if (rc_clip_line(&rl, oc) == 0 ||
(hypotf(rl.x0 - rl.x1, rl.y0 - rl.y1) < 0.05)) {
info("debug: %s cleared obstacle 2\n", \
pc->pc_robot->hostname);
memmove(&pc->pc_obstacles[lpc],
&pc->pc_obstacles[lpc + 1],
sizeof(struct obstacle_config) * (32 - lpc));
pc->pc_obstacle_count -= 1;
lpc -= 1;
pc->pc_waypoint_tries = 0;
continue;
}
pc->pc_flags |= PCF_WAYPOINT;
pc->pc_waypoint_tries += 1;
bearing = atan2f(rl.y0 - rl.y1, rl.x1 - rl.x0); bearing = atan2f(rl.y0 - rl.y1, rl.x1 - rl.x0);
compass = mtp_compass(bearing);
if (debug) { if (debug) {
info("debug: waypoint bearing %f\n", bearing); info("debug: waypoint bearing %f\n", bearing);
info("debug: side=%s; compass=%s\n",
RC_CODE_STRING(rc),
MTP_COMPASS_STRING(compass));
} }
/* /*
...@@ -317,103 +405,95 @@ void pc_plot_waypoint(struct pilot_connection *pc) ...@@ -317,103 +405,95 @@ void pc_plot_waypoint(struct pilot_connection *pc)
* control if we've tried stalling. * control if we've tried stalling.
*/ */
while (!in_vision) { while (!in_vision) {
int new_rc = 0;
switch (rc) {
case RCF_TOP|RCF_LEFT:
if (compass & MCF_EAST)
new_rc = RCF_TOP|RCF_RIGHT;
else if (compass & MCF_SOUTH)
new_rc = RCF_BOTTOM|RCF_LEFT;
else
assert(0);
break;
case RCF_BOTTOM|RCF_LEFT:
if (compass & MCF_EAST)
new_rc = RCF_BOTTOM|RCF_RIGHT;
else if (compass & MCF_NORTH)
new_rc = RCF_TOP|RCF_LEFT;
else
assert(0);
break;
case RCF_BOTTOM|RCF_RIGHT:
if (compass & MCF_NORTH)
new_rc = RCF_TOP|RCF_RIGHT;
else if (compass & MCF_WEST)
new_rc = RCF_BOTTOM|RCF_LEFT;
else
assert(0);
break;
case RCF_TOP|RCF_RIGHT:
if (compass & MCF_SOUTH)
new_rc = RCF_BOTTOM|RCF_RIGHT;
else if (compass & MCF_WEST)
new_rc = RCF_TOP|RCF_LEFT;
else
assert(0);
break;
case RCF_LEFT:
if (compass & MCF_EAST)
new_rc = rc_closest_corner(rl.x0, rl.y0, oc);
else if (compass & MCF_NORTH)
new_rc = RCF_TOP|RCF_LEFT;
else if (compass & MCF_SOUTH)
new_rc = RCF_BOTTOM|RCF_LEFT;
else
assert(0);
break;
case RCF_TOP:
if (compass & MCF_SOUTH)
new_rc = rc_closest_corner(rl.x0, rl.y0, oc);
else if (compass & MCF_EAST)
new_rc = RCF_TOP|RCF_RIGHT;
else if (compass & MCF_WEST)
new_rc = RCF_TOP|RCF_LEFT;
else
assert(0);
break;
case RCF_RIGHT:
if (compass & MCF_WEST)
new_rc = rc_closest_corner(rl.x0, rl.y0, oc);
else if (compass & MCF_NORTH)
new_rc = RCF_TOP|RCF_RIGHT;
else if (compass & MCF_SOUTH)
new_rc = RCF_BOTTOM|RCF_RIGHT;
else
assert(0);
break;
case RCF_BOTTOM:
if (compass & MCF_NORTH)
new_rc = rc_closest_corner(rl.x0, rl.y0, oc);
else if (compass & MCF_EAST)
new_rc = RCF_BOTTOM|RCF_RIGHT;
else if (compass & MCF_WEST)
new_rc = RCF_BOTTOM|RCF_LEFT;
else
assert(0);
break;
if (rl.x0 == oc->xmin && rl.y0 == oc->ymin) { default:
printf(" top left\n");
if (bearing > -M_PI_4) {
pc->pc_waypoint.x = oc->xmax;
pc->pc_waypoint.y = oc->ymin;
}
else {
pc->pc_waypoint.x = oc->xmin;
pc->pc_waypoint.y = oc->ymax;
}
}
else if (rl.x0 == oc->xmin && rl.y0 == oc->ymax) {
printf(" bottom left\n");
if (bearing > M_PI_4) {
pc->pc_waypoint.x = oc->xmin;
pc->pc_waypoint.y = oc->ymin;
}
else {
pc->pc_waypoint.x = oc->xmax;
pc->pc_waypoint.y = oc->ymax;
}
}
else if (rl.x0 == oc->xmax && rl.y0 == oc->ymax) {
printf(" bottom right\n");
if (bearing > (M_PI_2 + M_PI_4)) {
pc->pc_waypoint.x = oc->xmin;
pc->pc_waypoint.y = oc->ymax;
}
else {
pc->pc_waypoint.x = oc->xmax;
pc->pc_waypoint.y = oc->ymin;
}
}
else if (rl.x0 == oc->xmax && rl.y0 == oc->ymin) {
printf(" top right\n");
if (bearing < -(M_PI_2 + M_PI_4)) {
pc->pc_waypoint.x = oc->xmin;
pc->pc_waypoint.y = oc->ymin;
}
else {
pc->pc_waypoint.x = oc->xmax;
pc->pc_waypoint.y = oc->ymax;
}
}
else if (rl.x0 == oc->xmin) {
printf(" left\n");
if (bearing > 0.0) {
pc->pc_waypoint.x = oc->xmin;
pc->pc_waypoint.y = oc->ymin;
}
else {
pc->pc_waypoint.x = oc->xmin;
pc->pc_waypoint.y = oc->ymax;
}
}
else if (rl.x0 == oc->xmax) {
printf(" right\n");
if (bearing > M_PI_2) {
pc->pc_waypoint.x = oc->xmax;
pc->pc_waypoint.y = oc->ymin;
}
else {
pc->pc_waypoint.x = oc->xmax;
pc->pc_waypoint.y = oc->ymax;
}
}
else if (rl.y0 == oc->ymin) {
printf(" top\n");
if (bearing < -M_PI_2) {
pc->pc_waypoint.x = oc->xmin;
pc->pc_waypoint.y = oc->ymin;
}
else {
pc->pc_waypoint.x = oc->xmax;
pc->pc_waypoint.y = oc->ymin;
}
}
else if (rl.y0 == oc->ymax) {
printf(" bottom\n");
if (bearing < M_PI_2) {
pc->pc_waypoint.x = oc->xmax;
pc->pc_waypoint.y = oc->ymax;
}
else {
pc->pc_waypoint.x = oc->xmin;
pc->pc_waypoint.y = oc->ymax;
}
}
else {
assert(0); assert(0);
break;
}
if (debug) {
info("debug: new corner -- %s\n", RC_CODE_STRING(new_rc));
} }
rc_corner(new_rc, &pc->pc_waypoint, oc);
if (pc_point_in_bounds(&pc_data, if (pc_point_in_bounds(pc->pc_waypoint.x,
pc->pc_waypoint.x, pc->pc_waypoint.y)) {
pc->pc_waypoint.y
)) {
in_vision = 1; in_vision = 1;
info("waypoint in vision\n"); info("waypoint in vision\n");
} }
...@@ -449,6 +529,16 @@ void pc_plot_waypoint(struct pilot_connection *pc) ...@@ -449,6 +529,16 @@ void pc_plot_waypoint(struct pilot_connection *pc)
} }
} }
} }
if (!(pc->pc_flags & PCF_WAYPOINT)) {
float distance, theta;
mtp_polar(&pc->pc_actual_pos, &pc->pc_goal_pos, &distance, &theta);
if (distance > 1.5) {
pc->pc_flags |= PCF_WAYPOINT;
mtp_cartesian(&pc->pc_actual_pos, 1.5, theta, &pc->pc_waypoint);
}
}
} }
void pc_set_goal(struct pilot_connection *pc, struct robot_position *rp) void pc_set_goal(struct pilot_connection *pc, struct robot_position *rp)
...@@ -480,7 +570,8 @@ void pc_set_goal(struct pilot_connection *pc, struct robot_position *rp) ...@@ -480,7 +570,8 @@ void pc_set_goal(struct pilot_connection *pc, struct robot_position *rp)
} }
else { else {
pc->pc_goal_pos = *rp; pc->pc_goal_pos = *rp;
pc_change_state(pc, PS_PENDING_POSITION); if (pc->pc_state != PS_START_WIGGLING && pc->pc_state != PS_WIGGLING)
pc_change_state(pc, PS_PENDING_POSITION);
} }
} }
...@@ -626,7 +717,6 @@ void pc_change_state(struct pilot_connection *pc, pilot_state_t ps) ...@@ -626,7 +717,6 @@ void pc_change_state(struct pilot_connection *pc, pilot_state_t ps)
break; break;
case PS_WIGGLING: case PS_WIGGLING:
printf("try wiggle %d\n", pc->pc_flags);
if (pc->pc_flags & PCF_WIGGLE_REVERSE) { if (pc->pc_flags & PCF_WIGGLE_REVERSE) {
mtp_init_packet(&mp, mtp_init_packet(&mp,