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

Tekkotsu v2.0
Generated Wed Jan 21 03:20:30 2004 by Doxygen 1.3.4