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
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)
......
......@@ -59,8 +59,10 @@ static int mtp_read(char *handle, char *data, int len)
assert(handle != 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) {
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;
......
......@@ -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.
*
......
This diff is collapsed.
......@@ -51,6 +51,7 @@ struct pilot_connection {
struct obstacle_config pc_obstacles[32];
unsigned int pc_obstacle_count;
unsigned int pc_waypoint_tries;
};
#define REL2ABS(_dst, _theta, _rpoint, _apoint) { \
......@@ -64,6 +65,8 @@ struct pilot_connection {
struct pilot_connection *pc_add_robot(struct robot_config *rc);
void pc_dump_info(void);
/**
* Map the robot ID to a gorobot_conn object.
*
......@@ -81,7 +84,7 @@ void pc_override_state(struct pilot_connection *pc, pilot_state_t ps);
void pc_change_state(struct pilot_connection *pc, pilot_state_t ps);
void pc_handle_packet(struct pilot_connection *pc, struct mtp_packet *mp);
void pc_handle_signal(fd_set *rready, fd_set *wready);
void pc_handle_timeout(struct timeval *current_time);
/**
* How close does the robot have to be before it is considered at the intended
......@@ -102,7 +105,7 @@ void pc_handle_signal(fd_set *rready, fd_set *wready);
/**
* Maximum number of times to try and refine the position before giving up.
*/
#define MAX_REFINE_RETRIES 5
#define MAX_REFINE_RETRIES 4
#define MAX_PILOT_CONNECTIONS 128
......@@ -117,7 +120,7 @@ struct pilot_connection_data {
extern struct pilot_connection_data pc_data;
int pc_point_in_bounds(struct pilot_connection_data *pcd,float x,float y);
int pc_point_in_obstacle(struct pilot_connection_data *pcd,float x,float y);
int pc_point_in_bounds(float x,float y);
int pc_point_in_obstacle(float x,float y);
#endif
#include "config.h"
#include <math.h>
#include <stdio.h>
#include <assert.h>
......@@ -63,6 +64,61 @@ rc_code_t rc_compute_closest(float x, float y, rc_rectangle_t r)
return retval;
}
rc_code_t rc_closest_corner(float x, float y, rc_rectangle_t r)
{
float tl, tr, bl, br, closest;
rc_code_t retval = 0;
assert(r != NULL);
tl = hypotf(x - r->xmin, y - r->ymin);
tr = hypotf(x - r->xmax, y - r->ymin);
bl = hypotf(x - r->xmin, y - r->ymax);
br = hypotf(x - r->xmax, y - r->ymax);
closest = min(tl, min(tr, min(bl, br)));
if (tl == closest)
retval = RCF_TOP|RCF_LEFT;
else if (tr == closest)
retval = RCF_TOP|RCF_RIGHT;
else if (bl == closest)
retval = RCF_BOTTOM|RCF_LEFT;
else if (br == closest)
retval = RCF_BOTTOM|RCF_RIGHT;
else {
assert(0);
}
return retval;
}
void rc_corner(rc_code_t rc, struct robot_position *rp, rc_rectangle_t r)
{
assert(rc != 0);
assert(rp != NULL);
assert(r != NULL);
switch (rc) {
case RCF_TOP|RCF_LEFT:
rp->x = r->xmin;
rp->y = r->ymin;
break;
case RCF_TOP|RCF_RIGHT:
rp->x = r->xmax;
rp->y = r->ymin;
break;
case RCF_BOTTOM|RCF_LEFT:
rp->x = r->xmin;
rp->y = r->ymax;
break;
case RCF_BOTTOM|RCF_RIGHT:
rp->x = r->xmax;
rp->y = r->ymax;
break;
}
}
int rc_clip_line(rc_line_t line, rc_rectangle_t clip)
{
rc_code_t C0, C1, C;
......
......@@ -35,8 +35,20 @@ typedef struct rc_line {
float y1;
} *rc_line_t;
#define RC_CODE_STRING(x) ( \
(x) == (RCF_TOP|RCF_LEFT) ? "tl" : \
(x) == (RCF_TOP) ? "t" : \
(x) == (RCF_TOP|RCF_RIGHT) ? "tr" : \
(x) == (RCF_RIGHT) ? "r" : \
(x) == (RCF_BOTTOM|RCF_RIGHT) ? "br" : \
(x) == (RCF_BOTTOM) ? "b" : \
(x) == (RCF_BOTTOM|RCF_LEFT) ? "bl" : \
(x) == (RCF_LEFT) ? "l" : "u")
void rc_corner(rc_code_t rc, struct robot_position *rp, rc_rectangle_t r);
rc_code_t rc_compute_code(float x, float y, rc_rectangle_t r);
rc_code_t rc_compute_closest(float x, float y, rc_rectangle_t r);
rc_code_t rc_closest_corner(float x, float y, rc_rectangle_t r);
int rc_clip_line(rc_line_t line, rc_rectangle_t clip);
#endif
......@@ -72,6 +72,26 @@ static void usage(void)
"Usage: rmcd [-hd] [-l logfile] [-i pidfile] [-e emchost] [-p emcport]\n");
}
#if defined(SIGINFO)
/* SIGINFO-related stuff */
/**
* Variable used to tell the main loop that we received a SIGINFO.
*/
static int got_siginfo = 0;
/**
* Handler for SIGINFO that sets the got_siginfo variable and breaks the main
* loop so we can really handle the signal.
*
* @param sig The actual signal number received.
*/
static void siginfo(int sig)
{
got_siginfo = 1;
}
#endif
/**
* Handle an mtp packet from emcd.
*
......@@ -154,14 +174,19 @@ int main(int argc, char *argv[])
int c, emc_port = 0, retval = EXIT_SUCCESS;
char *logfile = NULL, *pidfile = NULL;
mtp_handle_t emc_handle = NULL;
struct timeval last_time;
fd_set readfds;
#if 0
{
struct robot_config rc = { 1, "fakegarcia" };
struct pilot_connection *pc;
struct mtp_config_rmc rmc;
debug = 3;
memset(&rmc, 0, sizeof(rmc));
pc_data.pcd_config = &rmc;
pc = pc_add_robot(&rc);
......@@ -270,6 +295,10 @@ int main(int argc, char *argv[])
}
}
#if defined(SIGINFO)
signal(SIGINFO, siginfo);
#endif
if (emc_hostname != NULL || emc_path != NULL) {
struct mtp_packet mp, rmp;
......@@ -301,7 +330,7 @@ int main(int argc, char *argv[])
pc_data.pcd_config = rmc_config;
/*
* Walk through the robot list and connect to the gorobots daemons
* Walk through the robot list and connect to the pilot daemons
* running on the robots.
*/
for (lpc = 0; lpc < rmc_config->robots.robots_len; lpc++) {
......@@ -332,10 +361,21 @@ int main(int argc, char *argv[])
while (looping) {
fd_set rreadyfds = readfds;
struct timeval tv, diff;
int rc;
rc = select(FD_SETSIZE, &rreadyfds, NULL, NULL, NULL);
#if defined(SIGINFO)
if (got_siginfo) {
pc_dump_info();
got_siginfo = 0;
}
#endif
tv.tv_sec = 1;
tv.tv_usec = 0;
rc = select(FD_SETSIZE, &rreadyfds, NULL, NULL, &tv);
gettimeofday(&tv, NULL);
if (rc > 0) {
if (FD_ISSET(emc_handle->mh_fd, &rreadyfds)) {
do {
......@@ -345,8 +385,14 @@ int main(int argc, char *argv[])
pc_handle_signal(&rreadyfds, NULL);
}
else {
errorc("accept\n");
else if (rc == -1 && errno != EINTR) {
errorc("select\n");
}
timersub(&tv, &last_time, &diff);
if (diff.tv_sec > 1) {
pc_handle_timeout(&tv);
last_time = tv;
}
}
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment