Commit d99ea2f2 authored by Daniel Flickinger's avatar Daniel Flickinger

Instrumented garcia-pilot to calculate mean and variance of iteration times....

Instrumented garcia-pilot to calculate mean and variance of iteration times. Also, commented out wheel velocity output within main loop of controller demo.
parent a99a4e9f
...@@ -148,7 +148,11 @@ int main(int argc, char *argv[]) ...@@ -148,7 +148,11 @@ int main(int argc, char *argv[])
const char *batteryfile = BATTERY_LOG_PATH; const char *batteryfile = BATTERY_LOG_PATH;
const char *sfile = DEFAULT_SFILE; const char *sfile = DEFAULT_SFILE;
int retval = EXIT_SUCCESS; int retval = EXIT_SUCCESS;
unsigned long now; unsigned long now, t_offset, t_elapsed, ti_start;
unsigned long ti_list[1000]; /* list of iteration elapsed times */
float ti_mean, ti_var;
FILE *batterylog; FILE *batterylog;
FILE *sdata_in; FILE *sdata_in;
aIOLib ioRef; aIOLib ioRef;
...@@ -293,6 +297,10 @@ int main(int argc, char *argv[]) ...@@ -293,6 +297,10 @@ int main(int argc, char *argv[])
sfile); sfile);
exit(1); exit(1);
} }
aIO_GetMSTicks(ioRef, &t_offset, NULL);
nullb = garcia.createNamedBehavior("null", NULL); nullb = garcia.createNamedBehavior("null", NULL);
av.set(0.2f); av.set(0.2f);
...@@ -305,7 +313,8 @@ int main(int argc, char *argv[]) ...@@ -305,7 +313,8 @@ int main(int argc, char *argv[])
/* These states will come from the RMCD/EMCD in the future */ /* 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)) { while (3 == fscanf(sdata_in, "%f %f %f\n", &e_in, &alpha_in, &theta_in)) {
lcount++;
aIO_GetMSTicks(ioRef, &ti_start, NULL);
/* controller: */ /* controller: */
C_u = 0.8 * tanh(K_gamma * cos(alpha_in) * e_in); C_u = 0.8 * tanh(K_gamma * cos(alpha_in) * e_in);
...@@ -337,22 +346,23 @@ int main(int argc, char *argv[]) ...@@ -337,22 +346,23 @@ int main(int argc, char *argv[])
garcia.setNamedValue("damped-speed-left", &av_L); garcia.setNamedValue("damped-speed-left", &av_L);
garcia.setNamedValue("damped-speed-right", &av_R); garcia.setNamedValue("damped-speed-right", &av_R);
vleft = garcia.getNamedValue("damped-speed-left")->getFloatVal(); // vleft = garcia.getNamedValue("damped-speed-left")->getFloatVal();
vright = garcia.getNamedValue("damped-speed-right")->getFloatVal(); // vright = garcia.getNamedValue("damped-speed-right")->getFloatVal();
if (debug) { /* if (debug) {
fprintf(stderr, "L/R velocities: %f %f\n", vleft, vright); fprintf(stderr, "L/R velocities: %f %f\n", vleft, vright);
} }*/
/* wait 0.033 (1/30) seconds */ /* wait 0.033 (1/30) seconds */
garcia.handleCallbacks(33); garcia.handleCallbacks(33);
aIO_GetMSTicks(ioRef, &now, NULL);
} ti_list[lcount] = now - ti_start;
if (debug) {
fprintf(stderr, "Done with file: read %d lines\n", lcount); lcount++;
} }
/* abort the null primitive: */ /* abort the null primitive: */
av.set(aGARCIA_ERRFLAG_ABORT); av.set(aGARCIA_ERRFLAG_ABORT);
garcia.setNamedValue("status", &av); garcia.setNamedValue("status", &av);
...@@ -363,7 +373,27 @@ int main(int argc, char *argv[]) ...@@ -363,7 +373,27 @@ int main(int argc, char *argv[])
sdata_in = NULL; sdata_in = NULL;
} }
aIO_GetMSTicks(ioRef, &now, NULL);
t_elapsed = now - t_offset;
ti_mean = 0.0f;
ti_var = 0.0f;
for (int incr_i = 0; incr_i < lcount; incr_i++) {
ti_mean += (float)(ti_list[incr_i]);
ti_var += (float)(ti_list[incr_i]) * (float)(ti_list[incr_i]);
}
ti_var = (ti_var - ti_mean*ti_mean / ((float)(lcount))) / ((float)(lcount));
ti_mean = ti_mean / ((float)(lcount));
if (debug) {
fprintf(stderr, "Done with file: read %d lines\n", lcount);
fprintf(stderr, "Elapsed time: %d\n", t_elapsed);
fprintf(stderr, "Iteration times:\n mean: %f, variance: %f\n", ti_mean, ti_var);
}
} }
else { else {
......
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