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.
*
......@@ -124,6 +132,7 @@ static void usage(void)
"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"
......@@ -134,16 +143,18 @@ static void usage(void)
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) {
while ((c = getopt(argc, argv, "hdop:l:i:b:")) != -1) {
switch (c) {
case 'h':
usage();
......@@ -152,6 +163,9 @@ int main(int argc, char *argv[])
case 'd':
debug += 1;
break;
case 'o':
ol_demo = 1;
break;
case 'l':
logfile = optarg;
break;
......@@ -251,6 +265,111 @@ int main(int argc, char *argv[])
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);
......@@ -412,6 +531,7 @@ int main(int argc, char *argv[])
garcia.handleCallbacks(1000);
}
}
}
aIO_ReleaseLibRef(ioRef, &err);
......
Supports Markdown
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