Homepage Demos Overview Downloads Tutorials Reference
Credits

WalkCalibration.cc

Go to the documentation of this file.
00001 #include "WalkCalibration.h"
00002 #include "FileInputControl.h"
00003 #include "StringInputControl.h"
00004 #include "NullControl.h"
00005 #include "Wireless/Wireless.h"
00006 #include "Behaviors/Controller.h"
00007 #include "Shared/WorldState.h"
00008 #include "Shared/string_util.h"
00009 #include <stdlib.h>
00010 
00011 char * WalkCalibration::datanames[WalkCalibration::NUM_SRC] = { "fs","fr","sr","br","bs","rr" };
00012 
00013 
00014 WalkCalibration::WalkCalibration()
00015   : ControlBase("Interactive Calibration","Leads you through the process of calibrating the current walk"),
00016     st(ROOT), curType(NUM_SRC), 
00017     old_x(0), old_y(0), old_a(0), startTime(0), stopTime(0), 
00018     help(NULL), load(NULL), save(NULL), measure(NULL), clear(NULL), polar(NULL), rect(NULL), isPolar(true),
00019     lastLoad(), firstIn(0), secondIn(0), status()
00020 {
00021   help=new ControlBase("Help");
00022   help->pushSlot(new NullControl("This control will"));
00023   help->pushSlot(new NullControl("lead you through the"));
00024   help->pushSlot(new NullControl("process of taking"));
00025   help->pushSlot(new NullControl("measurements."));
00026   help->pushSlot(new NullControl(""));
00027   help->pushSlot(new NullControl("In order to take"));
00028   help->pushSlot(new NullControl("accurate measurements,"));
00029   help->pushSlot(new NullControl("the types of motion"));
00030   help->pushSlot(new NullControl("have been broken"));
00031   help->pushSlot(new NullControl("down into 6"));
00032   help->pushSlot(new NullControl("categories:"));
00033   help->pushSlot(new NullControl(""));
00034   help->pushSlot(new NullControl("fs - forward/strafe"));
00035   help->pushSlot(new NullControl("fr - forward/rotate"));
00036   help->pushSlot(new NullControl("sr - strafe/rotate"));
00037   help->pushSlot(new NullControl("br - backwards/rotate"));
00038   help->pushSlot(new NullControl("bs - backwards/strafe"));
00039   help->pushSlot(new NullControl("rr - pure rotation"));
00040   help->pushSlot(new NullControl(""));
00041   help->pushSlot(new NullControl("In each of these"));
00042   help->pushSlot(new NullControl("categories, you will"));
00043   help->pushSlot(new NullControl("set the aibo to walk"));
00044   help->pushSlot(new NullControl("in a direction using"));
00045   help->pushSlot(new NullControl("mainly the two noted"));
00046   help->pushSlot(new NullControl("parameters, and then"));
00047   help->pushSlot(new NullControl("tweak the \"unused\""));
00048   help->pushSlot(new NullControl("parameter to cancel"));
00049   help->pushSlot(new NullControl("out any drift in"));
00050   help->pushSlot(new NullControl("that direction."));
00051   help->pushSlot(new NullControl(""));
00052   help->pushSlot(new NullControl("For example, if"));
00053   help->pushSlot(new NullControl("taking an 'fs'"));
00054   help->pushSlot(new NullControl("measurement, you"));
00055   help->pushSlot(new NullControl("would pick any"));
00056   help->pushSlot(new NullControl("forward and strafe"));
00057   help->pushSlot(new NullControl("values, and then"));
00058   help->pushSlot(new NullControl("fiddle with the"));
00059   help->pushSlot(new NullControl("rotational speed"));
00060   help->pushSlot(new NullControl("until it maintained"));
00061   help->pushSlot(new NullControl("a constant heading."));
00062   help->pushSlot(new NullControl(""));
00063   help->pushSlot(new NullControl("If measuring one of"));
00064   help->pushSlot(new NullControl("the rotational"));
00065   help->pushSlot(new NullControl("types, you will"));
00066   help->pushSlot(new NullControl("tweak the unused"));
00067   help->pushSlot(new NullControl("directional movement"));
00068   help->pushSlot(new NullControl("so the aibo is"));
00069   help->pushSlot(new NullControl("facing exactly 180"));
00070   help->pushSlot(new NullControl("degrees from its"));
00071   help->pushSlot(new NullControl("original heading"));
00072   help->pushSlot(new NullControl("whenever it is on"));
00073   help->pushSlot(new NullControl("the opposite side of"));
00074   help->pushSlot(new NullControl("the circle from"));
00075   help->pushSlot(new NullControl("where it started."));
00076   help->pushSlot(new NullControl(""));
00077   help->pushSlot(new NullControl("For pure rotations,"));
00078   help->pushSlot(new NullControl("tweak both strafe"));
00079   help->pushSlot(new NullControl("and forward so the"));
00080   help->pushSlot(new NullControl("rotation is centered"));
00081   help->pushSlot(new NullControl("around the middle of"));
00082   help->pushSlot(new NullControl("the body.  Take"));
00083   help->pushSlot(new NullControl("displacement"));
00084   help->pushSlot(new NullControl("readings for other"));
00085   help->pushSlot(new NullControl("measurements"));
00086   help->pushSlot(new NullControl("relative to this"));
00087   help->pushSlot(new NullControl("point."));
00088   help->pushSlot(new NullControl(""));
00089   help->pushSlot(new NullControl("Enter angular"));
00090   help->pushSlot(new NullControl("measurements in"));
00091   help->pushSlot(new NullControl("degrees.  Enter"));
00092   help->pushSlot(new NullControl("distances as "));
00093   help->pushSlot(new NullControl("centimeters."));
00094   help->pushSlot(new NullControl(""));
00095   help->pushSlot(new NullControl("You will almost"));
00096   help->pushSlot(new NullControl("always enter"));
00097   help->pushSlot(new NullControl("positive values for"));
00098   help->pushSlot(new NullControl("all measurements."));
00099   help->pushSlot(new NullControl("Enter negative"));
00100   help->pushSlot(new NullControl("values only when"));
00101   help->pushSlot(new NullControl("using cartesian"));
00102   help->pushSlot(new NullControl("measurements with a"));
00103   help->pushSlot(new NullControl("right strafe or"));
00104   help->pushSlot(new NullControl("backward motion, or"));
00105   help->pushSlot(new NullControl("when a rotation is"));
00106   help->pushSlot(new NullControl("opposite of the"));
00107   help->pushSlot(new NullControl("intended direction."));
00108   help->pushSlot(new NullControl(""));
00109   help->pushSlot(new NullControl("Remember that"));
00110   help->pushSlot(new NullControl("POSITIVE strafe is"));
00111   help->pushSlot(new NullControl("to the Aibo's LEFT."));
00112   help->pushSlot(new NullControl(""));
00113   help->pushSlot(new NullControl("Finally, when you"));
00114   help->pushSlot(new NullControl("have taken at least"));
00115   help->pushSlot(new NullControl("6 (I suggest at"));
00116   help->pushSlot(new NullControl("least 16)"));
00117   help->pushSlot(new NullControl("measurements of each"));
00118   help->pushSlot(new NullControl("type, you can save"));
00119   help->pushSlot(new NullControl("the data set."));
00120   help->pushSlot(new NullControl(""));
00121   help->pushSlot(new NullControl("You can also save"));
00122   help->pushSlot(new NullControl("intermediate sets"));
00123   help->pushSlot(new NullControl("and concatenate them"));
00124   help->pushSlot(new NullControl("later."));
00125   help->pushSlot(new NullControl(""));
00126   help->pushSlot(new NullControl("IMPORTANT: It is"));
00127   help->pushSlot(new NullControl("recommended to keep"));
00128   help->pushSlot(new NullControl("a telnet connection"));
00129   help->pushSlot(new NullControl("to port 59000 open"));
00130   help->pushSlot(new NullControl("so you can log data"));
00131   help->pushSlot(new NullControl("samples there, in"));
00132   help->pushSlot(new NullControl("case of an"));
00133   help->pushSlot(new NullControl("unexpected crash,"));
00134   help->pushSlot(new NullControl("freeze, or shutdown."));
00135   help->pushSlot(new NullControl("Each data sample is"));
00136   help->pushSlot(new NullControl("a line of 6 numbers."));
00137   help->pushSlot(new NullControl(""));
00138   help->pushSlot(new NullControl("You will then need"));
00139   help->pushSlot(new NullControl("to run the matlab"));
00140   help->pushSlot(new NullControl("function in"));
00141   help->pushSlot(new NullControl("tools/WalkCalibration.m"));
00142   help->pushSlot(new NullControl("(Linear Least"));
00143   help->pushSlot(new NullControl("Squares) on these"));
00144   help->pushSlot(new NullControl("files to get the"));
00145   help->pushSlot(new NullControl("calibration matrix."));
00146   help->pushSlot(new NullControl(""));
00147   help->pushSlot(new NullControl("Copy this matrix"));
00148   help->pushSlot(new NullControl("back on to the"));
00149   help->pushSlot(new NullControl("memory stick and use"));
00150   help->pushSlot(new NullControl("the \"Load"));
00151   help->pushSlot(new NullControl("Calibration\""));
00152   help->pushSlot(new NullControl("control to apply it"));
00153   help->pushSlot(new NullControl("to the current walk."));
00154   help->pushSlot(new NullControl(""));
00155   help->pushSlot(new NullControl("You will then need"));
00156   help->pushSlot(new NullControl("to save the walk"));
00157   help->pushSlot(new NullControl("itself to retain the"));
00158   help->pushSlot(new NullControl("calibration for the"));
00159   help->pushSlot(new NullControl("next reboot."));
00160 
00161 
00162   load=new FileInputControl("Load Data Set","Load data files - select any file from the set, all will be loaded","/ms");
00163   save=new StringInputControl("Save Data Set","Saves current data (/ms)","Enter the base name (up to 6 char)");
00164   measure=new NullControl("Take Measurements","Begins the data gathering process");
00165   measure->pushSlot(new NullControl());
00166   clear=new NullControl("Clear Data","Clear the current data and start over");
00167   clear->pushSlot(new NullControl());
00168   for(unsigned int i=0; i<NUM_SRC; i++)
00169     cnts[i]=0;
00170   setupRoot();
00171 }
00172 
00173 
00174 WalkCalibration::~WalkCalibration() {
00175   options.clear();
00176   delete help;
00177   delete load;
00178   delete save;
00179   delete measure;
00180   delete clear;
00181 }
00182 
00183 ControlBase * WalkCalibration::activate(MotionManager::MC_ID disp_id, Socket * gui) {
00184   erouter->addListener(this,EventBase::locomotionEGID);
00185   return ControlBase::activate(disp_id, gui);
00186 }
00187 
00188 
00189 void WalkCalibration::refresh() {
00190   if(load->getLastInput()!=lastLoad) {
00191     for(int i=0; i<NUM_SRC; i++) {
00192       unsigned int strlen=load->getLastInput().size();
00193       loadData(load->getLastInput().substr(0,strlen-6)+datanames[i]+std::string(".txt"),data[i]);
00194       cnts[i]=data[i].size();
00195     }
00196     lastLoad=load->getLastInput();
00197   }
00198   if(save->getLastInput().size()>0) {
00199     for(int i=0; i<NUM_SRC; i++)
00200       saveData(save->getLastInput()+datanames[i]+std::string(".txt"),data[i]);
00201     save->takeInput("");
00202   }
00203   if(st==READY && (curType==fs || curType==bs))
00204     isPolar=polar->getStatus();
00205   ControlBase::refresh();
00206   if(gui_comm!=NULL && wireless->isConnected(gui_comm->sock)) {
00207     if(status.size()==0)
00208       gui_comm->printf("status\n# Samples: fs=%d fr=%d sr=%d br=%d bs=%d r=%d\n",cnts[0], cnts[1], cnts[2], cnts[3], cnts[4], cnts[5]);
00209     else
00210       gui_comm->printf("status\n%s\n",status.c_str());
00211   }
00212 }
00213 
00214 ControlBase* WalkCalibration::doSelect() {
00215   if(st==ROOT) {
00216     if(hilights.size()==0)
00217       return this;
00218     if(options[hilights.front()]==measure) {
00219       sndman->PlayFile(config->controller.select_snd);
00220       setupChoose();
00221       refresh();
00222       return this;
00223     } else if(options[hilights.front()]==clear) {
00224       sndman->PlayFile(config->controller.select_snd);
00225       setupClear();
00226       refresh();
00227       return this;
00228     } else {
00229       return ControlBase::doSelect();
00230     }
00231   } else if(st==CLEAR) {
00232     if(hilights.size()==0)
00233       return this;
00234     sndman->PlayFile(config->controller.select_snd);
00235     if(hilights.front()!=0) {
00236       sout->printf("Clearing data...\n");
00237       sndman->PlayFile(config->controller.cancel_snd);
00238       for(int i=0; i<NUM_SRC; i++) {
00239         clearData(data[i]);
00240         cnts[i]=0;
00241       }
00242     }
00243     setupRoot();
00244     refresh();
00245     return this;
00246   } else if(st==CHOOSE) {
00247     if(hilights.size()==0)
00248       return this;
00249     if(hilights.front()-2<NUM_SRC) {
00250       curType=static_cast<dataSource>(hilights.front()-2);
00251       sndman->PlayFile(config->controller.select_snd);
00252       setupReady();
00253       refresh();
00254       return this;
00255     }
00256     sndman->PlayFile(config->controller.cancel_snd);
00257     setupRoot();
00258     refresh();
00259     return this;
00260   } else if(st==READY) {
00261     if(hilights.size()==0 || options[hilights.front()]->getName()=="Go!") {
00262       sndman->PlayFile(config->controller.select_snd);
00263       setupMoving();
00264       refresh();
00265       return this;
00266     }
00267     if(options[hilights.front()]==polar || options[hilights.front()]==rect) {
00268       return ControlBase::doSelect();
00269     }
00270     sndman->PlayFile(config->controller.cancel_snd);
00271     setupChoose();
00272     refresh();
00273     return this;
00274   } else {
00275     sndman->PlayFile(config->controller.cancel_snd);
00276     setupChoose();
00277     refresh();
00278     return this;
00279   }
00280 }
00281 
00282 void WalkCalibration::processEvent(const EventBase& e) {
00283   if(st==MOVING) {
00284     stopTime=e.getTimeStamp();
00285     sout->printf("Ran for %g seconds\n",(stopTime-startTime)/1000.f);
00286     sndman->PlayFile(config->controller.select_snd);
00287     setupReading1();
00288     refresh();
00289     if(curType!=5)
00290       doReadStdIn(std::string("Enter ")+getFirstMeasure(curType));
00291     else
00292       doReadStdIn(std::string("Enter ")+getSecondMeasure(curType));
00293   }
00294 }
00295  
00296 
00297 ControlBase * WalkCalibration::takeInput(const std::string& msg) {
00298   if(st!=READING_1 && st!=READING_2)
00299     return ControlBase::takeInput(msg);
00300   else {
00301     char * end;
00302     const char * str=msg.c_str();
00303     if(st==READING_1) {
00304       firstIn=strtod(str,&end);
00305       if(end==str) {
00306         err("Invalid input: "+msg);
00307         return doReadStdIn(std::string("Enter ")+getFirstMeasure(curType));
00308       }
00309       sndman->PlayFile(config->controller.select_snd);
00310       setupReading2();
00311       refresh();
00312       if(*end!='\0')
00313         return takeInput(end);
00314       else
00315         return doReadStdIn(std::string("Enter ")+getSecondMeasure(curType));
00316     }
00317     if(st==READING_2) {
00318       secondIn=strtod(str,&end);
00319       if(end==str) {
00320         err("Invalid input: "+msg);
00321         return doReadStdIn(std::string("Enter ")+getSecondMeasure(curType));
00322       }
00323       addSample();
00324       sndman->PlayFile(config->controller.select_snd);
00325       setupReady();
00326       refresh();
00327       return this;
00328     }
00329     return NULL;
00330   }
00331 }
00332  
00333 
00334 void WalkCalibration::setHilights(const std::vector<unsigned int>& hi) {
00335   if(st==ROOT || st==CLEAR)
00336     ControlBase::setHilights(hi);
00337   else {
00338     std::vector<unsigned int> h;
00339     for(unsigned int i=0; i<hi.size(); i++)
00340       if(options[hi[i]]!=NULL && options[hi[i]]->slotsSize()>0 || (st==READY && options[hi[i]]==polar || options[hi[i]]==rect))
00341         h.push_back(hi[i]);
00342     ControlBase::setHilights(h);
00343   }
00344 }
00345  
00346 
00347 void WalkCalibration::hilightFirst() {
00348   if(st==ROOT || st==CLEAR)
00349     ControlBase::hilightFirst();
00350   else {
00351     std::vector<unsigned int> h;
00352     for(unsigned int i=0; i<options.size(); i++)
00353       if(options[i]!=NULL && options[i]->slotsSize()>0) {
00354         h.push_back(i);
00355         break;
00356       }
00357     ControlBase::setHilights(h);    
00358   }
00359 }
00360  
00361 void WalkCalibration::loadData(const std::string& n, std::vector<float*>& dat) {
00362   clearData(dat);
00363   sout->printf("Loading data from '%s'...\n",n.c_str());
00364   std::ifstream in(n.c_str());
00365   if(!in) {
00366     err("Could not open file for saving");
00367     return;
00368   }
00369   while(in) {
00370     dat.push_back(new float[6]);
00371     for(unsigned int c=0; c<6; c++) {
00372       in >> dat.back()[c];
00373       if(!in) {
00374         if(c!=0)
00375           err("Data file ended mid row");
00376         break;
00377       }
00378     }
00379   }
00380   delete [] dat.back();
00381   dat.pop_back();
00382   in.close();
00383 }
00384 
00385 void WalkCalibration::saveData(const std::string& n, const std::vector<float*>& dat) {
00386   std::string p=( n.substr(0,3)=="/ms/" ? n : "/ms/"+n );
00387   sout->printf("Saving data to '%s'...\n",p.c_str());
00388   std::ofstream out(p.c_str());
00389   if(!out) {
00390     err("Could not open file for saving");
00391     return;
00392   }
00393   unsigned int i=0;
00394   for(; i<dat.size(); i++) {
00395     for(unsigned int c=0; c<6; c++)
00396       out << dat[i][c] << ' ';
00397     out << endl;
00398     if(!out) {
00399       err("It appears the memory stick ran out of space?");
00400       break;
00401     }
00402   }
00403   out.close();
00404   sout->printf("%d rows written\n",i);
00405 }
00406 
00407 void WalkCalibration::clearData(std::vector<float*>& dat) {
00408   for(unsigned int i=0; i<dat.size(); i++)
00409     delete [] dat[i];
00410   dat.clear();
00411 }
00412 
00413 void WalkCalibration::setupRoot() {
00414   clearSlots();
00415   setName("Interactive Calibration");
00416   status="";
00417   pushSlot(help);
00418   pushSlot(load);
00419   pushSlot(save);
00420   pushSlot(measure);
00421   pushSlot(clear);
00422   st=ROOT;
00423 }
00424 
00425 void WalkCalibration::setupChoose() {
00426   bool isFromRoot=(st==ROOT);
00427   if(isFromRoot) {
00428     options.clear();
00429     hilights.clear();
00430   } else
00431     clearSlots();
00432   pushSlot(new NullControl("Choose Category"));
00433   pushSlot(NULL);
00434   NullControl * tmp;
00435   for(unsigned int i=0; i<NUM_SRC; i++) {
00436     dataSource s=static_cast<dataSource>(i);
00437     if(s==r)
00438       pushSlot(tmp=new NullControl(getIndexName(getSecondIndex(s))));
00439     else
00440       pushSlot(tmp=new NullControl(getIndexName(getFirstIndex(s))+std::string("/")+getIndexName(getSecondIndex(s))));
00441     tmp->pushSlot(NULL);
00442   }
00443   pushSlot(NULL);
00444   pushSlot(tmp=new NullControl(isFromRoot?"Cancel":"Done"));
00445   tmp->pushSlot(NULL);
00446   st=CHOOSE;
00447 }
00448 
00449 void WalkCalibration::setupReady() {
00450   polar=rect=NULL;
00451   clearSlots();
00452   pushSlot(new NullControl("Ready to record"));
00453   if(curType==r)
00454     pushSlot(new NullControl(string_util::makeUpper(getIndexName(getSecondIndex(curType)))));
00455   else
00456     pushSlot(new NullControl(string_util::makeUpper(getIndexName(getFirstIndex(curType)))+std::string("/")+string_util::makeUpper(getIndexName(getSecondIndex(curType)))));
00457   pushSlot(NULL);
00458   switch(curType) {
00459   case fs:
00460     pushSlot(new NullControl("Choose any forward"));
00461     pushSlot(new NullControl("and sideways vel."));
00462     pushSlot(new NullControl("Then tweak rotation"));
00463     pushSlot(new NullControl("to maintain heading"));
00464     polar=new ToggleControl("Take polar measurements","Check this if you want to take measurements as displacement,heading",new ToggleControl::RadioGroup());
00465     rect=new ToggleControl("Take cartesian measurements","Check this if you want to take measurements as x,y",polar->getRadioGroup());
00466     if(isPolar)
00467       polar->setStatus(true);
00468     else
00469       rect->setStatus(true);
00470     pushSlot(polar);
00471     pushSlot(rect);
00472     break;
00473   case fr:
00474     pushSlot(new NullControl("Choose any rotational"));
00475     pushSlot(new NullControl("and forward vel."));
00476     pushSlot(new NullControl("Tweak sideways vel."));
00477     pushSlot(new NullControl("to line up heading"));
00478     pushSlot(new NullControl("on opposite side of"));
00479     pushSlot(new NullControl("circle."));
00480     break;
00481   case sr:
00482     pushSlot(new NullControl("Choose any sideways"));
00483     pushSlot(new NullControl("vel.  Tweak forward"));
00484     pushSlot(new NullControl("vel. to cancel drift"));
00485     pushSlot(new NullControl("Then choose any"));
00486     pushSlot(new NullControl("rotational vel."));
00487     break;
00488   case br:
00489     pushSlot(new NullControl("Choose any rotational"));
00490     pushSlot(new NullControl("and sideways vel."));
00491     pushSlot(new NullControl("Tweak forward vel."));
00492     pushSlot(new NullControl("to line up heading"));
00493     pushSlot(new NullControl("on opposite side of"));
00494     pushSlot(new NullControl("circle."));
00495     break;
00496   case bs:
00497     pushSlot(new NullControl("Choose any forward"));
00498     pushSlot(new NullControl("and sideways vel."));
00499     pushSlot(new NullControl("Then tweak rotation"));
00500     pushSlot(new NullControl("to maintain heading"));
00501     polar=new ToggleControl("Take polar measurements","Check this if you want to take measurements as displacement,heading",new ToggleControl::RadioGroup());
00502     rect=new ToggleControl("Take cartesian measurements","Check this if you want to take measurements as x,y",polar->getRadioGroup());
00503     polar->setStatus(true);
00504     if(isPolar)
00505       polar->setStatus(true);
00506     else
00507       rect->setStatus(true);
00508     pushSlot(polar);
00509     pushSlot(rect);
00510     break;
00511   case r:
00512     pushSlot(new NullControl("Choose any"));
00513     pushSlot(new NullControl("rotational vel."));
00514     pushSlot(new NullControl("Tweak others to"));
00515     pushSlot(new NullControl("center rotation"));
00516     break;
00517   case NUM_SRC:
00518     {
00519       pushSlot(new NullControl("An error has occured."));
00520       pushSlot(new NullControl("Please go back and"));
00521       pushSlot(new NullControl("try again"));
00522       pushSlot(NULL);
00523       NullControl * tmp;
00524       pushSlot(tmp=new NullControl((st==CHOOSE)?"Cancel":"Done"));
00525       tmp->pushSlot(new NullControl());
00526       st=READY;
00527       return;
00528     }
00529   }
00530   pushSlot(NULL);
00531   NullControl * tmp;
00532   pushSlot(tmp=new NullControl("Go!"));
00533   tmp->pushSlot(new NullControl());
00534   pushSlot(tmp=new NullControl((st==CHOOSE)?"Cancel":"Done"));
00535   tmp->pushSlot(new NullControl());
00536   st=READY;
00537 }
00538 
00539 void WalkCalibration::setupMoving() {
00540   old_x=state->vel_x;
00541   old_y=state->vel_y;
00542   old_a=state->vel_a;
00543   if(old_x+old_y+old_a==0) {
00544     err("The WorldState velocities are all 0.\n\n"
00545         "Make sure whatever code is causing the motion is throwing\n"
00546         "a LocomotionEvent, and that the EStop is off.  You can use\n"
00547         "the EventLogger to check for LocomotionEvents.");
00548     return;
00549   }
00550   switch(curType) {
00551   case fs:
00552   case fr:
00553     if(old_x<0) {
00554       err("Have to be moving FORWARD for this category");
00555       return;
00556     }
00557     break;
00558   case bs:
00559   case br:
00560     if(old_x>0) {
00561       err("Have to be moving BACKWARD for this category");
00562       return;
00563     }
00564     break;
00565   case NUM_SRC:
00566     err("Bad category");
00567     return;
00568   case sr:
00569   case r:
00570     ;
00571   }
00572   sout->printf("Velocity is: %g %g %g\n",old_x,old_y,old_a);
00573   clearSlots();
00574   pushSlot(new NullControl("Running..."));
00575   if(curType!=5)
00576     pushSlot(new NullControl(std::string(getIndexName(getFirstIndex(curType)))+"/"+std::string(getIndexName(getSecondIndex(curType)))));
00577   else
00578     pushSlot(new NullControl(getIndexName(getSecondIndex(curType))));
00579   pushSlot(NULL);
00580   pushSlot(new NullControl("Recording will stop"));
00581   pushSlot(new NullControl("on the next"));
00582   pushSlot(new NullControl("LocomotionEvent."));
00583   pushSlot(new NullControl("EStop is recommended."));
00584   char str[255];
00585   sprintf(str,"vel_x=%g",old_x);
00586   pushSlot(new NullControl(str));
00587   sprintf(str,"vel_y=%g",old_y);
00588   pushSlot(new NullControl(str));
00589   sprintf(str,"vel_a=%g",old_a);
00590   pushSlot(new NullControl(str));
00591   pushSlot(NULL);
00592   NullControl * tmp;
00593   pushSlot(tmp=new NullControl("Cancel"));
00594   tmp->pushSlot(new NullControl());
00595   st=MOVING;
00596   startTime=get_time();
00597 }
00598 
00599 void WalkCalibration::setupReading1() {
00600   if(curType==5) {
00601     setupReading2();
00602     return;
00603   }
00604   clearSlots();
00605   pushSlot(new NullControl("Reading first input"));
00606   pushSlot(NULL);
00607   pushSlot(new NullControl("Please enter:"));
00608   pushSlot(new NullControl(getFirstMeasure(curType)));
00609   pushSlot(NULL);
00610   NullControl * tmp;
00611   pushSlot(tmp=new NullControl("Cancel"));
00612   tmp->pushSlot(new NullControl());
00613   st=READING_1;
00614 }
00615 
00616 void WalkCalibration::setupReading2() {
00617   clearSlots();
00618   if(curType==5)
00619     pushSlot(new NullControl("Reading input"));
00620   else
00621     pushSlot(new NullControl("Reading second input"));
00622   pushSlot(NULL);
00623   pushSlot(new NullControl("Please enter:"));
00624   pushSlot(new NullControl(getSecondMeasure(curType)));
00625   pushSlot(NULL);
00626   NullControl * tmp;
00627   pushSlot(tmp=new NullControl("Cancel"));
00628   tmp->pushSlot(new NullControl());
00629   st=READING_2;
00630 }
00631 
00632 
00633 
00634 void WalkCalibration::setupClear() {
00635   options.clear();
00636   setName("Clear Data");
00637   char tmp[255];
00638   sprintf(tmp,"There are %d samples.  Are you sure?",cnts[0]+cnts[1]+cnts[2]+cnts[3]+cnts[4]+cnts[5]);
00639   status=tmp;
00640   pushSlot(new NullControl("No","Don't clear the data"));
00641   pushSlot(new NullControl("Yes","Start over"));
00642   hilightFirst();
00643   st=CLEAR;
00644 }
00645 
00646 unsigned int WalkCalibration::getFirstIndex(WalkCalibration::dataSource t) {
00647   switch(t) {
00648   case fs:
00649   case fr:
00650     return 0;
00651   case sr:
00652     return 1;
00653   case bs:
00654   case br:
00655     return 3;
00656   case r:
00657   case NUM_SRC:
00658     return -1U;
00659   }
00660   return -1U;
00661 }
00662 
00663 unsigned int WalkCalibration::getSecondIndex(WalkCalibration::dataSource t) {
00664   switch(t) {
00665   case fs:
00666   case bs:
00667     return 1;
00668   case fr:
00669   case sr:
00670   case br:
00671   case r:
00672     return 2;
00673   case NUM_SRC:
00674     return -1U;
00675   }
00676   return -1U;
00677 }
00678 
00679 char * WalkCalibration::getIndexName(unsigned int t) {
00680   switch(t) {
00681   case 0:
00682     return "Forward";
00683   case 1:
00684     return "Strafe";
00685   case 2:
00686     return "Rotate";
00687   case 3:
00688     return "Backward";
00689   }
00690   return NULL;
00691 }
00692 
00693 char * WalkCalibration::getFirstMeasure(WalkCalibration::dataSource t) {
00694   switch(t) {
00695   case fs:
00696     if(isPolar)
00697       return "displacement";
00698     else
00699       return "forward distance";
00700   case fr:
00701   case sr:
00702   case br:
00703     return "displacement";
00704   case bs:
00705     if(isPolar)
00706       return "displacement";
00707     else
00708     return "backward distance";
00709   case r:
00710     return NULL;
00711   case NUM_SRC:
00712     return NULL;
00713   }
00714   return NULL;
00715 }
00716 
00717 char * WalkCalibration::getSecondMeasure(WalkCalibration::dataSource t) {
00718   switch(t) {
00719   case fs:
00720   case bs:
00721     if(isPolar)
00722       return "bearing";
00723     else
00724       return "sideways distance";
00725   case fr:
00726   case sr:
00727   case br:
00728   case r:
00729     return "angular distance";
00730   case NUM_SRC:
00731     return NULL;
00732   }
00733   return NULL;
00734 }
00735 
00736 float WalkCalibration::arclen(float d, float a, float sign) {
00737   if(fabs(a)<=1) //if the angle is small (in particular, 0) leads to numerical instability.
00738     return (sign<0)?-fabs(d):fabs(d);
00739   //otherwise, convert to radians
00740   a*=M_PI/180;
00741   //calculate radius of the circle we're tracing (there's some simple geometry to get this)
00742   float radius=fabs(d/2/sin(a/2));
00743   //and return the length of the sector we've traced (with the requested sign)
00744   if(sign<0)
00745     return -a*radius;
00746   return a*radius;
00747 }
00748 
00749 
00750 //this will allow you to solve for the conversion from commands to actual
00751 //which I was planning to then invert for the conversion from desired to command
00752 //but then I realized I could just solve for the inverted for directly
00753 // (never was actually tested btw, and also out of date with rest of this code)
00754 /*
00755 void WalkCalibration::addSample() {
00756   switch(getType()) {
00757   case 0: //fs
00758     addSample(forward,0, firstIn*10);
00759     addSample(forward,1, secondIn*10);
00760     addSample(forward,2, 0);
00761     break;
00762   case 1: //fr
00763     addSample(forward,0, arclen(firstIn*10,secondIn, 1));
00764     addSample(forward,1, 0);
00765     addSample(forward,2, (old_a>0?secondIn:-secondIn)*M_PI/180);
00766     break;
00767   case 2: //sr
00768     addSample(forward,0, 0);
00769     addSample(reverse,0, 0);
00770     addSample(forward,1, arclen(firstIn*10,secondIn,old_y));
00771     addSample(reverse,1, arclen(firstIn*10,secondIn,old_y));
00772     addSample(forward,2, (old_a>0?secondIn:-secondIn)*M_PI/180);
00773     addSample(reverse,2, (old_a>0?secondIn:-secondIn)*M_PI/180);
00774     break;
00775   case 3: //br
00776     addSample(reverse,0, arclen(firstIn*10,secondIn,-1));
00777     addSample(reverse,1, 0);
00778     addSample(reverse,2, (old_a>0?secondIn:-secondIn)*M_PI/180);
00779     break;
00780   case 4: //bs
00781     addSample(reverse,0, firstIn*10);
00782     addSample(reverse,1, secondIn*10);
00783     addSample(reverse,2, 0);
00784     break;
00785   case 5: //r
00786     addSample(forward,2, (old_a>0?secondIn:-secondIn)*M_PI/180);
00787     addSample(reverse,2, (old_a>0?secondIn:-secondIn)*M_PI/180);
00788     break;
00789   }
00790   cnts[getType()]++;
00791 }
00792 */
00793 
00794 void WalkCalibration::addSample() {
00795   float dx=0, dy=0, da=0;
00796   float dt=(stopTime-startTime)/1000.f;
00797   switch(curType) {
00798   case fs:
00799     if(isPolar) {
00800       dx=firstIn*cos(secondIn*M_PI/180)*10/dt;
00801       dy=firstIn*sin(secondIn*M_PI/180)*10/dt;
00802     } else {
00803       dx=firstIn*10/dt;
00804       dy=secondIn*10/dt;
00805     }
00806     addSample(data[curType], dx,dy,da);
00807     report(0,old_x,dx);
00808     report(1,old_y,dy);
00809     break;
00810   case fr:
00811     dx=arclen(firstIn*10,secondIn, 1)/dt;
00812     da=(old_a>0?secondIn:-secondIn)*M_PI/180/dt;
00813     addSample(data[curType], dx,dy,da);
00814     report(0,old_x,dx);
00815     report(2,old_a,da);
00816     break;
00817   case sr:
00818     dy=arclen(firstIn*10,secondIn,old_y)/dt;
00819     da=(old_a>0?secondIn:-secondIn)*M_PI/180/dt;
00820     addSample(data[curType], dx,dy,da);
00821     report(1,old_y,dy);
00822     report(2,old_a,da);
00823     break;
00824   case br:
00825     dx=arclen(firstIn*10,secondIn,-1)/dt;
00826     da=(old_a>0?secondIn:-secondIn)*M_PI/180/dt;
00827     addSample(data[curType], dx,dy,da);
00828     report(0,old_x,dx);
00829     report(2,old_a,da);
00830     break;
00831   case bs:
00832     if(isPolar) {
00833       dx=firstIn*cos(secondIn*M_PI/180)*10/dt;
00834       dy=firstIn*sin(secondIn*M_PI/180)*10/dt;
00835     } else {
00836       dx=firstIn*10/dt;
00837       dy=secondIn*10/dt;
00838     }
00839     addSample(data[curType], dx,dy,da);
00840     report(0,old_x,dx);
00841     report(1,old_y,dy);
00842     break;
00843   case r:
00844     da=(old_a>0?secondIn:-secondIn)*M_PI/180/dt;
00845     addSample(data[curType], dx,dy,da);
00846     report(2,old_a,da);
00847     break;
00848   default:
00849     serr->printf("unknown type! %d\n",curType);
00850     return;
00851   }
00852   cnts[curType]++;
00853 }
00854 
00855 void WalkCalibration::report(unsigned int row, float cmd, float actual) {
00856   switch(row) {
00857   case 0:
00858     sout->printf("Forwrd command=%6.4g  Actual=%6.4g\n",cmd,actual); break;
00859   case 1:
00860     sout->printf("Strafe command=%6.4g  Actual=%6.4g\n",cmd,actual); break;
00861   case 2:
00862     sout->printf("Rotate command=%6.4g  Actual=%6.4g\n",cmd,actual); break;
00863   }
00864 }
00865  
00866 void WalkCalibration::err(const std::string& str) {
00867     std::vector<std::string> errmsg;
00868     errmsg.push_back("Error");
00869     errmsg.push_back(str);
00870     serr->printf("%s\n",str.c_str());
00871     Controller::loadGUI("org.tekkotsu.mon.ControllerErr","msg",0,errmsg);
00872     sndman->PlayFile(config->controller.error_snd);
00873     return;
00874 }
00875 
00876 
00877 void WalkCalibration::addSample(std::vector<float*>& dat, unsigned int row, float sol) {
00878   float * d=new float[13];
00879   dat.push_back(d);
00880   for(unsigned int i=0; i<12; i++)
00881     d[i]=0;
00882   
00883   d[row*4+0]=old_x;
00884   d[row*4+1]=old_y;
00885   d[row*4+2]=old_a;
00886   d[row*4+3]=1;
00887   d[12]=sol/(stopTime-startTime);
00888 
00889   for(unsigned int i=0; i<13; i++)
00890     cout << d[i] << ' ';
00891   cout << endl;
00892 }
00893 
00894 void WalkCalibration::addSample(std::vector<float*>& dat, float x, float y, float a) {
00895   float * d=new float[6];
00896   dat.push_back(d);
00897   d[0]=x;
00898   d[1]=y;
00899   d[2]=a;
00900   d[3]=old_x;
00901   d[4]=old_y;
00902   d[5]=old_a;
00903 
00904   for(unsigned int i=0; i<6; i++)
00905     cout << d[i] << ' ';
00906   cout << endl;
00907 }
00908   
00909 
00910 /*! @file
00911  * @brief 
00912  * @author ejt (Creator)
00913  *
00914  * $Author: ejt $
00915  * $Name: tekkotsu-2_2 $
00916  * $Revision: 1.8 $
00917  * $State: Exp $
00918  * $Date: 2004/03/01 21:17:08 $
00919  */
00920 

Tekkotsu v2.2
Generated Tue Oct 19 14:19:16 2004 by Doxygen 1.3.9.1