Commit a99a4e9f authored by Daniel Flickinger's avatar Daniel Flickinger
Browse files

Added nonlinear controller for posture regulation into garcia pilot program....

Added nonlinear controller for posture regulation into garcia pilot program. This controller reads "fake" position/state data from file.
parent a7dccc71
......@@ -49,6 +49,14 @@ static const char *DEFAULT_LOG_PATH = "/tmp/garcia-pilot.log";
*/
static const char *BATTERY_LOG_PATH = "/var/log/battery.log";
/**
* Default file to read in state data
*/
static const char *DEFAULT_SFILE = "sdata.txt";
/**
* Determines whether or not to continue in the main loop.
*
......@@ -68,11 +76,11 @@ static void sigquit(int sig)
static int quit_count = 3;
if (quit_count == 0) {
static char *msg = "error: wedged, aborting...\n";
write(2, msg, strlen(msg));
static char *msg = "error: wedged, aborting...\n";
write(2, msg, strlen(msg));
abort();
abort();
}
looping = 0;
......@@ -87,9 +95,9 @@ static void sigquit(int sig)
static void sigdebug(int sig)
{
if (debug == 3)
debug = 1;
debug = 1;
else
debug = 3;
debug = 3;
}
/**
......@@ -119,96 +127,102 @@ static void sigpanic(int signum)
static void usage(void)
{
fprintf(stderr,
"usage: garcia-pilot -hd [-l logfile] [-i pidfile] [-p port]\n"
" [-b battery-log]\n"
"Options:\n"
" -h\t\tPrint this message\n"
" -d\t\tTurn on debugging messages and do not daemonize\n"
" -l logfile\tSend log output to the given file\n"
" -i pidfile\tWrite the process ID to the given file\n"
" -p port\tListen on the given port for MTP messages\n"
" -b battery-log\tAppend battery log data to the given file\n"
" \t\t(Default: %s)\n",
BATTERY_LOG_PATH);
"usage: garcia-pilot -hd [-l logfile] [-i pidfile] [-p port]\n"
" [-b battery-log]\n"
"Options:\n"
" -h\t\tPrint this message\n"
" -d\t\tTurn on debugging messages and do not daemonize\n"
" -o\t\tOpen loop path demo\n"
" -l logfile\tSend log output to the given file\n"
" -i pidfile\tWrite the process ID to the given file\n"
" -p port\tListen on the given port for MTP messages\n"
" -b battery-log\tAppend battery log data to the given file\n"
" \t\t(Default: %s)\n",
BATTERY_LOG_PATH);
}
int main(int argc, char *argv[])
{
int c, port = PILOT_PORT, serv_sock, on_off = 1;
int c, port = PILOT_PORT, serv_sock, on_off = 1, ol_demo = 0;
const char *logfile = NULL, *pidfile = NULL;
const char *batteryfile = BATTERY_LOG_PATH;
const char *sfile = DEFAULT_SFILE;
int retval = EXIT_SUCCESS;
unsigned long now;
FILE *batterylog;
FILE *sdata_in;
aIOLib ioRef;
aErr err;
while ((c = getopt(argc, argv, "hdp:l:i:b:")) != -1) {
switch (c) {
case 'h':
usage();
exit(0);
break;
case 'd':
debug += 1;
break;
case 'l':
logfile = optarg;
break;
case 'i':
pidfile = optarg;
break;
case 'p':
if (sscanf(optarg, "%d", &port) != 1) {
fprintf(stderr,
"error: -p option is not a number: %s\n",
optarg);
usage();
exit(1);
}
break;
case 'b':
batteryfile = optarg;
break;
}
while ((c = getopt(argc, argv, "hdop:l:i:b:")) != -1) {
switch (c) {
case 'h':
usage();
exit(0);
break;
case 'd':
debug += 1;
break;
case 'o':
ol_demo = 1;
break;
case 'l':
logfile = optarg;
break;
case 'i':
pidfile = optarg;
break;
case 'p':
if (sscanf(optarg, "%d", &port) != 1) {
fprintf(stderr,
"error: -p option is not a number: %s\n",
optarg);
usage();
exit(1);
}
break;
case 'b':
batteryfile = optarg;
break;
}
}
#if 0
if (getuid() != 0) {
fprintf(stderr, "error: must run as root!\n");
exit(1);
fprintf(stderr, "error: must run as root!\n");
exit(1);
}
#endif
if (!debug) {
/* Become a daemon */
daemon(1, 0);
/* Become a daemon */
daemon(1, 0);
}
if (logfile) {
int logfd;
if ((logfd = open(logfile, O_CREAT|O_WRONLY|O_APPEND, 0644)) != -1) {
dup2(logfd, 1);
dup2(logfd, 2);
if (logfd > 2)
close(logfd);
}
int logfd;
if ((logfd = open(logfile, O_CREAT|O_WRONLY|O_APPEND, 0644)) != -1) {
dup2(logfd, 1);
dup2(logfd, 2);
if (logfd > 2)
close(logfd);
}
}
if (pidfile) {
FILE *fp;
if ((fp = fopen(pidfile, "w")) != NULL) {
(void) fclose(fp);
}
FILE *fp;
if ((fp = fopen(pidfile, "w")) != NULL) {
(void) fclose(fp);
}
}
if ((batterylog = fopen(batteryfile, "a")) == NULL) {
fprintf(stderr,
"error: cannot open battery log file - %s\n",
batteryfile);
exit(1);
fprintf(stderr,
"error: cannot open battery log file - %s\n",
batteryfile);
exit(1);
}
acpGarcia garcia;
......@@ -227,197 +241,303 @@ int main(int argc, char *argv[])
aIO_GetLibRef(&ioRef, &err);
if (!wait_for_brainstem_link(ioRef, garcia)) {
fprintf(stderr,
"error: could not connect to robot %d\n",
garcia.getNamedValue("status")->getIntVal());
exit(1);
fprintf(stderr,
"error: could not connect to robot %d\n",
garcia.getNamedValue("status")->getIntVal());
exit(1);
}
else {
struct sockaddr_in saddr;
acpValue av;
if (debug) {
fprintf(stderr, "debug: connected to brainstem\n");
}
/* Make sure the units used by the robot are what we expect. */
av.set("radians");
garcia.setNamedValue("angle-units-string", &av);
av.set("meters");
garcia.setNamedValue("distance-units-string", &av);
/* turn off fall sensors */
av.set(0);
garcia.setNamedValue("down-ranger-enable", &av);
memset(&saddr, 0, sizeof(saddr));
struct sockaddr_in saddr;
acpValue av;
if (debug) {
fprintf(stderr, "debug: connected to brainstem\n");
}
/* Make sure the units used by the robot are what we expect. */
av.set("radians");
garcia.setNamedValue("angle-units-string", &av);
av.set("meters");
garcia.setNamedValue("distance-units-string", &av);
/* turn off fall sensors */
av.set(0);
garcia.setNamedValue("down-ranger-enable", &av);
if (1 == ol_demo) {
/* open loop demonstration mode */
float e_in = 0.0f, alpha_in = 0.0f, theta_in = 0.0f; /* states */
float C_u, C_omega; /* controller outputs */
/* controller parameters: */
float K_gamma = 1.0f, K_h = 1.0f, K_k = 1.0f;
float K_radius = 0.0889f;
float vleft, vright;
acpValue av_L;
acpValue av_R;
acpObject *nullb;
int lcount = 0;
if ((sdata_in = fopen(sfile, "r")) == NULL) {
fprintf(stderr,
"FATAL: Error opening state input file: %s\n",
sfile);
exit(1);
}
nullb = garcia.createNamedBehavior("null", NULL);
av.set(0.2f);
nullb->setNamedValue("acceleration", &av);
garcia.queueBehavior(nullb);
/* read states (e,alpha,theta) in from file: */
/* These states will come from the RMCD/EMCD in the future */
while (3 == fscanf(sdata_in, "%f %f %f\n", &e_in, &alpha_in, &theta_in)) {
lcount++;
/* controller: */
C_u = 0.8 * tanh(K_gamma * cos(alpha_in) * e_in);
if (0 == alpha_in) {
C_omega = 0.0f;
}
else {
C_omega = K_k * alpha_in + K_gamma * ((cos(alpha_in)*sin(alpha_in)) / alpha_in) * (alpha_in + K_h * theta_in);
if (C_omega > 26.25f) {
C_omega = 26.25f;
}
if (C_omega < -26.25f) {
C_omega = -26.25f;
}
}
/* end controller */
/* wheel velocity translator: */
vleft = C_u - K_radius * C_omega;
vright = C_u + K_radius * C_omega;
/* end wheel velocity translator */
av_L.set(vleft);
av_R.set(vright);
garcia.setNamedValue("damped-speed-left", &av_L);
garcia.setNamedValue("damped-speed-right", &av_R);
vleft = garcia.getNamedValue("damped-speed-left")->getFloatVal();
vright = garcia.getNamedValue("damped-speed-right")->getFloatVal();
if (debug) {
fprintf(stderr, "L/R velocities: %f %f\n", vleft, vright);
}
/* wait 0.033 (1/30) seconds */
garcia.handleCallbacks(33);
}
if (debug) {
fprintf(stderr, "Done with file: read %d lines\n", lcount);
}
/* abort the null primitive: */
av.set(aGARCIA_ERRFLAG_ABORT);
garcia.setNamedValue("status", &av);
if (sdata_in != NULL) {
fclose(sdata_in);
sdata_in = NULL;
}
}
else {
/* normal mode */
memset(&saddr, 0, sizeof(saddr));
#if !defined(linux)
saddr.sin_len = sizeof(saddr);
saddr.sin_len = sizeof(saddr);
#endif
saddr.sin_family = AF_INET;
saddr.sin_port = htons(port);
saddr.sin_addr.s_addr = INADDR_ANY;
if ((serv_sock = socket(AF_INET, SOCK_STREAM, 0)) == -1) {
perror("socket");
}
else if (setsockopt(serv_sock,
SOL_SOCKET,
SO_REUSEADDR,
&on_off,
sizeof(on_off)) == -1) {
perror("setsockopt");
}
else if (bind(serv_sock,
(struct sockaddr *)&saddr,
sizeof(saddr)) == -1) {
perror("bind");
}
else if (listen(serv_sock, 5) == -1) {
perror("listen");
}
else {
int rmc_telemetry_timeout = 60;
pilotClient::list clients;
fd_set readfds, writefds;
dashboard db(garcia, batterylog);
wheelManager wm(garcia);
pilotButtonCallback pbc(db, wm);
FD_ZERO(&readfds);
FD_ZERO(&writefds);
FD_SET(serv_sock, &readfds);
wm.setDashboard(&db);
do {
fd_set rreadyfds = readfds, wreadyfds = writefds;
struct timeval tv_zero = { 0, 0 };
int rc;
/* Poll the file descriptors, don't block */
rc = select(FD_SETSIZE,
&rreadyfds,
&wreadyfds,
NULL,
&tv_zero);
if (rc > 0) {
bool do_telem = db.wasTelemetryUpdated();
pilotClient::iterator i, j;
struct mtp_packet tmp;
if (FD_ISSET(serv_sock, &rreadyfds)) {
struct sockaddr_in peer_sin;
socklen_t slen;
int cfd;
slen = sizeof(peer_sin);
if ((cfd = accept(serv_sock,
(struct sockaddr *)&peer_sin,
&slen)) == -1) {
perror("accept");
}
else {
pilotClient *pc = new pilotClient(cfd, wm, db);
if (debug) {
fprintf(stderr,
"debug: connect from %s:%d\n",
inet_ntoa(peer_sin.sin_addr),
ntohs(peer_sin.sin_port));
}
pc->setFD(&readfds);
pc->setFD(&writefds);
clients.push_back(pc);
}
}
if (do_telem) {
mtp_init_packet(&tmp,
MA_Opcode, MTP_TELEMETRY,
MA_Role, MTP_ROLE_RMC,
MA_GarciaTelemetry, db.getTelemetry(),
MA_TAG_DONE);
if (pilotClient::pc_rmc_client != NULL) {
rmc_telemetry_timeout -= 1;
if (rmc_telemetry_timeout <= 0) {
pilotClient *pc = pilotClient::pc_rmc_client;
rmc_telemetry_timeout = 60;
if (mtp_send_packet(pc->getHandle(), &tmp) !=
MTP_PP_SUCCESS) {
fprintf(stderr,
"error: cannot send telemetry to "
"rmcd\n");
}
}
}
}
for (i = clients.begin(); i != clients.end(); ) {
bool bad_client = false;
pilotClient *pc = *i;
j = i++;
if (pc->isFDSet(&rreadyfds)) {
do {
struct mtp_packet mp;
if ((mtp_receive_packet(pc->getHandle(),
&mp) !=
MTP_PP_SUCCESS) ||
!pc->handlePacket(&mp, clients)) {
bad_client = true;
}
mtp_free_packet(&mp);
} while (pc && pc->getHandle()->mh_remaining);
}
if (!bad_client &&
(pc->getRole() == MTP_ROLE_EMULAB) &&
pc->isFDSet(&wreadyfds)) {
if (do_telem &&
mtp_send_packet(pc->getHandle(), &tmp) !=
MTP_PP_SUCCESS) {
fprintf(stderr,
"error: cannot send telemetry to "
"client\n");
bad_client = true;
}
}
if (bad_client) {
clients.erase(j);
pc->clearFD(&readfds);
pc->clearFD(&writefds);
delete pc;
pc = NULL;
}
}
}
garcia.handleCallbacks(50);
aIO_GetMSTicks(ioRef, &now, NULL);
} while (looping && db.update(now));
garcia.flushQueuedBehaviors();
garcia.handleCallbacks(1000);
}
saddr.sin_family = AF_INET;
saddr.sin_port = htons(port);
saddr.sin_addr.s_addr = INADDR_ANY;
if ((serv_sock = socket(AF_INET, SOCK_STREAM, 0)) == -1) {
perror("socket");
}
else if (setsockopt(serv_sock,
SOL_SOCKET,
SO_REUSEADDR,
&on_off,
sizeof(on_off)) == -1) {
perror("setsockopt");
}
else if (bind(serv_sock,
(struct sockaddr *)&saddr,
sizeof(saddr)) == -1) {
perror("bind");
}
else if (listen(serv_sock, 5) == -1) {
perror("listen");
}
else {
int rmc_telemetry_timeout = 60;
pilotClient::list clients;
fd_set readfds, writefds;
dashboard db(garcia, batterylog);
wheelManager wm(garcia);
pilotButtonCallback pbc(db, wm);
FD_ZERO(&readfds);
FD_ZERO(&writefds);
FD_SET(serv_sock, &readfds);
wm.setDashboard(&db);
do {
fd_set rreadyfds = readfds, wreadyfds = writefds;
struct timeval tv_zero = { 0, 0 };
int rc;
/* Poll the file descriptors, don't block */
rc = select(FD_SETSIZE,
&rreadyfds,
&wreadyfds,
NULL,
&tv_zero);
if (rc > 0) {
bool do_telem = db.wasTelemetryUpdated();
pilotClient::iterator i, j;
struct mtp_packet tmp;
if (FD_ISSET(serv_sock, &rreadyfds)) {
struct sockaddr_in peer_sin;
socklen_t slen;
int cfd;
slen = sizeof(peer_sin);
if ((cfd = accept(serv_sock,
(struct sockaddr *)&peer_sin,
&slen)) == -1) {
perror("accept");
}
else {
pilotClient *pc = new pilotClient(cfd, wm, db);
if (debug) {
fprintf(stderr,
"debug: connect from %s:%d\n",
inet_ntoa(peer_sin.sin_addr),
ntohs(peer_sin.sin_port));
}
pc->setFD(&readfds);
pc->setFD(&writefds);
clients.push_back(pc);
}
}
if (do_telem) {
mtp_init_packet(&tmp,
MA_Opcode, MTP_TELEMETRY,
MA_Role, MTP_ROLE_RMC,
MA_GarciaTelemetry, db.getTelemetry(),
MA_TAG_DONE);
if (pilotClient::pc_rmc_client != NULL) {
rmc_telemetry_timeout -= 1;
if (rmc_telemetry_timeout <= 0) {
pilotClient *pc = pilotClient::pc_rmc_client;