void mtsPIDQtWidget::SlotMaintainPosition(void)
{
// reset desired position
PID.StateJoint.Position().ElementwiseMultiply(UnitFactor);
QVWDesiredPositionWidget->SetValue(PID.StateJoint.Position());
PID.ResetController();
SlotPositionChanged();
}
void mtsPIDQtWidget::SlotZeroPosition(void)
{
// reset desired position
DesiredPosition.SetAll(0.0);
QVWDesiredPositionWidget->SetValue(DesiredPosition);
PID.ResetController();
SlotPositionChanged();
}
void mtsPIDQtWidget::SlotMaintainPosition(void)
{
// reset desired position
prmPositionJointGet prmFeedbackPos;
prmFeedbackPos.SetSize(NumberOfAxis);
PID.GetPositionJoint(prmFeedbackPos);
prmFeedbackPos.Position().ElementwiseMultiply(UnitFactor);
QVWDesiredPositionWidget->SetValue(prmFeedbackPos.Position());
PID.ResetController();
SlotPositionChanged();
}
////------------ Private Methods ----------------
void mtsPIDQtWidget::setupUi(void)
{
QFont font;
font.setBold(true);
font.setPointSize(12);
const double maximum = 30000;
QGridLayout * gridLayout = new QGridLayout();
gridLayout->setSpacing(1);
int row = 0;
QLabel * currentPosLabel = new QLabel("Current position (deg)");
currentPosLabel->setAlignment(Qt::AlignRight);
gridLayout->addWidget(currentPosLabel, row, 0);
QVRCurrentPositionWidget = new vctQtWidgetDynamicVectorDoubleRead();
QVRCurrentPositionWidget->SetPrecision(3);
gridLayout->addWidget(QVRCurrentPositionWidget, row, 1);
row++;
QLabel * desiredPosLabel = new QLabel("Desired position (deg)");
desiredPosLabel->setAlignment(Qt::AlignRight);
gridLayout->addWidget(desiredPosLabel, row, 0);
QVWDesiredPositionWidget = new vctQtWidgetDynamicVectorDoubleWrite(vctQtWidgetDynamicVectorDoubleWrite::SPINBOX_WIDGET);
QVWDesiredPositionWidget->SetStep(0.1);
QVWDesiredPositionWidget->SetRange(-360.0, 360.0);
gridLayout->addWidget(QVWDesiredPositionWidget, row, 1);
row++;
QLabel * currentEffortLabel = new QLabel("Current effort (Nm)");
currentEffortLabel->setAlignment(Qt::AlignRight);
gridLayout->addWidget(currentEffortLabel, row, 0);
QVRCurrentEffortWidget = new vctQtWidgetDynamicVectorDoubleRead();
QVRCurrentEffortWidget->SetPrecision(3);
gridLayout->addWidget(QVRCurrentEffortWidget, row, 1);
row++;
QLabel * desiredEffortLabel = new QLabel("Desired effort (Nm)");
desiredEffortLabel->setAlignment(Qt::AlignRight);
gridLayout->addWidget(desiredEffortLabel, row, 0);
QVWDesiredEffortWidget = new vctQtWidgetDynamicVectorDoubleRead();
QVWDesiredEffortWidget->SetPrecision(3);
gridLayout->addWidget(QVWDesiredEffortWidget, row, 1);
row++;
QLabel * pLabel = new QLabel("PGain");
pLabel->setAlignment(Qt::AlignRight);
gridLayout->addWidget(pLabel);
QVWPGainWidget = new vctQtWidgetDynamicVectorDoubleWrite(vctQtWidgetDynamicVectorDoubleWrite::SPINBOX_WIDGET);
QVWPGainWidget->SetStep(0.01);
QVWPGainWidget->SetPrecision(3);
QVWPGainWidget->SetRange(-maximum, maximum);
gridLayout->addWidget(QVWPGainWidget, row, 1);
row++;
QLabel * dLabel = new QLabel("DGain");
dLabel->setAlignment(Qt::AlignRight);
gridLayout->addWidget(dLabel);
QVWDGainWidget = new vctQtWidgetDynamicVectorDoubleWrite(vctQtWidgetDynamicVectorDoubleWrite::SPINBOX_WIDGET);
QVWDGainWidget->SetStep(0.01);
QVWDGainWidget->SetPrecision(3);
QVWDGainWidget->SetRange(-maximum, maximum);
gridLayout->addWidget(QVWDGainWidget, row, 1);
row++;
QLabel * iLabel = new QLabel("IGain");
iLabel->setAlignment(Qt::AlignRight);
gridLayout->addWidget(iLabel);
QVWIGainWidget = new vctQtWidgetDynamicVectorDoubleWrite(vctQtWidgetDynamicVectorDoubleWrite::SPINBOX_WIDGET);
QVWIGainWidget->SetStep(0.001);
QVWIGainWidget->SetPrecision(5);
QVWIGainWidget->SetRange(-maximum, maximum);
gridLayout->addWidget(QVWIGainWidget, row, 1);
row++;
// plot
QHBoxLayout * plotLayout = new QHBoxLayout;
// plot control
QVBoxLayout * plotButtonsLayout = new QVBoxLayout;
// - pick axis to display
QLabel * plotIndexLabel = new QLabel("Index");
plotButtonsLayout->addWidget(plotIndexLabel);
QSBPlotIndex = new QSpinBox();
QSBPlotIndex->setRange(0, NumberOfAxis);
plotButtonsLayout->addWidget(QSBPlotIndex);
// legend
QLabel * label;
QPalette palette;
palette.setColor(QPalette::Window, Qt::black);
label = new QLabel("Current position");
label->setAutoFillBackground(true);
palette.setColor(QPalette::WindowText, Qt::red);
label->setPalette(palette);
plotButtonsLayout->addWidget(label);
label = new QLabel("Desired position");
label->setAutoFillBackground(true);
palette.setColor(QPalette::WindowText, Qt::green);
label->setPalette(palette);
plotButtonsLayout->addWidget(label);
label = new QLabel("Current velocity");
label->setAutoFillBackground(true);
palette.setColor(QPalette::WindowText, Qt::gray);
label->setPalette(palette);
plotButtonsLayout->addWidget(label);
label = new QLabel("Current effort");
label->setAutoFillBackground(true);
palette.setColor(QPalette::WindowText, Qt::cyan);
label->setPalette(palette);
plotButtonsLayout->addWidget(label);
label = new QLabel("Desired effort");
label->setAutoFillBackground(true);
palette.setColor(QPalette::WindowText, Qt::white);
label->setPalette(palette);
plotButtonsLayout->addWidget(label);
plotButtonsLayout->addStretch();
plotLayout->addLayout(plotButtonsLayout);
// plotting area
QVPlot = new vctPlot2DOpenGLQtWidget();
vctPlot2DBase::Scale * scalePosition = QVPlot->AddScale("positions");
CurrentPositionSignal = scalePosition->AddSignal("current");
CurrentPositionSignal->SetColor(vctDouble3(1.0, 0.0, 0.0));
DesiredPositionSignal = scalePosition->AddSignal("desired");
DesiredPositionSignal->SetColor(vctDouble3(0.0, 1.0, 0.0));
vctPlot2DBase::Scale * scaleVelocity = QVPlot->AddScale("velocities");
CurrentVelocitySignal = scaleVelocity->AddSignal("current");
CurrentVelocitySignal->SetColor(vctDouble3(0.5, 0.5, 0.5));
vctPlot2DBase::Scale * scaleEffort = QVPlot->AddScale("efforts");
CurrentEffortSignal = scaleEffort->AddSignal("-current");
CurrentEffortSignal->SetColor(vctDouble3(0.0, 1.0, 1.0));
DesiredEffortSignal = scaleEffort->AddSignal("-desired");
DesiredEffortSignal->SetColor(vctDouble3(1.0, 1.0, 1.0));
QVPlot->setSizePolicy(QSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding));
plotLayout->addWidget(QVPlot);
// control
QCBEnableDirectControl = new QCheckBox("Direct control");
QCBEnablePID = new QCheckBox("Enable PID");
QCBEnableTorqueMode = new QCheckBox("Enable torque mode");
QPBMaintainPosition = new QPushButton("Maintain position");
QPBZeroPosition = new QPushButton("Zero position");
QPBResetPIDGain = new QPushButton("Reset PID gains");
QHBoxLayout * controlLayout = new QHBoxLayout;
controlLayout->addWidget(QCBEnableDirectControl);
controlLayout->addWidget(QCBEnablePID);
controlLayout->addWidget(QCBEnableTorqueMode);
controlLayout->addWidget(QPBMaintainPosition);
controlLayout->addWidget(QPBZeroPosition);
controlLayout->addWidget(QPBResetPIDGain);
controlLayout->addStretch();
QGroupBox * controlGroupBox = new QGroupBox("Control");
controlGroupBox->setLayout(controlLayout);
connect(QCBEnableDirectControl, SIGNAL(toggled(bool)), this, SLOT(SlotEnableDirectControl(bool)));
connect(QCBEnablePID, SIGNAL(clicked(bool)), this, SLOT(SlotEnablePID(bool)));
connect(this, SIGNAL(SignalEnablePID(bool)), this, SLOT(SlotEnableEventHandler(bool)));
connect(QCBEnableTorqueMode, SIGNAL(toggled(bool)), this, SLOT(SlotEnableTorqueMode(bool)));
connect(QPBMaintainPosition, SIGNAL(clicked()), this, SLOT(SlotMaintainPosition()));
connect(QPBZeroPosition, SIGNAL(clicked()), this, SLOT(SlotZeroPosition()));
connect(QPBResetPIDGain, SIGNAL(clicked()), this, SLOT(SlotResetPIDGain()));
connect(QSBPlotIndex, SIGNAL(valueChanged(int)), this, SLOT(SlotPlotIndex(int)));
// main layout
QVBoxLayout * mainLayout = new QVBoxLayout;
mainLayout->addLayout(gridLayout);
mainLayout->addLayout(plotLayout);
mainLayout->addWidget(controlGroupBox);
setLayout(mainLayout);
setWindowTitle(this->GetName().c_str());
setMinimumWidth(750);
resize(sizeHint());
// connect signals & slots
connect(QVWDesiredPositionWidget, SIGNAL(valueChanged()), this, SLOT(SlotPositionChanged()));
connect(QVWPGainWidget, SIGNAL(valueChanged()), this, SLOT(SlotPGainChanged()));
connect(QVWDGainWidget, SIGNAL(valueChanged()), this, SLOT(SlotDGainChanged()));
connect(QVWIGainWidget, SIGNAL(valueChanged()), this, SLOT(SlotIGainChanged()));
// set initial values
QCBEnableDirectControl->setChecked(DirectControl);
SlotEnableDirectControl(DirectControl);
}
