change controller_torquebrake_get. add sort function.

src/test/testinperplation2d/velacctable.txt

@@ -1,27 +1,25 @@
-0.0	0.0	0.0	0.0
-0.0	-1.0	0.0	30.0
-0.0	-10.0	0.0	30.0
-0.0	1.0	50	0.0
-0.0	2.0	100	0.0
-0.0	0.1	50	0.0
-0.0	10.0	1000	0.0
-5.0	-10.0	0.0	100.0
-5.0	-5.0	0.0	80.0
-5.0	-3.0	0.0	50.0
-5.0	-1.0	0.0	30.0
-5.0	-0.5	0.0	10.0
-5.0	-0.1	0.0	0.0
-5.0	-0.05	30.0	0.0
-5.0	0.0	35.0	0.0
-5.0	1.0	50.0	0.0
-5.0	3.0	300.0	0.0
-5.0	5.0	1500.0	0.0
-10.0	-10.0	0.0	100.0
-10.0	-5.0	0.0	80.0
-10.0	-1.0	0.0	5.0
-10.0	-0.5	0.0	0.0
-10.0	-0.3	30.0	0.0
-10.0	-0.1	31.0	0.0
-10.0	0.0	36.0	0.0
-10.0	1.0	100.0	0.0
-10.0	5.0	1500.0	0.0	
+0.000	-1.5	0.0	30.0
+0.000	 0.364	35.000	 0.000
+0.000	 1.3	600.000	 0.000
+2.023	 0.798	35.000	 0.0000
+3.668	 0.310	35.000	 0.000
+3.668	 0.310	35.000	 0.000
+5.045	 0.456	150.000	 0.000
+5.130	 0.954	300.000	 0.000
+6.120	0.000	35.000	 0.000
+8.834	 0.045	150.000	 0.000
+10.656	 0.220	200.000	 0.000
+10.000	-1.5	0.0	30.0
+11.614	 0.483	300.000	 0.000
+12.154	 0.000	150.000	 0.000
+14.530	 0.128	200.000	 0.000
+30.0	0.0000	200.0	0.000
+30.000	 1.3	600.000	 0.000
+30.000	-1.5	0.0	30.0
+30.060	 0.352	300.000	 0.000
+41.249	 0.220	300.000	 0.000
+60.0	0.000	300.0	0.000
+60.000	-1.5	0.0	30.0
+60.000	 1.0	600.000	 0.000
+120.000	 0.0	500.000	 0.000
+120.000	 1.0	1000.000	 0.000

src/tool/controller_torquebrake_get/mainwindow.cpp

@@ -1,6 +1,12 @@
 #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)
@@ -12,6 +18,7 @@ MainWindow::MainWindow(QWidget *parent)
     ui->lineEdit_speedlimit->setText(QString::number(mfSpeedLimit));
     ui->lineEdit_torqueatlimit->setText(QString::number(mfTorqueAtLimit));
     ui->lineEdit_TorqueStart->setText(QString::number(mfTorqueStart));
+    ui->lineEdit_VelStep->setText(QString::number(mfVelStep));
 
     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);
@@ -77,6 +84,8 @@ void MainWindow::on_checkBox_clicked()
     if(ui->checkBox->isChecked())
     {
         mbEnable = true;
+        mfAvgSpeed = 0.0;
+        mfVectorAcc.clear();
     }
     else
     {
@@ -87,6 +96,8 @@ void MainWindow::on_checkBox_clicked()
     mfSpeedLimit = ui->lineEdit_speedlimit->text().toDouble();
     mfTorqueAtLimit = ui->lineEdit_torqueatlimit->text().toDouble();
     mfTorqueStart = ui->lineEdit_TorqueStart->text().toDouble();
+    mfVelStep = ui->lineEdit_VelStep->text().toDouble();
+    if(mfVelStep == 0.0)mfVelStep = 1.0;
 
 }
 
@@ -112,18 +123,141 @@ void MainWindow::UpdateGPSIMU(const char *strdata, const unsigned int nSize, con
 void MainWindow::UpdatePlainText(iv::gps::gpsimu &xgpsimu)
 {
     char strout[1000];
-    snprintf(strout,1000,"%6.3f\t%6.3f\t%6.3f\t%6.3f\t%6.3f\t%6.3f\t%6.3f\t",
-            xgpsimu.speed(),xgpsimu.acce_x(),mfTorque,mfBrake,
-            xgpsimu.acc_calc(),xgpsimu.acce_y(),xgpsimu.acce_z());
+
+    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,xgpsimu.acce_x() * 9.8,mfTorque,mfBrake);
     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();
+            snprintf(strout,1000,"%6.3f\t%6.3f\t%6.3f\t%6.3f",
+                     mfAvgSpeed,fAvgAcc,mfTorque,mfBrake);
+            ui->plainTextEdit_AvgAcc->appendPlainText(strout);
+        }
+        mfAvgSpeed = froundspeed;
+        mfVectorAcc.clear();
+    }
+    else
+    {
+        mfVectorAcc.push_back(xgpsimu.acce_x()*9.8);
+    }
 }
 
 void MainWindow::on_pushButton_ClearRecord_clicked()
 {
     ui->plainTextEdit->clear();
+    ui->plainTextEdit_AvgAcc->clear();
 }
 
