WalkCalibration.ccGo 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)
00742 return (sign<0)?-fabs(d):fabs(d);
00743
00744 a*=M_PI/180;
00745
00746 float radius=fabs(d/2/sin(a/2));
00747
00748 if(sign<0)
00749 return -a*radius;
00750 return a*radius;
00751 }
00752
00753
00754
00755
00756
00757
00758
00759
00760
00761
00762
00763
00764
00765
00766
00767
00768
00769
00770
00771
00772
00773
00774
00775
00776
00777
00778
00779
00780
00781
00782
00783
00784
00785
00786
00787
00788
00789
00790
00791
00792
00793
00794
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
00897
00898
00899
00900
00901
00902
00903
00904
00905
00906
|