change ontroller_chery_sterra_es_fcm for use pid.

src/controller/controller_chery_sterra_es_fcm/main.cpp

@@ -92,6 +92,7 @@ static double MAXACC = 3.0;
 static int64_t gnLastAccUptime = 0;
 static bool gbUsePID = true;
 static bool gbPrintPIDOut = true;
+static double TorqueRatio = 1.4;
 
 static double gfVehAcc = 0.0;
 
@@ -116,7 +117,7 @@ double  PIDTorque(double torque)
 {
     if(gbUsePID == false)return torque;
 
-    int nnow = std::chrono::system_clock::now().time_since_epoch().count();
+    int64_t nnow = std::chrono::system_clock::now().time_since_epoch().count();
 
     if((nnow - gnLastAccUptime)>1e9)return torque;
 
@@ -127,6 +128,7 @@ double  PIDTorque(double torque)
     const double fRatio = 2.5;
     double accAim = (torque * fRatio - fRollForce)/fVehWeight;
 
+    static bool bPrintTitle = false;
 
 
     double error = accAim - gfVehAcc;
@@ -143,13 +145,19 @@ double  PIDTorque(double torque)
 
     error_previous = error;
 
+    if(bPrintTitle == false)
+    {
+        std::cout<<"Time\t"<<"Speed(km/h)\t"<<"RealAcc(m/s2)\t"<<"AimAcc(m/s2)\t"<<"AjustAcc(m/s2)\t"<<"Error(m/s2)\t"<<"RawTorque\t"<<"AjustTorque"<<std::endl;
+        bPrintTitle = true;
+    }
+
     if(gbPrintPIDOut)
     {
         int64_t timesecond = nnow/1e9;
         int64_t timepart = nnow - timesecond * 1e9;
         double fnow = timepart; fnow = fnow/1e9;
         fnow = fnow + timesecond;
-        std::cout<<fnow<<"\t"<<gfVehSpd<<"\t"<<accAim<<"\t"<<fAccAjust<<"\t"<<error<<"\t"<<torque<<"\t"<<ftorqueAjust<<std::endl;
+        std::cout<<fnow<<"\t"<<gfVehSpd<<"\t"<<gfVehAcc<<accAim<<"\t"<<fAccAjust<<"\t"<<error<<"\t"<<torque<<"\t"<<ftorqueAjust<<std::endl;
     }
 
     return ftorqueAjust;
@@ -173,7 +181,7 @@ void executeDecition(const iv::brain::decition &decition)
     if(fwheel<-430)fwheel = 430;
     if(fwheel>380)fwheel = 380;
 
-    double ftorque = decition.torque();
+    double ftorque = decition.torque() * TorqueRatio;
     double fbrake = decition.brake();
 
 #ifdef PIDTEST
@@ -193,6 +201,8 @@ void executeDecition(const iv::brain::decition &decition)
         ftorque = PIDTorque(acctotorque(fpidtestacc));
         fbrake = 0;
     }
+#else
+    ftorque = PIDTorque(ftorque);
 #endif
 
 
@@ -797,6 +807,7 @@ void Listencanrecv0()
                     double facc = value;
                     facc = facc * 0.001 - 2.0;
                     gfVehAcc = facc*9.8;
+                    gnLastAccUptime = std::chrono::system_clock::now().time_since_epoch().count();
   //                  std::cout<<" acc : "<<gfVehAcc<<std::endl;
                 }
             }
@@ -896,6 +907,13 @@ int main(int argc, char *argv[])
     gstrmemdecition = xp.GetParam("dection","deciton");
     gstrmemchassis = xp.GetParam("chassismsgname","chassis");
 
+    kp = xp.GetParam("kp",0.5);
+    ki = xp.GetParam("ki",0.3);
+    kd = xp.GetParam("kd",0.01);
+    gbPrintPIDOut = xp.GetParam("PringtPIDOut",false);
+    TorqueRatio = xp.GetParam("TorqueRatio",1.4);
+
+
     gpacansend = iv::modulecomm::RegisterSend(gstrmemcansend.data(),10000,1);
     gpadecition = iv::modulecomm::RegisterRecv(gstrmemdecition.data(),ListenDeciton);
     gpachassis = iv::modulecomm::RegisterRecv(gstrmemchassis.data(),UpdateChassis);

src/test/testcandbc/main.cpp

@@ -41,8 +41,26 @@ void testpack()
     xpv[1].value = 6.0;
 }
 
+// #include <iostream>
+// #include <thread>
+// #include <iomanip>
+// void test()
+// {
+//     while(1)
+//     {
+//         int64_t nnow= std::chrono::system_clock::now().time_since_epoch().count();
+//         int64_t timesecond = nnow/1e9;
+//         int64_t timepart = nnow - timesecond * 1e9;
+//         double fnow = timepart; fnow = fnow/1e9;
+//         fnow = fnow + timesecond;
+//         std::cout<<std::setprecision(16)<<fnow<<"\t"<<std::endl;
+//         std::this_thread::sleep_for(std::chrono::milliseconds(10));
+//     }
+// }
+
 int main(int argc, char *argv[])
 {
+//    test();
 //    testpack();
     QApplication a(argc, argv);
     MainWindow w;