-void MainWindow::on_pushButton_SaveRecord_clicked()
+
+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);
+            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();
 }

src/tool/controller_torquebrake_get/mainwindow.h

@@ -15,6 +15,17 @@ QT_BEGIN_NAMESPACE
 namespace Ui { class MainWindow; }
 QT_END_NAMESPACE
 
+namespace iv {
+struct tableunit
+{
+public:
+    double mfVel;
+    double mfAcc;
+    double mfTorque;
+    double mfBrake;
+};
+}
+
 class MainWindow : public QMainWindow
 {
     Q_OBJECT
@@ -30,7 +41,7 @@ private slots:
 
     void on_pushButton_ClearRecord_clicked();
 
-    void on_pushButton_SaveRecord_clicked();
+    void on_pushButton_SortTable_clicked();
 
 private:
     Ui::MainWindow *ui;
@@ -44,6 +55,8 @@ private:
     double mfTorqueAtLimit = 30;
     double mfTorqueStart = 100.0;
 
+    double mfVelStep = 5.0;
+
     double mfSpeedNow = 100.0;
 
     void * mpa;
@@ -56,5 +69,9 @@ private:
 
     void UpdatePlainText(iv::gps::gpsimu & xgpsimu);
 
+
+    double mfAvgSpeed = 0.0;
+    std::vector<double > mfVectorAcc;
+
 };
 #endif // MAINWINDOW_H

src/tool/controller_torquebrake_get/mainwindow.ui

@@ -19,16 +19,16 @@
      <rect>
       <x>50</x>
       <y>270</y>
-      <width>531</width>
-      <height>311</height>
+      <width>521</width>
+      <height>181</height>
      </rect>
     </property>
    </widget>
    <widget class="QCheckBox" name="checkBox">
     <property name="geometry">
      <rect>
-      <x>540</x>
-      <y>120</y>
+      <x>520</x>
+      <y>170</y>
       <width>211</width>
       <height>61</height>
      </rect>
@@ -64,7 +64,7 @@
     <property name="geometry">
      <rect>
       <x>620</x>
-      <y>296</y>
+      <y>330</y>
       <width>161</width>
       <height>51</height>
      </rect>
@@ -73,19 +73,6 @@
      <string>Clear Record</string>
     </property>
    </widget>
-   <widget class="QPushButton" name="pushButton_SaveRecord">
-    <property name="geometry">
-     <rect>
-      <x>620</x>
-      <y>506</y>
-      <width>161</width>
-      <height>51</height>
-     </rect>
-    </property>
-    <property name="text">
-     <string>Save Record</string>
-    </property>
-   </widget>
    <widget class="QLabel" name="label_2">
     <property name="geometry">
      <rect>
@@ -178,6 +165,52 @@
      </rect>
     </property>
    </widget>
+   <widget class="QPlainTextEdit" name="plainTextEdit_AvgAcc">
+    <property name="geometry">
+     <rect>
+      <x>50</x>
+      <y>470</y>
+      <width>521</width>
+      <height>131</height>
+     </rect>
+    </property>
+   </widget>
+   <widget class="QPushButton" name="pushButton_SortTable">
+    <property name="geometry">
+     <rect>
+      <x>620</x>
+      <y>510</y>
+      <width>161</width>
+      <height>41</height>
+     </rect>
+    </property>
+    <property name="text">
+     <string>Sort Table</string>
+    </property>
+   </widget>
+   <widget class="QLabel" name="label_6">
+    <property name="geometry">
+     <rect>
+      <x>450</x>
+      <y>110</y>
+      <width>91</width>
+      <height>21</height>
+     </rect>
+    </property>
+    <property name="text">
+     <string>速度步进</string>
+    </property>
+   </widget>
+   <widget class="QLineEdit" name="lineEdit_VelStep">
+    <property name="geometry">
+     <rect>
+      <x>550</x>
+      <y>104</y>
+      <width>151</width>
+      <height>31</height>
+     </rect>
+    </property>
+   </widget>
   </widget>
   <widget class="QMenuBar" name="menubar">
    <property name="geometry">