Skip to content

GitLab

Commit cff84df7 authored by Timothy Stack's avatar Timothy Stack
Browse files
Options

Robot-related tweaks and tuning

parent 3353e69f
Show whitespace changes
Inline Side-by-side
Showing with 452 additions and 131 deletions
robots/mtp/GNUmakefile.in
View file @ cff84df7
......@@ -44,16 +44,16 @@ mtp_dump.o: mtp.h
test_mtp.sh: mtp_send mtp_recv
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)
$(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)
$(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)
$(CC) $(CFLAGS) $(LDFLAGS) -o $@ mtp_dump.o -L. -lmtp
$(CC) $(CFLAGS) $(LDFLAGS) -o $@ mtp_dump.o -L. -lmtp -lm
CLASSES_SRC = $(wildcard $(SRCDIR)/*.java)
......
robots/mtp/mtp.c
View file @ cff84df7
......@@ -60,7 +60,9 @@ static int mtp_read(char *handle, char *data, int len)
assert(handle != NULL);
assert(data != NULL);
do {
retval = read(mh->mh_fd, data, len);
} while ((retval == -1) && (errno == EINTR));
if (retval == 0) {
mh->mh_flags |= MHF_EOF;
......@@ -69,6 +71,9 @@ static int mtp_read(char *handle, char *data, int len)
else if (retval > 0) {
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);
......@@ -621,6 +626,55 @@ float mtp_theta(float theta)
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)
{
struct mtp_garcia_telemetry *mgt;
......
robots/mtp/mtp.h
View file @ cff84df7
......@@ -128,8 +128,6 @@ typedef enum {
MA_ID, /*< (int) */
MA_Code, /*< (int) */
MA_Message, /*< (char *) */
MA_Horizontal, /*< (double) */
MA_Vertical, /*< (double) */
MA_RobotLen, /*< (int) */
MA_RobotVal, /*< (robot_config *) */
MA_CameraLen, /*< (int) */
......@@ -184,6 +182,42 @@ void mtp_free_packet(struct mtp_packet *mp);
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.
*
......
robots/rmcd/pilotConnection.c
View file @ cff84df7
......@@ -34,6 +34,9 @@ struct pilot_connection_data pc_data;
#define cmp_fuzzy(x1, x2, 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
* within some tolerance.
......@@ -179,6 +182,44 @@ struct pilot_connection *pc_add_robot(struct robot_config *rc)
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 *retval = NULL;
......@@ -219,6 +260,7 @@ void pc_plot_waypoint(struct pilot_connection *pc)
pc->pc_flags &= ~PCF_WAYPOINT;
pc->pc_obstacle_count = 0;
pc->pc_waypoint_tries = 0;
}
else {
int lpc;
......@@ -240,10 +282,34 @@ void pc_plot_waypoint(struct pilot_connection *pc)
pc->pc_tries_remaining = MAX_REFINE_RETRIES;
if ((rc_clip_line(&rl, oc) == 0) ||
(hypotf(rl.x0 - rl.x1, rl.y0 - rl.y1) < 0.20)) {
#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_compute_code(pc->pc_goal_pos.x,
pc->pc_goal_pos.y,
oc) == 0) {
if (debug) {
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", \
info("debug: %s cleared obstacle\n",
pc->pc_robot->hostname);
}
......@@ -252,18 +318,18 @@ void pc_plot_waypoint(struct pilot_connection *pc)
sizeof(struct obstacle_config) * (32 - lpc));
pc->pc_obstacle_count -= 1;
lpc -= 1;
pc->pc_waypoint_tries = 0;
}
else {
float bearing;
rc_code_t rc;
int in_vision = 0;
int flipped_bearing = 0;
pc->pc_flags |= PCF_WAYPOINT;
int compass;
printf("clip %f %f %f %f\n",
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:
rl.x0 = oc->xmin;
break;
......@@ -293,10 +359,32 @@ void pc_plot_waypoint(struct pilot_connection *pc)
rl.y0 = oc->ymax;
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);
compass = mtp_compass(bearing);
if (debug) {
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)
* control if we've tried stalling.
*/
while (!in_vision) {
int new_rc = 0;
if (rl.x0 == oc->xmin && rl.y0 == oc->ymin) {
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 {
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;
default:
assert(0);
break;
}
if (pc_point_in_bounds(&pc_data,
pc->pc_waypoint.x,
pc->pc_waypoint.y
)) {
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->pc_waypoint.x,
pc->pc_waypoint.y)) {
in_vision = 1;
info("waypoint in vision\n");
}
......@@ -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)
......@@ -480,6 +570,7 @@ void pc_set_goal(struct pilot_connection *pc, struct robot_position *rp)
}
else {
pc->pc_goal_pos = *rp;
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)
break;
case PS_WIGGLING:
printf("try wiggle %d\n", pc->pc_flags);
if (pc->pc_flags & PCF_WIGGLE_REVERSE) {
mtp_init_packet(&mp,
MA_Opcode, MTP_WIGGLE_STATUS,
......@@ -640,13 +730,13 @@ void pc_change_state(struct pilot_connection *pc, pilot_state_t ps)
ps = PS_ARRIVED;
}
else {
printf("** REVERSE %d\n", pc->pc_flags & PCF_CONTACT);
mtp_init_packet(&pmp,
MA_Opcode, MTP_COMMAND_GOTO,
MA_Role, MTP_ROLE_RMC,
MA_RobotID, pc->pc_robot->id,
MA_CommandID, 2,
MA_Theta, pc->pc_flags & PCF_CONTACT ? -M_PI : M_PI,
MA_Theta, (pc->pc_flags & PCF_CONTACT ?
-M_PI : M_PI),
MA_TAG_DONE);
send_pmp = 1;
......@@ -793,7 +883,6 @@ static void pc_handle_update(struct pilot_connection *pc,
case MTP_POSITION_STATUS_CONTACT:
pc->pc_flags &= ~PCF_VISION_POSITION;
pc->pc_flags |= PCF_CONTACT;
printf(" %d\n", pc->pc_flags);
pc_change_state(pc, pc->pc_state);
break;
case MTP_POSITION_STATUS_COMPLETE:
......@@ -876,7 +965,7 @@ static void pc_handle_report(struct pilot_connection *pc,
}
oc = &oc_fake;
oc->id = 1000; // XXX
oc->id = -1000; // XXX
if (bearing < M_PI_4 && bearing > -M_PI_4) {
mcr->points[lpc].x = 0.15;
......@@ -884,7 +973,8 @@ static void pc_handle_report(struct pilot_connection *pc,
}
else if (bearing >= -M_PI_4 && bearing < (M_PI_2 + M_PI_4)) {
}
else if (bearing >= (M_PI_2 + M_PI_4) || bearing < -(M_PI_2 + M_PI_4)) {
else if (bearing >= (M_PI_2 + M_PI_4) ||
bearing < -(M_PI_2 + M_PI_4)) {
}
else if (bearing < (M_PI_2 + M_PI_4) && (bearing > -M_PI_4)) {
mcr->points[lpc].x = -0.15;
......@@ -904,9 +994,27 @@ static void pc_handle_report(struct pilot_connection *pc,
oc->ymin = cp.y - OBSTACLE_BUFFER - 0.10;
oc->ymax = cp.y + OBSTACLE_BUFFER + 0.10;
if (pc->pc_obstacles[pc->pc_obstacle_count - 1].id == -1000) {
struct obstacle_config *oc_old;
oc_old = &pc->pc_obstacles[pc->pc_obstacle_count - 1];
oc_old->xmin = min(oc->xmin, oc_old->xmin);
oc_old->ymin = min(oc->ymin, oc_old->ymin);
oc_old->xmax = max(oc->xmax, oc_old->xmax);
oc_old->ymax = max(oc->ymax, oc_old->ymax);
oc = NULL;
info("debug: expanding dynamic obstacle %f %f %f %f\n",
oc_old->xmin,
oc_old->ymin,
oc_old->xmax,
oc_old->ymax);
}
else {
info("debug: dynamic obstacle %f %f %f %f\n",
oc->xmin, oc->ymin, oc->xmax, oc->ymax);
}
}
if (oc != NULL) {
pc->pc_obstacles[pc->pc_obstacle_count] = *oc;
......@@ -968,14 +1076,22 @@ void pc_handle_signal(fd_set *rready, fd_set *wready)
}
}
int pc_point_in_bounds(struct pilot_connection_data *pcd,float x,float y) {
void pc_handle_timeout(struct timeval *current_time)
{
assert(current_time != NULL);
}
int pc_point_in_bounds(float x, float y)
{
int i;
struct box *boxes;
int boxes_len;
int retval = 0;
boxes = pcd->pcd_config->bounds.bounds_val;
boxes_len = pcd->pcd_config->bounds.bounds_len;
boxes = pc_data.pcd_config->bounds.bounds_val;
boxes_len = pc_data.pcd_config->bounds.bounds_len;
for (i = 0; i < boxes_len; ++i) {