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

Tekkotsu v2.4.1
Generated Tue Aug 16 16:32:49 2005 by Doxygen 1.4.4