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); }