00001 #include "Robots/Scorbot/ScorbotConsole.qt.H"
00002 #include "QtGui/qboxlayout.h"
00003 #include "Image/DrawOps.H"
00004
00005 ScorbotConsole::ScorbotConsole(OptionManager& mgr,
00006 const std::string& descrName,
00007 const std::string& tagName) :
00008 ModelComponent(mgr, descrName, tagName)
00009 {
00010 jointStringMap[ScorbotIce::Base] = "Base";
00011 jointStringMap[ScorbotIce::Shoulder] = "Shoulder";
00012 jointStringMap[ScorbotIce::Elbow] = "Elbow";
00013 jointStringMap[ScorbotIce::Wrist1] = "Wrist1";
00014 jointStringMap[ScorbotIce::Wrist2] = "Wrist2";
00015 jointStringMap[ScorbotIce::Gripper] = "Gripper";
00016 jointStringMap[ScorbotIce::Slider] = "Slider";
00017
00018 itsSettings = new QSettings("iLab", "ScorbotConsole");
00019
00020 itsMaxPlotLength = 100;
00021
00022 int IceError = false;
00023 try
00024 {
00025 int argc = 1;
00026 char* argv[1];
00027 argv[0] = new char[128];
00028 sprintf(argv[0], "app-ScorbotConsole");
00029 ic = Ice::initialize(argc, argv);
00030 Ice::ObjectPrx base = ic->stringToProxy(
00031 "ScorbotServer:default -p 10000 -h ihead");
00032 itsScorbot = ScorbotIce::ScorbotPrx::checkedCast(base);
00033 if(!itsScorbot)
00034 throw "Invalid Proxy";
00035 }
00036 catch(const Ice::Exception& ex)
00037 {
00038 std::cerr << ex << std::endl;
00039 IceError = true;
00040 }
00041 catch(const char* msg)
00042 {
00043 std::cerr << msg << std::endl;
00044 IceError = false;
00045 }
00046
00047 if(IceError)
00048 exit(-1);
00049
00050
00051 itsScorbotThread = new ScorbotInterfaceThread(this,&itsScorbot);
00052 connect(itsScorbotThread, SIGNAL(gotGravityCompensation(float)), this, SLOT(gotGravityCompensation(float)));
00053 connect(itsScorbotThread, SIGNAL(gotEncoderVal(int, int)), this, SLOT(gotEncoderVal(int, int)));
00054 connect(itsScorbotThread, SIGNAL(gotPWMVal(int, float)), this, SLOT(gotPWMVal(int, float)));
00055 connect(itsScorbotThread, SIGNAL(gotTargetPos(int, int)), this, SLOT(gotTargetPos(int, int)));
00056 connect(itsScorbotThread, SIGNAL(updatedAllValues()), this, SLOT(updatePlots()));
00057 }
00058
00059
00060 ScorbotConsole::~ScorbotConsole()
00061 {
00062 if(ic)
00063 ic->destroy();
00064 }
00065
00066
00067
00068 void ScorbotConsole::loadParams()
00069 {
00070
00071 for(int jointIdx=(int)ScorbotIce::Base; jointIdx<=(int)ScorbotIce::Slider; jointIdx++)
00072 {
00073 ScorbotIce::JointType joint = (ScorbotIce::JointType)jointIdx;
00074 QString jointName = jointStringMap[joint];
00075
00076 if(!itsSettings->contains(jointName+"/pGain")) continue;
00077
00078 float pGain = itsSettings->value(jointName + "/pGain").toDouble();
00079 float iGain = itsSettings->value(jointName + "/iGain").toDouble();
00080 float dGain = itsSettings->value(jointName + "/dGain").toDouble();
00081 float maxI = itsSettings->value(jointName + "/maxI").toDouble();
00082 float maxPWM = itsSettings->value(jointName + "/maxPWM").toDouble();
00083 float pwmOffset = itsSettings->value(jointName + "/pwmOffset").toDouble();
00084
00085 itsScorbot->setControlParams(joint, pGain, iGain, dGain, maxI, maxPWM, pwmOffset);
00086 }
00087
00088 int32 foreArmMass, upperArmMass;
00089 float gravityScale;
00090 foreArmMass = itsSettings->value("ForeArmMass").toInt();
00091 upperArmMass = itsSettings->value("UpperArmMass").toInt();
00092 gravityScale = itsSettings->value("GravityScale").toDouble();
00093 itsScorbot->setGravityParameters(upperArmMass, foreArmMass, gravityScale);
00094
00095
00096 }
00097
00098
00099 void ScorbotConsole::saveParams()
00100 {
00101
00102 for(int jointIdx=(int)ScorbotIce::Base; jointIdx<=(int)ScorbotIce::Slider; jointIdx++)
00103 {
00104 ScorbotIce::JointType joint = (ScorbotIce::JointType)jointIdx;
00105 float pGain, iGain, dGain, maxI, maxPWM, pwmOffset;
00106 itsScorbot->getPIDVals(joint, pGain, iGain, dGain, maxI, maxPWM, pwmOffset);
00107
00108 QString jointName = jointStringMap[joint];
00109
00110 itsSettings->setValue(jointName + "/pGain", pGain);
00111 itsSettings->setValue(jointName + "/iGain", iGain);
00112 itsSettings->setValue(jointName + "/dGain", dGain);
00113 itsSettings->setValue(jointName + "/maxI", maxI);
00114 itsSettings->setValue(jointName + "/maxPWM", maxPWM);
00115 itsSettings->setValue(jointName + "/pwmOffset", pwmOffset);
00116 }
00117 int32 foreArmMass, upperArmMass;
00118 float gravityScale;
00119 itsScorbot->getGravityParameters(upperArmMass, foreArmMass, gravityScale);
00120 itsSettings->setValue("ForeArmMass", foreArmMass);
00121 itsSettings->setValue("UpperArmMass", upperArmMass);
00122 itsSettings->setValue("GravityScale", gravityScale);
00123 itsSettings->sync();
00124 delete itsSettings;
00125
00126 }
00127
00128
00129 void ScorbotConsole::stop1()
00130 {
00131
00132
00133
00134 itsScorbot->setEnabled(false);
00135 itsScorbotThread->running = false;
00136 LINFO("Waiting for shutdown...");
00137 while(!itsScorbotThread->isFinished())
00138 usleep(10000);
00139 }
00140
00141 void ScorbotConsole::start2()
00142 {
00143
00144
00145 QWidget* centralWidget = new QWidget(this);
00146 QVBoxLayout *mainVertLayout = new QVBoxLayout(centralWidget);
00147 QHBoxLayout *mainHorizLayout = new QHBoxLayout(centralWidget);
00148 mainVertLayout->addLayout(mainHorizLayout);
00149
00150
00151 QGridLayout *controlLayout = new QGridLayout(centralWidget);
00152 controlLayout->setVerticalSpacing(0);
00153
00154 int row = 0;
00155 int column = 0;
00156
00157
00158 QLabel* jointLbl = new QLabel("<b>Joint</b>", this);
00159 jointLbl->setAlignment(Qt::AlignHCenter);
00160 QLabel* pwmLbl = new QLabel("<b>PWM<b>", this);
00161 pwmLbl->setAlignment(Qt::AlignHCenter);
00162 QLabel* encoderLbl = new QLabel("<b>Encoder Val<b>", this);
00163 encoderLbl->setAlignment(Qt::AlignHCenter);
00164 QLabel* desiredLbl = new QLabel("<b>Desired Encoder<b>", this);
00165 desiredLbl->setAlignment(Qt::AlignHCenter);
00166 QLabel* durationLbl = new QLabel("<b>Duration<b>", this);
00167 durationLbl->setAlignment(Qt::AlignHCenter);
00168 QLabel* setLbl = new QLabel("<b>Set Position<b>", this);
00169 setLbl->setAlignment(Qt::AlignHCenter);
00170
00171 controlLayout->addWidget(jointLbl, row, column++);
00172 controlLayout->addWidget(pwmLbl, row, column++);
00173 controlLayout->addWidget(encoderLbl, row, column++);
00174 controlLayout->addWidget(desiredLbl, row, column++);
00175 controlLayout->addWidget(durationLbl, row, column++);
00176 controlLayout->addWidget(setLbl, row, column++);
00177 controlLayout->setRowStretch(0, 0);
00178
00179
00180 row=1; column=0;
00181 controlLayout->addWidget(new QLabel("<b>Base<b>", this) , row++, column);
00182 controlLayout->addWidget(new QLabel("<b>Shoulder<b>", this), row++, column);
00183 controlLayout->addWidget(new QLabel("<b>Elbow<b>", this) , row++, column);
00184 controlLayout->addWidget(new QLabel("<b>Wrist1<b>", this) , row++, column);
00185 controlLayout->addWidget(new QLabel("<b>Wrist2<b>", this) , row++, column);
00186 controlLayout->addWidget(new QLabel("<b>Gripper<b>", this) , row++, column);
00187 controlLayout->addWidget(new QLabel("<b>Slider<b>", this) , row++, column);
00188
00189
00190 row=1; column++;
00191 pwmLbls[ScorbotIce::Base] = new QLabel(centralWidget);
00192 controlLayout->addWidget(pwmLbls[ScorbotIce::Base], row++, column);
00193 pwmLbls[ScorbotIce::Shoulder] = new QLabel(centralWidget);
00194 controlLayout->addWidget(pwmLbls[ScorbotIce::Shoulder], row++, column);
00195 pwmLbls[ScorbotIce::Elbow] = new QLabel(centralWidget);
00196 controlLayout->addWidget(pwmLbls[ScorbotIce::Elbow], row++, column);
00197 pwmLbls[ScorbotIce::Wrist1] = new QLabel(centralWidget);
00198 controlLayout->addWidget(pwmLbls[ScorbotIce::Wrist1], row++, column);
00199 pwmLbls[ScorbotIce::Wrist2] = new QLabel(centralWidget);
00200 controlLayout->addWidget(pwmLbls[ScorbotIce::Wrist2], row++, column);
00201 pwmLbls[ScorbotIce::Gripper] = new QLabel(centralWidget);
00202 controlLayout->addWidget(pwmLbls[ScorbotIce::Gripper], row++, column);
00203 pwmLbls[ScorbotIce::Slider] = new QLabel(centralWidget);
00204 controlLayout->addWidget(pwmLbls[ScorbotIce::Slider], row++, column);
00205 for(QMap<ScorbotIce::JointType, QLabel*>::iterator it=pwmLbls.begin(); it!=pwmLbls.end(); ++it)
00206 { it.value()->setMaximumWidth(50); it.value()->setMinimumWidth(50); }
00207
00208
00209 row=1; column++;
00210 encoderLbls[ScorbotIce::Base] = new QLabel(centralWidget);
00211 controlLayout->addWidget(encoderLbls[ScorbotIce::Base], row++, column);
00212 encoderLbls[ScorbotIce::Shoulder] = new QLabel(centralWidget);
00213 controlLayout->addWidget(encoderLbls[ScorbotIce::Shoulder], row++, column);
00214 encoderLbls[ScorbotIce::Elbow] = new QLabel(centralWidget);
00215 controlLayout->addWidget(encoderLbls[ScorbotIce::Elbow], row++, column);
00216 encoderLbls[ScorbotIce::Wrist1] = new QLabel(centralWidget);
00217 controlLayout->addWidget(encoderLbls[ScorbotIce::Wrist1], row++, column);
00218 encoderLbls[ScorbotIce::Wrist2] = new QLabel(centralWidget);
00219 controlLayout->addWidget(encoderLbls[ScorbotIce::Wrist2], row++, column);
00220 encoderLbls[ScorbotIce::Gripper] = new QLabel(centralWidget);
00221 controlLayout->addWidget(encoderLbls[ScorbotIce::Gripper], row++, column);
00222 encoderLbls[ScorbotIce::Slider] = new QLabel(centralWidget);
00223 controlLayout->addWidget(encoderLbls[ScorbotIce::Slider], row++, column);
00224 for(QMap<ScorbotIce::JointType, QLabel*>::iterator it=encoderLbls.begin(); it!=encoderLbls.end(); ++it)
00225 { it.value()->setMaximumWidth(50); it.value()->setMinimumWidth(50); }
00226
00227
00228 row=1; column++;
00229 encoderEdits[ScorbotIce::Base] = new QLineEdit(centralWidget);
00230 controlLayout->addWidget(encoderEdits[ScorbotIce::Base], row++, column);
00231 encoderEdits[ScorbotIce::Shoulder] = new QLineEdit(centralWidget);
00232 controlLayout->addWidget(encoderEdits[ScorbotIce::Shoulder], row++, column);
00233 encoderEdits[ScorbotIce::Elbow] = new QLineEdit(centralWidget);
00234 controlLayout->addWidget(encoderEdits[ScorbotIce::Elbow], row++, column);
00235 encoderEdits[ScorbotIce::Wrist1] = new QLineEdit(centralWidget);
00236 controlLayout->addWidget(encoderEdits[ScorbotIce::Wrist1], row++, column);
00237 encoderEdits[ScorbotIce::Wrist2] = new QLineEdit(centralWidget);
00238 controlLayout->addWidget(encoderEdits[ScorbotIce::Wrist2], row++, column);
00239 encoderEdits[ScorbotIce::Gripper] = new QLineEdit(centralWidget);
00240 controlLayout->addWidget(encoderEdits[ScorbotIce::Gripper], row++, column);
00241 encoderEdits[ScorbotIce::Slider] = new QLineEdit(centralWidget);
00242 controlLayout->addWidget(encoderEdits[ScorbotIce::Slider], row++, column);
00243
00244
00245 row=1; column++;
00246 durationEdits[ScorbotIce::Base] = new QLineEdit(centralWidget);
00247 controlLayout->addWidget(durationEdits[ScorbotIce::Base], row++, column);
00248 durationEdits[ScorbotIce::Shoulder] = new QLineEdit(centralWidget);
00249 controlLayout->addWidget(durationEdits[ScorbotIce::Shoulder], row++, column);
00250 durationEdits[ScorbotIce::Elbow] = new QLineEdit(centralWidget);
00251 controlLayout->addWidget(durationEdits[ScorbotIce::Elbow], row++, column);
00252 durationEdits[ScorbotIce::Wrist1] = new QLineEdit(centralWidget);
00253 controlLayout->addWidget(durationEdits[ScorbotIce::Wrist1], row++, column);
00254 durationEdits[ScorbotIce::Wrist2] = new QLineEdit(centralWidget);
00255 controlLayout->addWidget(durationEdits[ScorbotIce::Wrist2], row++, column);
00256 durationEdits[ScorbotIce::Gripper] = new QLineEdit(centralWidget);
00257 controlLayout->addWidget(durationEdits[ScorbotIce::Gripper], row++, column);
00258 durationEdits[ScorbotIce::Slider] = new QLineEdit(centralWidget);
00259 controlLayout->addWidget(durationEdits[ScorbotIce::Slider], row++, column);
00260
00261
00262 row=1; column++;
00263 QSignalMapper* posButtonMapper = new QSignalMapper(this);
00264 setPosButtons[ScorbotIce::Base] = new QPushButton("Set Position", centralWidget);
00265 controlLayout->addWidget(setPosButtons[ScorbotIce::Base], row++, column);
00266 posButtonMapper->setMapping(setPosButtons[ScorbotIce::Base], (int)ScorbotIce::Base);
00267 connect(setPosButtons[ScorbotIce::Base], SIGNAL(pressed()), posButtonMapper, SLOT(map()));
00268
00269 setPosButtons[ScorbotIce::Shoulder] = new QPushButton("Set Position", centralWidget);
00270 controlLayout->addWidget(setPosButtons[ScorbotIce::Shoulder], row++, column);
00271 posButtonMapper->setMapping(setPosButtons[ScorbotIce::Shoulder], (int)ScorbotIce::Shoulder);
00272 connect(setPosButtons[ScorbotIce::Shoulder], SIGNAL(pressed()), posButtonMapper, SLOT(map()));
00273
00274 setPosButtons[ScorbotIce::Elbow] = new QPushButton("Set Position", centralWidget);
00275 controlLayout->addWidget(setPosButtons[ScorbotIce::Elbow], row++, column);
00276 posButtonMapper->setMapping(setPosButtons[ScorbotIce::Elbow], (int)ScorbotIce::Elbow);
00277 connect(setPosButtons[ScorbotIce::Elbow], SIGNAL(pressed()), posButtonMapper, SLOT(map()));
00278
00279 setPosButtons[ScorbotIce::Wrist1] = new QPushButton("Set Position", centralWidget);
00280 controlLayout->addWidget(setPosButtons[ScorbotIce::Wrist1], row++, column);
00281 posButtonMapper->setMapping(setPosButtons[ScorbotIce::Wrist1], (int)ScorbotIce::Wrist1);
00282 connect(setPosButtons[ScorbotIce::Wrist1], SIGNAL(pressed()), posButtonMapper, SLOT(map()));
00283
00284 setPosButtons[ScorbotIce::Wrist2] = new QPushButton("Set Position", centralWidget);
00285 controlLayout->addWidget(setPosButtons[ScorbotIce::Wrist2], row++, column);
00286 posButtonMapper->setMapping(setPosButtons[ScorbotIce::Wrist2], (int)ScorbotIce::Wrist2);
00287 connect(setPosButtons[ScorbotIce::Wrist2], SIGNAL(pressed()), posButtonMapper, SLOT(map()));
00288
00289 setPosButtons[ScorbotIce::Gripper] = new QPushButton("Set Position", centralWidget);
00290 controlLayout->addWidget(setPosButtons[ScorbotIce::Gripper], row++, column);
00291 posButtonMapper->setMapping(setPosButtons[ScorbotIce::Gripper], (int)ScorbotIce::Gripper);
00292 connect(setPosButtons[ScorbotIce::Gripper], SIGNAL(pressed()), posButtonMapper, SLOT(map()));
00293
00294 setPosButtons[ScorbotIce::Slider] = new QPushButton("Set Position", centralWidget);
00295 controlLayout->addWidget(setPosButtons[ScorbotIce::Slider], row++, column);
00296 posButtonMapper->setMapping(setPosButtons[ScorbotIce::Slider], (int)ScorbotIce::Slider);
00297 connect(setPosButtons[ScorbotIce::Slider], SIGNAL(pressed()), posButtonMapper, SLOT(map()));
00298
00299 connect(posButtonMapper, SIGNAL(mapped(int)), this, SLOT(setPosition(int)));
00300
00301
00302 controlLayout->addWidget(new QLabel("<b>Gravity Compensation<b>", this), ++row, 0);
00303 gravityCompLbl = new QLabel(this);
00304 gravityCompLbl->setMaximumWidth(50); gravityCompLbl->setMinimumWidth(50);
00305 controlLayout->addWidget(gravityCompLbl, row++, 1);
00306
00307
00308 row=1; column=7;
00309
00310 QPushButton* resetEncoderButton = new QPushButton("Reset Encoders",centralWidget);
00311 controlLayout->addWidget(resetEncoderButton, row++, column);
00312 connect(resetEncoderButton, SIGNAL(pressed()), this, SLOT(resetEncoders()));
00313
00314
00315 enableMotorsButton = new QPushButton("Enable Motors", centralWidget);
00316 controlLayout->addWidget(enableMotorsButton, row++, column);
00317 connect(enableMotorsButton, SIGNAL(pressed()), this, SLOT(toggleMotorsEnabled()));
00318 itsMotorsEnabled = false;
00319 itsScorbot->setEnabled(false);
00320
00321
00322 QPushButton* printPosCodeButton = new QPushButton("Print Position Code", centralWidget);
00323 controlLayout->addWidget(printPosCodeButton, row++, column);
00324 connect(printPosCodeButton, SIGNAL(pressed()), this, SLOT(printPosCode()));
00325
00326 mainHorizLayout->addLayout(controlLayout);
00327
00328 mainHorizLayout->addSpacing(10);
00329 mainHorizLayout->addStretch(10);
00330
00331
00332 row=1; column = 0;
00333 QGridLayout *pidLayout = new QGridLayout(this);
00334 itsAxisComboBox = new QComboBox(this);
00335 itsAxisComboBox->addItem("Base", 0);
00336 itsAxisComboBox->addItem("Shoulder", 1);
00337 itsAxisComboBox->addItem("Elbow", 2);
00338 itsAxisComboBox->addItem("Wrist1", 3);
00339 itsAxisComboBox->addItem("Wrist2", 4);
00340 itsAxisComboBox->addItem("Gripper", 5);
00341 itsAxisComboBox->addItem("Slider", 6);
00342 connect(itsAxisComboBox, SIGNAL(activated(int)), this, SLOT(pidAxisSelected(int)));
00343 pidLayout->addWidget(itsAxisComboBox, 0, 1);
00344
00345 pGainEdit = new QLineEdit(this);
00346 pidLayout->addWidget(new QLabel("P Gain", this), row, 0);
00347 pidLayout->addWidget(pGainEdit, row++, 1);
00348
00349 iGainEdit = new QLineEdit(this);
00350 pidLayout->addWidget(new QLabel("I Gain", this), row, 0);
00351 pidLayout->addWidget(iGainEdit, row++, 1);
00352
00353 dGainEdit = new QLineEdit(this);
00354 pidLayout->addWidget(new QLabel("D Gain", this), row, 0);
00355 pidLayout->addWidget(dGainEdit, row++, 1);
00356 maxIEdit = new QLineEdit(this);
00357 pidLayout->addWidget(new QLabel("Max I", this), row, 0);
00358 pidLayout->addWidget(maxIEdit, row++, 1);
00359 maxPWMEdit = new QLineEdit(this);
00360 pidLayout->addWidget(new QLabel("Max PWM", this), row, 0);
00361 pidLayout->addWidget(maxPWMEdit, row++, 1);
00362
00363 pwmOffsetEdit = new QLineEdit(this);
00364 pidLayout->addWidget(new QLabel("PWM Offset", this), row, 0);
00365 pidLayout->addWidget(pwmOffsetEdit, row++, 1);
00366
00367 foreArmMassEdit = new QLineEdit(this);
00368 pidLayout->addWidget(new QLabel("Fore Arm Mass", this), row, 0);
00369 pidLayout->addWidget(foreArmMassEdit, row++, 1);
00370
00371 upperArmMassEdit = new QLineEdit(this);
00372 pidLayout->addWidget(new QLabel("Upper Arm Mass", this), row, 0);
00373 pidLayout->addWidget(upperArmMassEdit, row++, 1);
00374
00375 gravityCompScaleEdit = new QLineEdit(this);
00376 pidLayout->addWidget(new QLabel("Gravity Scale", this), row, 0);
00377 pidLayout->addWidget(gravityCompScaleEdit, row++, 1);
00378
00379 QPushButton* setPIDButton = new QPushButton("Set PID", this);
00380 connect(setPIDButton, SIGNAL(pressed()), this, SLOT(setPIDVals()));
00381 pidLayout->addWidget(setPIDButton, row++, 1);
00382 mainHorizLayout->addLayout(pidLayout);
00383 pidAxisSelected(0);
00384
00385
00386 itsGraphicsScene = new QGraphicsScene(this);
00387 itsGraphicsView = new QGraphicsView(itsGraphicsScene);
00388 itsImageDisplay = new ImageGraphicsItem;
00389 itsGraphicsScene->addItem(itsImageDisplay);
00390 itsGraphicsView->show();
00391 mainVertLayout->addWidget(itsGraphicsView);
00392 itsGraphicsView->setMinimumSize(640, 550);
00393
00394
00395
00396
00397 setCentralWidget(centralWidget);
00398 centralWidget->setLayout(mainVertLayout);
00399
00400 itsScorbotThread->start();
00401 }
00402
00403
00404 void ScorbotConsole::resetEncoders()
00405 {
00406 itsScorbot->resetEncoders();
00407 }
00408
00409
00410 void ScorbotConsole::gotEncoderVal(int joint, int val)
00411 {
00412
00413 ScorbotIce::JointType _joint = (ScorbotIce::JointType)joint;
00414 encoderLbls[_joint]->setNum(val);
00415 addData(jointStringMap[_joint]+"_Encoder", val);
00416 }
00417
00418
00419 void ScorbotConsole::addData(QString plotName, double data)
00420 {
00421 plotData[plotName].push_back(data);
00422 if(plotData[plotName].size() > itsMaxPlotLength)
00423 plotData[plotName].erase(plotData[plotName].begin());
00424 }
00425
00426
00427 void ScorbotConsole::gotPWMVal(int joint, float val)
00428 {
00429 ScorbotIce::JointType _joint = (ScorbotIce::JointType)joint;
00430 pwmLbls[_joint]->setNum(val);
00431 addData(jointStringMap[_joint]+"_PWM", val*1000);
00432 }
00433
00434
00435 void ScorbotConsole::gotTargetPos(int joint, int val)
00436 {
00437 ScorbotIce::JointType _joint = (ScorbotIce::JointType)joint;
00438 addData(jointStringMap[_joint]+"_TargetPos", val);
00439 }
00440
00441
00442 void ScorbotConsole::pidAxisSelected(int index)
00443 {
00444 ScorbotIce::JointType joint = (ScorbotIce::JointType)itsAxisComboBox->itemData(index).toInt();
00445
00446 itsScorbotThread->setSelectedJoint(joint);
00447
00448 float pGain, iGain, dGain, maxI, maxPWM, pwmOffset, gravityCompScale;
00449 int32 foreArmMass, upperArmMass;
00450 itsScorbot->getPIDVals(joint, pGain, iGain, dGain, maxI, maxPWM, pwmOffset);
00451 usleep(1000);
00452 itsScorbot->getGravityParameters(upperArmMass, foreArmMass, gravityCompScale);
00453
00454 pGainEdit->setText(QString::number(pGain));
00455 iGainEdit->setText(QString::number(iGain));
00456 dGainEdit->setText(QString::number(dGain));
00457 maxIEdit->setText(QString::number(maxI));
00458 maxPWMEdit->setText(QString::number(maxPWM));
00459 pwmOffsetEdit->setText(QString::number(pwmOffset));
00460 foreArmMassEdit->setText(QString::number(foreArmMass));
00461 upperArmMassEdit->setText(QString::number(upperArmMass));
00462 gravityCompScaleEdit->setText(QString::number(gravityCompScale));
00463
00464 plotDataColor.clear();
00465 plotDataCheck.clear();
00466 plotData.clear();
00467
00468
00469 plotDataColor[jointStringMap[joint]+"_Encoder"] = PixRGB<byte> (255,0,0);
00470 plotDataColor[jointStringMap[joint]+"_Desired"] = PixRGB<byte> (0,0,255);
00471 plotDataColor[jointStringMap[joint]+"_PWM"] = PixRGB<byte> (0,255,0);
00472 plotDataColor[jointStringMap[joint]+"_TargetPos"] = PixRGB<byte> (0,255,255);
00473 plotDataColor["Gravity_Comp"] = PixRGB<byte> (255,255,255);
00474 plotDataCheck[jointStringMap[joint]+"_Encoder"] = true;
00475 plotDataCheck[jointStringMap[joint]+"_Desired"] = true;
00476 plotDataCheck[jointStringMap[joint]+"_PWM"] = true;
00477 plotDataCheck[jointStringMap[joint]+"_TargetPos"] = true;
00478 plotDataCheck["Gravity_Comp"] = true;
00479 }
00480
00481
00482 void ScorbotConsole::setPIDVals()
00483 {
00484 ScorbotIce::JointType joint = (ScorbotIce::JointType)itsAxisComboBox->itemData(itsAxisComboBox->currentIndex()).toInt();
00485
00486 itsScorbot->setControlParams(joint,
00487 pGainEdit->text().toDouble(),
00488 iGainEdit->text().toDouble(),
00489 dGainEdit->text().toDouble(),
00490 maxIEdit->text().toDouble(),
00491 maxPWMEdit->text().toDouble(),
00492 pwmOffsetEdit->text().toDouble()
00493 );
00494
00495 itsScorbot->setGravityParameters(
00496 upperArmMassEdit->text().toInt(),
00497 foreArmMassEdit->text().toInt(),
00498 gravityCompScaleEdit->text().toDouble());
00499 }
00500
00501
00502 void ScorbotConsole::setPosition(int joint)
00503 {
00504 ScorbotIce::JointType _joint = (ScorbotIce::JointType)joint;
00505 itsScorbot->setJoint(_joint, encoderEdits[_joint]->text().toInt(), durationEdits[_joint]->text().toInt());
00506 plotData[jointStringMap[_joint]+"_Desired"] = (std::vector<float>(itsMaxPlotLength, encoderEdits[_joint]->text().toInt()));
00507 }
00508
00509
00510 void ScorbotConsole::toggleMotorsEnabled()
00511 {
00512 itsMotorsEnabled = !itsMotorsEnabled;
00513 itsScorbot->setEnabled(itsMotorsEnabled);
00514 if(itsMotorsEnabled)
00515 enableMotorsButton->setText("Disable Motors");
00516 else
00517 enableMotorsButton->setText("Enable Motors");
00518 }
00519
00520
00521 void ScorbotConsole::printPosCode()
00522 {
00523 std::cout << "------------------POSITION------------------" << std::endl;
00524 ScorbotIce::encoderValsType pos = itsScorbot->getEncoders();
00525 for(ScorbotIce::encoderValsType::iterator posIt=pos.begin(); posIt!=pos.end(); ++posIt)
00526 std::cout << "encoders[ScorbotIce::" << jointStringMap[posIt->first].toStdString() << "] = " << posIt->second << ";" << std::endl;
00527 std::cout << "--------------------------------------------" << std::endl;
00528 }
00529
00530
00531 void ScorbotConsole::plotLine(std::vector<float> data, PixRGB<byte> color)
00532 {
00533 if(data.size() == 0) return;
00534
00535 QPainterPath path;
00536 path.moveTo(0, data[0]);
00537 for(size_t i=1; i<data.size(); i++)
00538 path.lineTo(i, data[i]);
00539 itsGraphicsScene->addPath(path, QPen(QColor(color.red(), color.green(), color.blue())));
00540 }
00541
00542
00543 void ScorbotConsole::updatePlots()
00544 {
00545 itsGraphicsView->setBackgroundBrush(QBrush(Qt::darkGray));
00546
00547
00548 QList<QGraphicsItem*> items = itsGraphicsScene->items();
00549 for(int i=0; i<items.size(); i++)
00550 {
00551 itsGraphicsScene->removeItem(items[i]);
00552 delete items[i];
00553 }
00554
00555 int textPos = -5000;
00556 int textOffset=500;
00557 QMap<QString, bool>::iterator dataCheckIt = plotDataCheck.begin();
00558 for(; dataCheckIt != plotDataCheck.end(); ++dataCheckIt)
00559 {
00560 if (dataCheckIt.value() && plotData.contains(dataCheckIt.key()))
00561 {
00562 PixRGB<byte> color = plotDataColor[dataCheckIt.key()];
00563 QGraphicsTextItem *text = itsGraphicsScene->addText(dataCheckIt.key());
00564 text->setDefaultTextColor(QColor(color.red(), color.green(), color.blue()));
00565 text->setPos(1, textPos);
00566 text->setFlags(QGraphicsItem::ItemIgnoresTransformations);
00567 textPos+=textOffset;
00568 plotLine(plotData[dataCheckIt.key()], color);
00569 }
00570 }
00571 itsGraphicsScene->addLine(QLineF(0, -5000, 0, 5000), QPen(QColor(128, 0, 0)));
00572 itsGraphicsScene->addLine(QLineF(0, 0, itsMaxPlotLength, 0), QPen(QColor(128, 0, 0)));
00573
00574 itsGraphicsView->fitInView(itsGraphicsScene->itemsBoundingRect());
00575 itsGraphicsView->ensureVisible(itsGraphicsScene->itemsBoundingRect());
00576 itsGraphicsView->centerOn(itsMaxPlotLength/2, 0);
00577 }
00578
00579
00580
00581 void ScorbotConsole::gotGravityCompensation(float compensation)
00582 {
00583 plotData["Gravity_Comp"].push_back(compensation*10000);
00584 if(plotData["Gravity_Comp"].size() > itsMaxPlotLength)
00585 plotData["Gravity_Comp"].erase(plotData["Gravity_Comp"].begin());
00586
00587 gravityCompLbl->setNum(compensation);
00588 }
00589
1.6.3