|
@@ -0,0 +1,357 @@
|
|
|
|
+#include "mainwindow.h"
|
|
|
|
+#include "ui_mainwindow.h"
|
|
|
|
+
|
|
|
|
+#include <QFileDialog>
|
|
|
|
+#include <QFile>
|
|
|
|
+#include <QMessageBox>
|
|
|
|
+
|
|
|
|
+#include <math.h>
|
|
|
|
+
|
|
|
|
+MainWindow::MainWindow(QWidget *parent)
|
|
|
|
+ : QMainWindow(parent)
|
|
|
|
+ , ui(new Ui::MainWindow)
|
|
|
|
+{
|
|
|
|
+ ui->setupUi(this);
|
|
|
|
+
|
|
|
|
+ mfBrake = 1.0;
|
|
|
|
+ mfTorque = 1.0;
|
|
|
|
+ ui->lineEdit_Brake->setText("1.0");
|
|
|
|
+ ui->lineEdit_Torque->setText("1.0");
|
|
|
|
+ ui->lineEdit_speedlimit->setText(QString::number(mfSpeedLimit));
|
|
|
|
+ ui->lineEdit_TorqueStart->setText(QString::number(mfTorqueStart));
|
|
|
|
+ ui->lineEdit_VelStep->setText(QString::number(mfVelStep));
|
|
|
|
+
|
|
|
|
+ mbEnable = false;
|
|
|
|
+ mnTestStep = 0;
|
|
|
|
+ ui->progressBar->setRange(0,100);
|
|
|
|
+
|
|
|
|
+ ui->comboBox_accaxis->addItem("x轴");
|
|
|
|
+ ui->comboBox_accaxis->addItem("y轴");
|
|
|
|
+ ui->comboBox_accaxis->setCurrentIndex(1);
|
|
|
|
+
|
|
|
|
+ ui->progressBar->setValue(0);
|
|
|
|
+
|
|
|
|
+ ui->checkBox_TestEnable->setChecked(false);
|
|
|
|
+ ui->checkBox_notrecordbrake->setChecked(false);
|
|
|
|
+ ui->pushButton_StartTest->setEnabled(false);
|
|
|
|
+ ui->pushButton_StartTest_2->setEnabled(false);
|
|
|
|
+
|
|
|
|
+ mpa = iv::modulecomm::RegisterSend("torquebrake",1000,1);
|
|
|
|
+ ModuleFun fungpsimu =std::bind(&MainWindow::UpdateGPSIMU,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
|
|
|
|
+ mpagpsimu = iv::modulecomm::RegisterRecvPlus("hcp2_gpsimu",fungpsimu);
|
|
|
|
+
|
|
|
|
+
|
|
|
|
+
|
|
|
|
+ mpTimer = new QTimer();
|
|
|
|
+ connect(mpTimer,SIGNAL(timeout()),this,SLOT(onTimer()));
|
|
|
|
+ mpTimer->start(10);
|
|
|
|
+
|
|
|
|
+
|
|
|
|
+
|
|
|
|
+ setWindowTitle("Get Torque Brake acc Relation");
|
|
|
|
+
|
|
|
|
+}
|
|
|
|
+
|
|
|
|
+MainWindow::~MainWindow()
|
|
|
|
+{
|
|
|
|
+ delete ui;
|
|
|
|
+}
|
|
|
|
+
|
|
|
|
+void MainWindow::onTimer()
|
|
|
|
+{
|
|
|
|
+ iv::controller::torquebrake xtb;
|
|
|
|
+
|
|
|
|
+ double fstep = 0;
|
|
|
|
+ switch (mnTestStep) {
|
|
|
|
+ case 0:
|
|
|
|
+ xtb.set_torque(0);
|
|
|
|
+ xtb.set_brake(mfBrake);
|
|
|
|
+ break;
|
|
|
|
+ case 1:
|
|
|
|
+ if(mfSpeedNow < mfSpeedLimit)
|
|
|
|
+ {
|
|
|
|
+ xtb.set_torque(mfTorque);
|
|
|
|
+ xtb.set_brake(0);
|
|
|
|
+ }
|
|
|
|
+ else
|
|
|
|
+ {
|
|
|
|
+ mnTestStep = 2;
|
|
|
|
+ xtb.set_torque(0);
|
|
|
|
+ xtb.set_brake(mfBrake);
|
|
|
|
+ }
|
|
|
|
+ if(mfSpeedLimit >0)
|
|
|
|
+ {
|
|
|
|
+ fstep = (mfSpeedNow/mfSpeedLimit)/2.0;
|
|
|
|
+ fstep = fstep * 100.0;
|
|
|
|
+ ui->progressBar->setValue(fstep);
|
|
|
|
+ }
|
|
|
|
+ break;
|
|
|
|
+ case 2:
|
|
|
|
+ xtb.set_torque(0);
|
|
|
|
+ xtb.set_brake(mfBrake);
|
|
|
|
+ if(mfSpeedLimit >0)
|
|
|
|
+ {
|
|
|
|
+ fstep = ((mfSpeedLimit-mfSpeedNow)/mfSpeedLimit)/2.0 + 0.5;
|
|
|
|
+ fstep = fstep * 100.0;
|
|
|
|
+ ui->progressBar->setValue(fstep);
|
|
|
|
+ }
|
|
|
|
+ if(mfSpeedNow<0.1)
|
|
|
|
+ {
|
|
|
|
+ mnTestStep = 0;
|
|
|
|
+ fstep = 1.0;
|
|
|
|
+ ui->pushButton_StartTest->setEnabled(true);
|
|
|
|
+ fstep = fstep * 100.0;
|
|
|
|
+ ui->progressBar->setValue(fstep);
|
|
|
|
+ }
|
|
|
|
+ break;
|
|
|
|
+ default:
|
|
|
|
+ break;
|
|
|
|
+ }
|
|
|
|
+ xtb.set_enable(mbEnable);
|
|
|
|
+
|
|
|
|
+ mfSendTorque = xtb.torque();
|
|
|
|
+ mfSendBrake = xtb.brake();
|
|
|
|
+
|
|
|
|
+ int ndatasize = xtb.ByteSize();
|
|
|
|
+ std::shared_ptr<char> strbuf_ptr = std::shared_ptr<char>(new char[ndatasize]);
|
|
|
|
+ if(xtb.SerializeToArray(strbuf_ptr.get(),ndatasize))
|
|
|
|
+ {
|
|
|
|
+ iv::modulecomm::ModuleSendMsg(mpa,strbuf_ptr.get(),ndatasize);
|
|
|
|
+ }
|
|
|
|
+
|
|
|
|
+ if(mbEnable)
|
|
|
|
+ {
|
|
|
|
+ if(mbGPSIMUUpdate)
|
|
|
|
+ {
|
|
|
|
+ iv::gps::gpsimu xgpsimu;
|
|
|
|
+ mMutexGPSIMU.lock();
|
|
|
|
+ xgpsimu.CopyFrom(mgpsimu);
|
|
|
|
+ mbGPSIMUUpdate = false;
|
|
|
|
+ mMutexGPSIMU.unlock();
|
|
|
|
+
|
|
|
|
+ UpdatePlainText(xgpsimu);
|
|
|
|
+
|
|
|
|
+ }
|
|
|
|
+ }
|
|
|
|
+
|
|
|
|
+}
|
|
|
|
+
|
|
|
|
+
|
|
|
|
+void MainWindow::UpdateGPSIMU(const char *strdata, const unsigned int nSize, const unsigned int index, const QDateTime *dt, const char *strmemname)
|
|
|
|
+{
|
|
|
|
+ (void)strdata;
|
|
|
|
+ (void)nSize;
|
|
|
|
+ (void)index;
|
|
|
|
+ iv::gps::gpsimu xgpsimu;
|
|
|
|
+ if(!xgpsimu.ParseFromArray(strdata,nSize))
|
|
|
|
+ {
|
|
|
|
+ qDebug("UpdateGPSIMU Parse Error.");
|
|
|
|
+ return;
|
|
|
|
+ }
|
|
|
|
+
|
|
|
|
+ mfSpeedNow = 3.6 * sqrt(pow(xgpsimu.ve(),2) + pow(xgpsimu.vn(),2));
|
|
|
|
+ mMutexGPSIMU.lock();
|
|
|
|
+ mgpsimu.CopyFrom(xgpsimu);
|
|
|
|
+ mbGPSIMUUpdate = true;
|
|
|
|
+ mMutexGPSIMU.unlock();
|
|
|
|
+}
|
|
|
|
+
|
|
|
|
+void MainWindow::UpdatePlainText(iv::gps::gpsimu &xgpsimu)
|
|
|
|
+{
|
|
|
|
+ char strout[1000];
|
|
|
|
+
|
|
|
|
+ double facce = 0.0;
|
|
|
|
+
|
|
|
|
+ if(mnAxis == 0)
|
|
|
|
+ {
|
|
|
|
+ facce = xgpsimu.acce_x();
|
|
|
|
+ }
|
|
|
|
+ else
|
|
|
|
+ {
|
|
|
|
+ facce = xgpsimu.acce_y();
|
|
|
|
+ }
|
|
|
|
+
|
|
|
|
+ double fspeed = 3.6 * sqrt(pow(xgpsimu.ve(),2) + pow(xgpsimu.vn(),2));
|
|
|
|
+ snprintf(strout,1000,"%6.3f\t%6.3f\t%6.3f\t%6.3f",
|
|
|
|
+ fspeed,facce * 9.8,mfSendTorque,mfSendBrake);
|
|
|
|
+ ui->plainTextEdit->appendPlainText(strout);
|
|
|
|
+
|
|
|
|
+ double froundspeed = mfVelStep * std::round(fspeed/mfVelStep);
|
|
|
|
+ if(froundspeed != mfAvgSpeed)
|
|
|
|
+ {
|
|
|
|
+ if(mfVectorAcc.size()> 0)
|
|
|
|
+ {
|
|
|
|
+ double fAvgAcc = 0.0;
|
|
|
|
+ unsigned int i;
|
|
|
|
+ for(i=0;i<mfVectorAcc.size();i++)
|
|
|
|
+ {
|
|
|
|
+ fAvgAcc = fAvgAcc + mfVectorAcc[i];
|
|
|
|
+
|
|
|
|
+ }
|
|
|
|
+ fAvgAcc = fAvgAcc/mfVectorAcc.size();
|
|
|
|
+ if((mfAvgSpeed > 1.0)&&(mbEnable))
|
|
|
|
+ {
|
|
|
|
+ if(ui->checkBox_notrecordbrake->isChecked() &&(mfSendBrake>0.00001))
|
|
|
|
+ {
|
|
|
|
+
|
|
|
|
+ }
|
|
|
|
+ else
|
|
|
|
+ {
|
|
|
|
+ snprintf(strout,1000,"%6.3f\t%6.3f\t%6.3f\t%6.3f",
|
|
|
|
+ mfAvgSpeed,fAvgAcc,mfSendTorque,mfSendBrake);
|
|
|
|
+ ui->plainTextEdit_AvgAcc->appendPlainText(strout);
|
|
|
|
+ }
|
|
|
|
+ }
|
|
|
|
+ }
|
|
|
|
+ mfAvgSpeed = froundspeed;
|
|
|
|
+ mfVectorAcc.clear();
|
|
|
|
+ }
|
|
|
|
+ else
|
|
|
|
+ {
|
|
|
|
+ mfVectorAcc.push_back(facce*9.8);
|
|
|
|
+ }
|
|
|
|
+}
|
|
|
|
+
|
|
|
|
+void MainWindow::on_pushButton_ClearRecord_clicked()
|
|
|
|
+{
|
|
|
|
+ ui->plainTextEdit->clear();
|
|
|
|
+ ui->plainTextEdit_AvgAcc->clear();
|
|
|
|
+}
|
|
|
|
+
|
|
|
|
+
|
|
|
|
+void MainWindow::on_pushButton_SortTable_clicked()
|
|
|
|
+{
|
|
|
|
+ QString str = QFileDialog::getOpenFileName(this,"Load Table",".","*.txt");
|
|
|
|
+ if(str.isEmpty())return;
|
|
|
|
+
|
|
|
|
+ std::vector<iv::tableunit> xvectorunit;
|
|
|
|
+ std::vector<iv::tableunit> xvectorunitsave;
|
|
|
|
+
|
|
|
|
+ QFile xFile;
|
|
|
|
+ xFile.setFileName(str);
|
|
|
|
+ if(xFile.open(QIODevice::ReadOnly))
|
|
|
|
+ {
|
|
|
|
+ QByteArray ba = xFile.readAll();
|
|
|
|
+ QString strba;
|
|
|
|
+ strba.append(ba);
|
|
|
|
+ QStringList strlinelist =strba.split("\n");// strba.split(QRegExp("[\t ;]+"));
|
|
|
|
+ int nline = strlinelist.size();
|
|
|
|
+ int i;
|
|
|
|
+ for(i=0;i<nline;i++)
|
|
|
|
+ {
|
|
|
|
+ QString str = strlinelist.at(i);
|
|
|
|
+ str = str.trimmed();
|
|
|
|
+ QStringList strvaluelist = str.split(QRegExp("[\t ;]+"));
|
|
|
|
+ if(strvaluelist.size()>=4)
|
|
|
|
+ {
|
|
|
|
+ double vel,acc,torque,brake;
|
|
|
|
+ vel = QString(strvaluelist.at(0)).toDouble();
|
|
|
|
+ acc = QString(strvaluelist.at(1)).toDouble();
|
|
|
|
+ torque = QString(strvaluelist.at(2)).toDouble();
|
|
|
|
+ brake = QString(strvaluelist.at(3)).toDouble();
|
|
|
|
+ iv::tableunit xunit;
|
|
|
|
+ xunit.mfVel = vel;
|
|
|
|
+ xunit.mfAcc = acc;
|
|
|
|
+ xunit.mfTorque = torque;
|
|
|
|
+ xunit.mfBrake = brake;
|
|
|
|
+ xvectorunit.push_back(xunit);
|
|
|
|
+ }
|
|
|
|
+ }
|
|
|
|
+
|
|
|
|
+ }
|
|
|
|
+ xFile.close();
|
|
|
|
+
|
|
|
|
+ if(xvectorunit.size()<1)
|
|
|
|
+ {
|
|
|
|
+ QMessageBox::warning(this,"Warning","No Table Found.",QMessageBox::YesAll);
|
|
|
|
+ return;
|
|
|
|
+ }
|
|
|
|
+
|
|
|
|
+ while(xvectorunit.size()>0)
|
|
|
|
+ {
|
|
|
|
+ iv::tableunit xunit = xvectorunit[0];
|
|
|
|
+ xvectorunit.erase(xvectorunit.begin());
|
|
|
|
+ unsigned int i = 0;
|
|
|
|
+ for(i=0;i<xvectorunitsave.size();i++)
|
|
|
|
+ {
|
|
|
|
+ if(xunit.mfVel==xvectorunitsave[i].mfVel)
|
|
|
|
+ {
|
|
|
|
+ if(xunit.mfAcc < xvectorunitsave[i].mfAcc)
|
|
|
|
+ {
|
|
|
|
+ xvectorunitsave.insert(xvectorunitsave.begin()+i,xunit);
|
|
|
|
+ break;
|
|
|
|
+ }
|
|
|
|
+ }
|
|
|
|
+ else
|
|
|
|
+ {
|
|
|
|
+ if(xunit.mfVel < xvectorunitsave[i].mfVel)
|
|
|
|
+ {
|
|
|
|
+ xvectorunitsave.insert(xvectorunitsave.begin()+i,xunit);
|
|
|
|
+ break;
|
|
|
|
+ }
|
|
|
|
+ }
|
|
|
|
+ }
|
|
|
|
+ if(i == xvectorunitsave.size())xvectorunitsave.push_back(xunit);
|
|
|
|
+ }
|
|
|
|
+
|
|
|
|
+
|
|
|
|
+ QString strsavename = QFileDialog::getSaveFileName(this,"Save Table",".","*.txt");
|
|
|
|
+ if(strsavename.isEmpty())return;
|
|
|
|
+
|
|
|
|
+ QFile xFileSave;
|
|
|
|
+ xFileSave.setFileName(strsavename);
|
|
|
|
+ if(xFileSave.open(QIODevice::ReadWrite))
|
|
|
|
+ {
|
|
|
|
+ unsigned int i;
|
|
|
|
+ for(i=0;i<xvectorunitsave.size();i++)
|
|
|
|
+ {
|
|
|
|
+ char strout[1000];
|
|
|
|
+ snprintf(strout,1000,"%6.3f\t%6.3f\t%6.3f\t%6.3f\n",
|
|
|
|
+ xvectorunitsave[i].mfVel,xvectorunitsave[i].mfAcc,
|
|
|
|
+ xvectorunitsave[i].mfTorque,xvectorunitsave[i].mfBrake);
|
|
|
|
+ xFileSave.write(strout,strnlen(strout,1000));
|
|
|
|
+ }
|
|
|
|
+
|
|
|
|
+ }
|
|
|
|
+ else
|
|
|
|
+ {
|
|
|
|
+ QMessageBox::warning(this,"Warning","Save File Error.",QMessageBox::YesAll);
|
|
|
|
+ }
|
|
|
|
+ xFileSave.close();
|
|
|
|
+}
|
|
|
|
+
|
|
|
|
+void MainWindow::on_pushButton_StartTest_clicked()
|
|
|
|
+{
|
|
|
|
+ mfTorque = ui->lineEdit_Torque->text().toDouble();
|
|
|
|
+ mfBrake = ui->lineEdit_Brake->text().toDouble();
|
|
|
|
+ mfSpeedLimit = ui->lineEdit_speedlimit->text().toDouble();
|
|
|
|
+
|
|
|
|
+ mnTestStep = 1;
|
|
|
|
+
|
|
|
|
+ mnAxis = ui->comboBox_accaxis->currentIndex();
|
|
|
|
+
|
|
|
|
+ ui->progressBar->setValue(0);
|
|
|
|
+ ui->pushButton_StartTest->setEnabled(false);
|
|
|
|
+}
|
|
|
|
+
|
|
|
|
+void MainWindow::on_pushButton_StartTest_2_clicked()
|
|
|
|
+{
|
|
|
|
+ mnTestStep = 0;
|
|
|
|
+ ui->pushButton_StartTest->setEnabled(true);
|
|
|
|
+}
|
|
|
|
+
|
|
|
|
+void MainWindow::on_checkBox_TestEnable_clicked()
|
|
|
|
+{
|
|
|
|
+ if(ui->checkBox_TestEnable->isChecked())
|
|
|
|
+ {
|
|
|
|
+ ui->pushButton_StartTest->setEnabled(true);
|
|
|
|
+ ui->pushButton_StartTest_2->setEnabled(true);
|
|
|
|
+ mbEnable = true;
|
|
|
|
+ }
|
|
|
|
+ else
|
|
|
|
+ {
|
|
|
|
+ ui->pushButton_StartTest->setEnabled(false);
|
|
|
|
+ ui->pushButton_StartTest_2->setEnabled(false);
|
|
|
|
+ mbEnable = false;
|
|
|
|
+ }
|
|
|
|
+}
|