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