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)
00739 return (sign<0)?-fabs(d):fabs(d);
00740
00741 a*=M_PI/180;
00742
00743 float radius=fabs(d/2/sin(a/2));
00744
00745 if(sign<0)
00746 return -a*radius;
00747 return a*radius;
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
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
00912
00913
00914
00915
00916
00917
00918
00919
00920
00921