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)
00735 return (sign<0)?-fabs(d):fabs(d);
00736
00737 a*=M_PI/180;
00738
00739 float radius=fabs(d/2/sin(a/2));
00740
00741 if(sign<0)
00742 return -a*radius;
00743 return a*radius;
00744 }
00745
00746
00747
00748
00749
00750
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 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
00908
00909
00910
00911
00912
00913
00914
00915
00916
00917