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

Tekkotsu v2.2.2
Generated Tue Jan 4 15:43:16 2005 by Doxygen 1.4.0