1、增加细节曲线图反馈

2、优化算法

src/canbus/CMakeLists.txt

@@ -50,6 +50,7 @@ add_message_files(
   canMsg.msg
   obdMsg.msg
   rackTestReplyMsg.msg
+  rackTestReplyDetailMsg.msg
 )
 
 ## Generate services in the 'srv' folder

src/canbus/canbus.cpp

@@ -102,6 +102,7 @@ int Canbus::Start() {
   canRCUData_pub = node_handle_->advertise<canbus::canMsg>(robot::common::CanDataAnalysis, 1);
   canObdData_pub = node_handle_->advertise<canbus::obdMsg>(robot::common::ObdData, 1);
   canRackData_pub = node_handle_->advertise<canbus::rackTestReplyMsg>(robot::common::RackReplyData,1);
+  canRackDetailData_pub = node_handle_->advertise<canbus::rackTestReplyDetailMsg>(robot::common::RackReplyDetailData,1);
 
   ros::ServiceServer ActuatorEnableService = node_handle_->advertiseService(robot::common::ActuatorEnableService,
                                                                 &Canbus::ActuatorEnableRequest, this);
@@ -189,48 +190,6 @@ int Canbus::Start() {
   ros::Subscriber GeometrySub = node_handle_->subscribe(robot::common::Geometry, 1,
                                                         &Canbus::RTKLocationCallback, this);
 
-
-  string str = "/home/madesheng/angle.txt";
-  int flag = Canbus::read_txt(str,1);
-  if(flag != ROBOT_SUCCESS)
-  {
-    return ROBOT_FAILTURE;
-  }
-   str = "/home/madesheng/psng.txt";
-   flag = Canbus::read_txt(str,2);
-   if(flag != ROBOT_SUCCESS)
-   {
-     return ROBOT_FAILTURE;
-   }
-
-   str = "/home/madesheng/acc.txt";
-   flag = Canbus::read_txt(str,3);
-   if(flag != ROBOT_SUCCESS)
-   {
-     return ROBOT_FAILTURE;
-   }
-
-   str = "/home/madesheng/angleLine.txt";
-   flag = Canbus::read_txt(str,4);
-   if(flag != ROBOT_SUCCESS)
-   {
-     return ROBOT_FAILTURE;
-   }
-
-   str = "/home/madesheng/sft.txt";
-   flag = Canbus::read_txt(str,5);
-   if(flag != ROBOT_SUCCESS)
-   {
-     return ROBOT_FAILTURE;
-   }
-
-   str = "/home/madesheng/NEDC Speed.txt";
-   flag = Canbus::read_txt(str,6);
-   if(flag != ROBOT_SUCCESS)
-   {
-     return ROBOT_FAILTURE;
-   }
-
   //data receive
   while(ros::ok())
   {
@@ -248,45 +207,45 @@ int Canbus::Start() {
 void Canbus::OnTimer(const ros::TimerEvent &) {
   //LOG(INFO)<< Name()<< " timer start ..";
   //TODO dakai
-  if(heatbeat > 15)
-  {
-      heatbeat = 0;
-  }
-  icuMonitor.uiICUAlive = heatbeat++;
-
-  try
-  {
-      canObdData_pub.publish(obd);
-      TPCANMsg canIcuMonitor; //ICUMonitor Can Message
-      MessageCoder::CanMsgPackage((void*)&icuMonitor, robot::common::ICUMonitor, &canIcuMonitor);
-      write_data(&canIcuMonitor, false);
-
-      if(workModeCnfg.uiOnlineOffline == VEH_DATA_TYPE_VIHICLE_DEPEND)
-      {
-        TPCANMsg canStatus1; //VehicleStaus1 Can Message
-        MessageCoder::CanMsgPackage((void*)&status1, robot::common::VehicleStatus1, &canStatus1);
-        write_data(&canStatus1, false);
-        TPCANMsg canStatus2;  //VehicleStatus2 Can Message
-        MessageCoder::CanMsgPackage((void*)&status2, robot::common::VehicleStatus2, &canStatus2);
-        write_data(&canStatus2, false);
-        TPCANMsg canStatus3;  //VehicleStatus3 Can Message
-        MessageCoder::CanMsgPackage((void*)&status3, robot::common::VehicleStatus3, &canStatus3);
-        write_data(&canStatus3, false);
-      }
-      else if(workModeCnfg.uiOnlineOffline == VEH_DATA_TYPE_STAND_ALONE)
-      {
-        status2.iStrAng = 0;
-        status2.iStrAngGrd = 0;
-        status2.uiEngSpd = 0;
-        TPCANMsg canStatus2;  //VehicleStatus2 Can Message
-        MessageCoder::CanMsgPackage((void*)&status2, robot::common::VehicleStatus2, &canStatus2);
-        write_data(&canStatus2, false);
-      }
-  }
-  catch (ros::Exception ex)
-  {
-       LOG(ERROR)<<Name()<< " OnTimer error. "<< ex.what();
-  }
+//  if(heatbeat > 15)
+//  {
+//      heatbeat = 0;
+//  }
+//  icuMonitor.uiICUAlive = heatbeat++;
+
+//  try
+//  {
+//      canObdData_pub.publish(obd);
+//      TPCANMsg canIcuMonitor; //ICUMonitor Can Message
+//      MessageCoder::CanMsgPackage((void*)&icuMonitor, robot::common::ICUMonitor, &canIcuMonitor);
+//      write_data(&canIcuMonitor, false);
+
+//      if(workModeCnfg.uiOnlineOffline == VEH_DATA_TYPE_VIHICLE_DEPEND)
+//      {
+//        TPCANMsg canStatus1; //VehicleStaus1 Can Message
+//        MessageCoder::CanMsgPackage((void*)&status1, robot::common::VehicleStatus1, &canStatus1);
+//        write_data(&canStatus1, false);
+//        TPCANMsg canStatus2;  //VehicleStatus2 Can Message
+//        MessageCoder::CanMsgPackage((void*)&status2, robot::common::VehicleStatus2, &canStatus2);
+//        write_data(&canStatus2, false);
+//        TPCANMsg canStatus3;  //VehicleStatus3 Can Message
+//        MessageCoder::CanMsgPackage((void*)&status3, robot::common::VehicleStatus3, &canStatus3);
+//        write_data(&canStatus3, false);
+//      }
+//      else if(workModeCnfg.uiOnlineOffline == VEH_DATA_TYPE_STAND_ALONE)
+//      {
+//        status2.iStrAng = 0;
+//        status2.iStrAngGrd = 0;
+//        status2.uiEngSpd = 0;
+//        TPCANMsg canStatus2;  //VehicleStatus2 Can Message
+//        MessageCoder::CanMsgPackage((void*)&status2, robot::common::VehicleStatus2, &canStatus2);
+//        write_data(&canStatus2, false);
+//      }
+//  }
+//  catch (ros::Exception ex)
+//  {
+//       LOG(ERROR)<<Name()<< " OnTimer error. "<< ex.what();
+//  }
   //TODO
 }
 
@@ -333,23 +292,23 @@ int Canbus::read_loop()
     TPCANRdMsg msg;
     __u32 status;
     //TODO dakai
-    if (funLINUX_CAN_Read(pcan_handle, &msg)) {
-        LOG(ERROR)<<Name()<<" receive: LINUX_CAN_Read() error.";
-        return errno;
-    }
-
-    read_data(&(msg.Msg));
+//    if (funLINUX_CAN_Read(pcan_handle, &msg)) {
+//        LOG(ERROR)<<Name()<<" receive: LINUX_CAN_Read() error.";
+//        return errno;
+//    }
 
-     // deal receive data
-    // check if a CAN status is pending
-    if (msg.Msg.MSGTYPE & MSGTYPE_STATUS) {
-        status = funCAN_Status(pcan_handle);
-        if ((int)status < 0) {
-            errno = funnGetLastError();
-            LOG(ERROR)<<Name()<<" receive: CAN_Status() error.";
-            return errno;
-        }
-    }
+//    read_data(&(msg.Msg));
+
+//     // deal receive data
+//    // check if a CAN status is pending
+//    if (msg.Msg.MSGTYPE & MSGTYPE_STATUS) {
+//        status = funCAN_Status(pcan_handle);
+//        if ((int)status < 0) {
+//            errno = funnGetLastError();
+//            LOG(ERROR)<<Name()<<" receive: CAN_Status() error.";
+//            return errno;
+//        }
+//    }
 //TODO
     return ROBOT_SUCCESS;
 }
@@ -1162,6 +1121,13 @@ bool Canbus::StrTxtReq(canbus::StrTxtReq::Request &req,canbus::StrTxtReq::Respon
         res.uiReturn = 1;
         return true;
       }
+      string str = "/home/madesheng/angle.txt";
+      int flag = Canbus::read_txt(str,1);
+      if(flag != ROBOT_SUCCESS)
+      {
+        res.uiReturn = 0;
+        return false;
+      }
       if(strTxtTimerState)
       {
         str_txt_timer_.stop();
@@ -1238,6 +1204,13 @@ bool Canbus::BrkTxtReq(canbus::BrkTxtReq::Request &req,canbus::BrkTxtReq::Respon
       res.uiReturn = 1;
       return true;
     }
+    string str = "/home/madesheng/psng.txt";
+    int flag = Canbus::read_txt(str,2);
+    if(flag != ROBOT_SUCCESS)
+    {
+      res.uiReturn = 0;
+      return false;
+    }
     if(brkTxtTimerState)
     {
       brk_txt_timer_.stop();
@@ -1309,6 +1282,13 @@ bool Canbus::AccTxtReq(canbus::AccTxtReq::Request &req,canbus::AccTxtReq::Respon
       res.uiReturn = 1;
       return true;
     }
+    string str = "/home/madesheng/acc.txt";
+    int flag = Canbus::read_txt(str,3);
+    if(flag != ROBOT_SUCCESS)
+    {
+      res.uiReturn = 0;
+      return false;
+    }
     if(accTxtTimerState)
     {
       acc_txt_timer_.stop();
@@ -1378,6 +1358,13 @@ bool Canbus::StrTxtLineReq(canbus::StrTxtLineReq::Request &req,canbus::StrTxtLin
         res.uiReturn = 1;
         return true;
       }
+      string str = "/home/madesheng/angleLine.txt";
+      int flag = Canbus::read_txt(str,4);
+      if(flag != ROBOT_SUCCESS)
+      {
+        res.uiReturn = 0;
+        return false;
+      }
       if(strTxtLineTimerState)
       {
         str_txt_line_timer_.stop();
@@ -1446,6 +1433,13 @@ bool Canbus::SftTxtReq(canbus::SftTxtReq::Request &req,canbus::SftTxtReq::Respon
       res.uiReturn = 1;
       return true;
     }
+    string str = "/home/madesheng/sft.txt";
+    int flag = Canbus::read_txt(str,5);
+    if(flag != ROBOT_SUCCESS)
+    {
+      res.uiReturn = 0;
+      return false;
+    }
     if(sftTxtTimerState)
     {
       sft_txt_timer_.stop();
@@ -1607,7 +1601,7 @@ int Canbus::read_txt(string &path,int flag)
   }
   else if(flag == 7)
   {
-    for (int i = 0; i < 3; i++)
+    for (int i = 0; i < 10; i++)
     {
         if(myfile.eof())
         {
@@ -1617,7 +1611,7 @@ int Canbus::read_txt(string &path,int flag)
         myfile >> fuzzy_pid[i];
 
     }
-    cout << "Fuzzy PID NUMBER:" <<fuzzy_pid_number<<endl;
+   // cout << "Fuzzy PID NUMBER:" <<fuzzy_pid_number<<endl;
   }
 
 
@@ -2044,20 +2038,31 @@ bool Canbus::RackTestReq(canbus::RackTestReq::Request &req,
       if(rackTestState)
       {
          //TODO zhushidiao
-        //rack_test_timer_.stop();
+        rack_test_timer_.stop();
          //TODO
         rackTestState = false;
       }
       if(rackTestSendTimerState)
       {
         rack_reply_timer_.stop();
+        rack_reply_detail_timer_.stop();
         rackTestSendTimerState = false;
         rackTestReplyState = 0;
       }
       publish_rack_speed();
+      publis_rack_detail_speed();
       res.uiReturn = 1;
       return true;
     }
+    nedc_speeds[1182] = {0};
+    string str = "/home/madesheng/NEDC Speed.txt";
+    int flag = Canbus::read_txt(str,6);
+    if(flag != ROBOT_SUCCESS)
+    {
+      res.uiReturn = 0;
+      return false;
+    }
+
     if(rackTestState || rackTestSendTimerState)
     {
       res.uiReturn = 2;
@@ -2065,15 +2070,19 @@ bool Canbus::RackTestReq(canbus::RackTestReq::Request &req,
     }
     nedc_speed_count = 0;
     rack_test_send_count = 0;
+    set_speed_last = 0;
 //TODO zhushidiao
-   // rack_test_timer_ = CreateTimer(ros::Duration(duration_rack), &Canbus::RackTestOnTimer, this,false,false);
-  //  rack_test_timer_.start();
+    rack_test_timer_ = CreateTimer(ros::Duration(duration_rack), &Canbus::RackTestOnTimer, this,false,false);
+    rack_test_timer_.start();
     //TODO
     rackTestState = true;
 
     rack_reply_timer_ =
         CreateTimer(ros::Duration(duration_rack_reply), &Canbus::RackTestReplyOnTimer, this,false,false);
     rack_reply_timer_.start();
+    rack_reply_detail_timer_ =
+        CreateTimer(ros::Duration(duration_rack_reply_detail), &Canbus::RackTestReplyDetailOnTimer, this,false,false);
+    rack_reply_detail_timer_.start();
     rackTestSendTimerState = true;
     res.uiReturn = 1;
   }
@@ -2089,28 +2098,36 @@ bool Canbus::RackTestReq(canbus::RackTestReq::Request &req,
 
 void Canbus::RackTestOnTimer(const ros::TimerEvent &event)
 {
-  PID PidControll;
   try
   {
     if(nedc_speed_count == nedc_speed_number)
     {
-      rack_test_timer_.stop();
       rackTestState = false;
       rack_reply_timer_.stop();
+      rack_reply_detail_timer_.stop();
       rackTestSendTimerState = false;
       rackTestReplyState = 0;
       publish_rack_speed();
+      publis_rack_detail_speed();
+      return;
+    }
+    if(!rackTestState)
+    {
       return;
     }
+ //   LOG(INFO)<< "RackTestOnTimer Start FUZZY PID";
+    PID PidControll;
+
 
     gettimeofday(&now_time, NULL);
     float delta_time = 0.0;
     if(sizeof(last_time) != 0)
     {
       long elapsed = (now_time.tv_sec-last_time.tv_sec) * 1000000 + (now_time.tv_usec - last_time.tv_usec);
-      delta_time = elapsed / 1000000;
+      delta_time = (float)elapsed / 1000000;
     }
-    cout << "time sub" << delta_time <<endl;
+
+  //  LOG(INFO)<< "delta time:"<<std::setprecision(10)<<delta_time<<" s";
 
     last_time = now_time;
 
@@ -2125,80 +2142,135 @@ void Canbus::RackTestOnTimer(const ros::TimerEvent &event)
     {
       set_speed2 = nedc_speeds[nedc_speed_count + 1];
     }
-    float set_speed = set_speed1 + (set_speed2 - set_speed1) * delta_time;
-    float actual_speed = status2.uiVehSpd;
-
-    string str = "/home/madesheng/FuzzyPidFactor.txt";
+    float set_speed = 0;
+    if(set_speed1 == 0 && set_speed2 ==0)
+    {
+      set_speed = 0;
+    }
+    else
+    {
+      set_speed = set_speed_last + (set_speed2 - set_speed1) * delta_time;
+    }
+    if(set_speed < 0)
+    {
+      set_speed = 0;
+    }
+    float actual_speed = msg_RTK.Speed;
+    if(actual_speed - set_speed > 20 || actual_speed - set_speed < -20)
+    {
+     // LOG(INFO)<< "actual_speed too more";
+     // return;
+    }
+    set_speed_reply = set_speed;
+//    LOG(INFO)<< "nedc_speed_count:"<<nedc_speed_count;
+    string str = "/home/madesheng/FuzzyPid.txt";
     int flag = Canbus::read_txt(str,7);
     if(flag != ROBOT_SUCCESS)
     {
-      LOG(INFO)<< "Get FuzzyPIDFactor.txt error";
+      LOG(INFO)<< "Get FuzzyPID.txt error";
       return;
     }
 
     float pi_out = 0.01;
     pi_out = PidControll.FuzzyPIDcontroller(set_speed,actual_speed,delta_time,
-                                            fuzzy_pid[0],fuzzy_pid[1],fuzzy_pid[2]);
+                                            fuzzy_pid[0],fuzzy_pid[1],fuzzy_pid[2],
+                                            fuzzy_pid[3],fuzzy_pid[4],fuzzy_pid[5]);
+
+//    LOG(INFO)<< "P:"<<std::setprecision(10)<<fuzzy_pid[0];
+//    LOG(INFO)<< "I:"<<std::setprecision(10)<<fuzzy_pid[1];
+//    LOG(INFO)<< "D:"<<std::setprecision(10)<<fuzzy_pid[2];
+
+//    LOG(INFO)<< "P_F:"<<std::setprecision(10)<<fuzzy_pid[3];
+//    LOG(INFO)<< "I_F:"<<std::setprecision(10)<<fuzzy_pid[4];
+//    LOG(INFO)<< "D_F:"<<std::setprecision(10)<<fuzzy_pid[5];
 
-    LOG(INFO)<< "P:"<<std::setprecision(18)<<fuzzy_pid[0];
-    LOG(INFO)<< "I:"<<std::setprecision(18)<<fuzzy_pid[1];
-    LOG(INFO)<< "D:"<<std::setprecision(18)<<fuzzy_pid[2];
-    LOG(INFO)<< "Set Speed:"<<std::setprecision(18)<<set_speed;
-    LOG(INFO)<< "Actual Speed:"<<std::setprecision(18)<<actual_speed;
-    LOG(INFO)<< "pi_out:"<<std::setprecision(18)<<pi_out;
+//    LOG(INFO)<< "Set Speed:"<<std::setprecision(8)<<set_speed;
+//    LOG(INFO)<< "Actual Speed:"<<std::setprecision(8)<<actual_speed;
+//    LOG(INFO)<< "pi_out:"<<std::setprecision(8)<<pi_out;
 
     TPCANMsg canBrkPedalActCtrReqMsg;
     T_BRK_PEDAL_ACT_CTR_REQ requestBrk;
     TPCANMsg canAccPedalActCtrReqMsg;
     T_ACC_PEDAL_ACT_CTR_REQ requestAcc;
 
-    if(set_speed == 0)
+    if(set_speed == 0 && actual_speed < 3 || nedc_speed_count == nedc_speed_number - 1)
     {
+      requestAcc.uiAccSpdCtrReq = 0;
+      requestAcc.uiAccAccCtrReq = 0;
+      requestAcc.uiAccPsngCtrReq = 0;
+      requestAcc.uiAccDecCtrReq = 0;
+
+      MessageCoder::CanMsgPackage((void*)&requestAcc, robot::common::AccPedalActCtrReq, &canAccPedalActCtrReqMsg);
+ //     write_data(&canAccPedalActCtrReqMsg,true);
 
-          requestBrk.uiBrkSpdCtrReq = 0;
+          requestBrk.uiBrkSpdCtrReq = 5;
           requestBrk.uiBrkAccCtrReq = 0;
-          requestBrk.uiBrkCtrMdReq = 0;
+          requestBrk.uiBrkCtrMdReq = 1;
           requestBrk.uiBrkDecCtrReq = 0;
           requestBrk.uiBrkFocCtrReq = 0;
-          requestBrk.uiBrkPsngCtrReq = (50/30) * 5;
+          requestBrk.uiBrkPsngCtrReq = 35 * 10;
 
-          LOG(INFO)<< "Send Brk Psng:"<<requestBrk.uiBrkPsngCtrReq;
+//          LOG(INFO)<< "Send Brk Psng:"<<requestBrk.uiBrkPsngCtrReq;
 
           MessageCoder::CanMsgPackage((void*)&requestBrk, robot::common::BrkPedalActCtrReq, &canBrkPedalActCtrReqMsg);
-          write_data(&canBrkPedalActCtrReqMsg);
+//          write_data(&canBrkPedalActCtrReqMsg,true);
     }
     else
     {
       if(pi_out >= 0)
       {
+
+        requestBrk.uiBrkSpdCtrReq = 0;
+        requestBrk.uiBrkAccCtrReq = 0;
+        requestBrk.uiBrkCtrMdReq = 0;
+        requestBrk.uiBrkDecCtrReq = 0;
+        requestBrk.uiBrkFocCtrReq = 0;
+        requestBrk.uiBrkPsngCtrReq = 0;
+
+        MessageCoder::CanMsgPackage((void*)&requestBrk, robot::common::BrkPedalActCtrReq, &canBrkPedalActCtrReqMsg);
+//        write_data(&canBrkPedalActCtrReqMsg,true);
+
          requestAcc.uiAccSpdCtrReq = 0;
          requestAcc.uiAccAccCtrReq = 0;
-         requestAcc.uiAccPsngCtrReq = pi_out *100;
+         requestAcc.uiAccPsngCtrReq = pi_out *100 * 10;
          requestAcc.uiAccDecCtrReq = 0;
 
-         LOG(INFO)<< "Send Acc Psng:"<<requestAcc.uiAccPsngCtrReq;
+//         LOG(INFO)<< "Send Acc Psng:"<<requestAcc.uiAccPsngCtrReq;
 
          MessageCoder::CanMsgPackage((void*)&requestAcc, robot::common::AccPedalActCtrReq, &canAccPedalActCtrReqMsg);
-         write_data(&canAccPedalActCtrReqMsg);
+//         write_data(&canAccPedalActCtrReqMsg,true);
 
       }
       else
       {
-        requestBrk.uiBrkSpdCtrReq = 0;
+
+        requestAcc.uiAccSpdCtrReq = 0;
+        requestAcc.uiAccAccCtrReq = 0;
+        requestAcc.uiAccPsngCtrReq = 0;
+        requestAcc.uiAccDecCtrReq = 0;
+
+        MessageCoder::CanMsgPackage((void*)&requestAcc, robot::common::AccPedalActCtrReq, &canAccPedalActCtrReqMsg);
+//        write_data(&canAccPedalActCtrReqMsg,true);
+
+        requestBrk.uiBrkSpdCtrReq = 100;
         requestBrk.uiBrkAccCtrReq = 0;
-        requestBrk.uiBrkCtrMdReq = 0;
+        requestBrk.uiBrkCtrMdReq = 1;
         requestBrk.uiBrkDecCtrReq = 0;
         requestBrk.uiBrkFocCtrReq = 0;
-        requestBrk.uiBrkPsngCtrReq = (50/30) * (-pi_out * 30);
+        requestBrk.uiBrkPsngCtrReq = (85 * (-pi_out * 30) / 30) * 10;
+        if(requestBrk.uiBrkPsngCtrReq > 350)
+        {
+          requestBrk.uiBrkPsngCtrReq = 350;
+        }
 
-        LOG(INFO)<< "Send Brk Psng:"<<requestBrk.uiBrkPsngCtrReq;
+//        LOG(INFO)<< "Send Brk Psng:"<<requestBrk.uiBrkPsngCtrReq;
 
         MessageCoder::CanMsgPackage((void*)&requestBrk, robot::common::BrkPedalActCtrReq, &canBrkPedalActCtrReqMsg);
-        write_data(&canBrkPedalActCtrReqMsg);
+//        write_data(&canBrkPedalActCtrReqMsg,true);
       }
     }
+    set_speed_last = set_speed;
     nedc_speed_count = rack_test_send_count;
-
   }
   catch (ros::Exception ex)
   {
@@ -2230,6 +2302,27 @@ void Canbus::RackTestReplyOnTimer(const ros::TimerEvent &event)
 
 }
 
+void Canbus::RackTestReplyDetailOnTimer(const ros::TimerEvent &event)
+{
+  try
+  {
+    if(rackTestSendTimerState)
+    {
+       rackTestReplyState = 1;
+    }
+    else
+    {
+      rackTestReplyState = 0;
+    }
+    // 发布消息
+    publis_rack_detail_speed();
+  }
+  catch (ros::Exception ex)
+  {
+    LOG(ERROR)<<Name()<< " RackTestReplyDetailOnTimer error. "<< ex.what();
+  }
+}
+
 //获取当前时间和毫秒
 int Canbus::get_datetime_mill(tm *datetime)
 {
@@ -2269,14 +2362,40 @@ int Canbus::get_datetime_mill(tm *datetime)
 //发布台架测试任务实际车速
 void Canbus::publish_rack_speed()
 {
-  canbus::rackTestReplyMsg rackMsg;
-  //rackMsg.ActualSpeed = nedc_speeds[nedc_speed_count];
-  rackMsg.ActualSpeed = msg_RTK.Speed;
-  rackMsg.RackTestReplyState = rackTestReplyState;
-  canRackData_pub.publish(rackMsg);
+  try
+  {
+    canbus::rackTestReplyMsg rackMsg;
+    //TODO
+    rackMsg.ActualSpeed = nedc_speeds[nedc_speed_count];
+    //rackMsg.ActualSpeed = msg_RTK.Speed;
+    rackMsg.RackTestReplyState = rackTestReplyState;
+    canRackData_pub.publish(rackMsg);
+  }
+  catch (ros::Exception ex)
+  {
+    LOG(ERROR)<<Name()<< " publish rackTestReplyMsg error. "<< ex.what();
+  }
 
 }
 
+void Canbus::publis_rack_detail_speed()
+{
+  try
+  {
+    canbus::rackTestReplyDetailMsg detailMsg;
+    //TODO
+    //detailMsg.ActualSpeed = msg_RTK.Speed;
+    detailMsg.ActualSpeed = set_speed_reply;
+    detailMsg.SetSpeed = set_speed_reply;
+    detailMsg.RackTestReplyState = rackTestReplyState;
+    canRackDetailData_pub.publish(detailMsg);
+  }
+  catch (ros::Exception ex)
+  {
+    LOG(ERROR)<<Name()<< " publish rackTestReplyDetailMsg error. "<< ex.what();
+  }
+}
+
 // 接收ＲＴＫ数据
 void Canbus::RTKLocationCallback(const localization::localMsg::ConstPtr& msg)
 {
@@ -2294,6 +2413,7 @@ void Canbus::RTKLocationCallback(const localization::localMsg::ConstPtr& msg)
       rackTestSendTimerState = false;
       rackTestReplyState = 0;
       publish_rack_speed();
+      publis_rack_detail_speed();
       return;
     }
     if(!rackTestState)
@@ -2312,8 +2432,7 @@ void Canbus::RTKLocationCallback(const localization::localMsg::ConstPtr& msg)
       delta_time = (float)elapsed / 1000000;
     }
 
-    LOG(INFO)<< "delta time:"<<std::setprecision(18)<<delta_time<<" s";
-   // cout << "time sub " << delta_time <<" s"<<endl;
+    LOG(INFO)<< "delta time:"<<std::setprecision(10)<<delta_time<<" s";
 
     last_time = now_time;
 
@@ -2328,34 +2447,65 @@ void Canbus::RTKLocationCallback(const localization::localMsg::ConstPtr& msg)
     {
       set_speed2 = nedc_speeds[nedc_speed_count + 1];
     }
-    float set_speed = set_speed1 + (set_speed2 - set_speed1) * delta_time;
+    float set_speed = 0;
+    if(set_speed1 == 0 && set_speed2 ==0)
+    {
+      set_speed = 0;
+    }
+    else
+    {
+      set_speed = set_speed_last + (set_speed2 - set_speed1) * delta_time;
+    }
+    if(set_speed < 0)
+    {
+      set_speed = 0;
+    }
     float actual_speed = msg_RTK.Speed;
-
-    string str = "/home/madesheng/FuzzyPidFactor.txt";
+    if(actual_speed - set_speed > 20 || actual_speed - set_speed < -20)
+    {
+      LOG(INFO)<< "actual_speed too more";
+      return;
+    }
+LOG(INFO)<< "nedc_speed_count:"<<nedc_speed_count;
+    set_speed_reply = set_speed;
+    string str = "/home/madesheng/FuzzyPid.txt";
     int flag = Canbus::read_txt(str,7);
     if(flag != ROBOT_SUCCESS)
     {
-      LOG(INFO)<< "Get FuzzyPIDFactor.txt error";
+      LOG(INFO)<< "Get FuzzyPID.txt error";
       return;
     }
 
+//    if(nedc_speed_count - 1 >=0 && nedc_speeds[nedc_speed_count - 1] > nedc_speeds[nedc_speed_count])
+//    {
+//      fuzzy_pid[3] = 0;
+//      fuzzy_pid[4] = 0;
+//      fuzzy_pid[5] = 0;
+//    }
+
     float pi_out = 0.01;
     pi_out = PidControll.FuzzyPIDcontroller(set_speed,actual_speed,delta_time,
-                                            fuzzy_pid[0],fuzzy_pid[1],fuzzy_pid[2]);
+                                            fuzzy_pid[0],fuzzy_pid[1],fuzzy_pid[2],
+                                            fuzzy_pid[3],fuzzy_pid[4],fuzzy_pid[5]);
+
+    LOG(INFO)<< "P:"<<std::setprecision(10)<<fuzzy_pid[0];
+    LOG(INFO)<< "I:"<<std::setprecision(10)<<fuzzy_pid[1];
+    LOG(INFO)<< "D:"<<std::setprecision(10)<<fuzzy_pid[2];
 
-    LOG(INFO)<< "P:"<<std::setprecision(18)<<fuzzy_pid[0];
-    LOG(INFO)<< "I:"<<std::setprecision(18)<<fuzzy_pid[1];
-    LOG(INFO)<< "D:"<<std::setprecision(18)<<fuzzy_pid[2];
-    LOG(INFO)<< "Set Speed:"<<std::setprecision(18)<<set_speed;
-    LOG(INFO)<< "Actual Speed:"<<std::setprecision(18)<<actual_speed;
-    LOG(INFO)<< "pi_out:"<<std::setprecision(18)<<pi_out;
+    LOG(INFO)<< "P_F:"<<std::setprecision(10)<<fuzzy_pid[3];
+    LOG(INFO)<< "I_F:"<<std::setprecision(10)<<fuzzy_pid[4];
+    LOG(INFO)<< "D_F:"<<std::setprecision(10)<<fuzzy_pid[5];
+
+    LOG(INFO)<< "Set Speed:"<<std::setprecision(8)<<set_speed;
+    LOG(INFO)<< "Actual Speed:"<<std::setprecision(8)<<actual_speed;
+    LOG(INFO)<< "pi_out:"<<std::setprecision(8)<<pi_out;
 
     TPCANMsg canBrkPedalActCtrReqMsg;
     T_BRK_PEDAL_ACT_CTR_REQ requestBrk;
     TPCANMsg canAccPedalActCtrReqMsg;
     T_ACC_PEDAL_ACT_CTR_REQ requestAcc;
 
-    if(set_speed == 0)
+    if(set_speed == 0 && actual_speed < 3 || nedc_speed_count == nedc_speed_number - 1)
     {
       requestAcc.uiAccSpdCtrReq = 0;
       requestAcc.uiAccAccCtrReq = 0;
@@ -2365,9 +2515,9 @@ void Canbus::RTKLocationCallback(const localization::localMsg::ConstPtr& msg)
       MessageCoder::CanMsgPackage((void*)&requestAcc, robot::common::AccPedalActCtrReq, &canAccPedalActCtrReqMsg);
       write_data(&canAccPedalActCtrReqMsg,true);
 
-          requestBrk.uiBrkSpdCtrReq = 0;
+          requestBrk.uiBrkSpdCtrReq = 5;
           requestBrk.uiBrkAccCtrReq = 0;
-          requestBrk.uiBrkCtrMdReq = 0;
+          requestBrk.uiBrkCtrMdReq = 1;
           requestBrk.uiBrkDecCtrReq = 0;
           requestBrk.uiBrkFocCtrReq = 0;
           requestBrk.uiBrkPsngCtrReq = 35 * 10;
@@ -2414,12 +2564,16 @@ void Canbus::RTKLocationCallback(const localization::localMsg::ConstPtr& msg)
         MessageCoder::CanMsgPackage((void*)&requestAcc, robot::common::AccPedalActCtrReq, &canAccPedalActCtrReqMsg);
         write_data(&canAccPedalActCtrReqMsg,true);
 
-        requestBrk.uiBrkSpdCtrReq = 0;
+        requestBrk.uiBrkSpdCtrReq = 100;
         requestBrk.uiBrkAccCtrReq = 0;
-        requestBrk.uiBrkCtrMdReq = 0;
+        requestBrk.uiBrkCtrMdReq = 1;
         requestBrk.uiBrkDecCtrReq = 0;
         requestBrk.uiBrkFocCtrReq = 0;
-        requestBrk.uiBrkPsngCtrReq = (80 * (-pi_out * 30) / 30) * 10;
+        requestBrk.uiBrkPsngCtrReq = (85 * (-pi_out * 30) / 30) * 10;
+        if(requestBrk.uiBrkPsngCtrReq > 350)
+        {
+          requestBrk.uiBrkPsngCtrReq = 350;
+        }
 
         LOG(INFO)<< "Send Brk Psng:"<<requestBrk.uiBrkPsngCtrReq;
 
@@ -2427,6 +2581,7 @@ void Canbus::RTKLocationCallback(const localization::localMsg::ConstPtr& msg)
         write_data(&canBrkPedalActCtrReqMsg,true);
       }
     }
+    set_speed_last = set_speed;
     nedc_speed_count = rack_test_send_count;
 
   }

src/canbus/canbus.h

@@ -60,6 +60,7 @@
 #include <canbus/SftTxtReq.h>
 #include <canbus/RackTestReq.h>
 #include <canbus/rackTestReplyMsg.h>
+#include <canbus/rackTestReplyDetailMsg.h>
 #include <localization/localMsg.h>
 #include <ctime>
 
@@ -148,6 +149,8 @@ class Canbus : public robot::common::RobotApp{
 
   void RackTestReplyOnTimer(const ros::TimerEvent &event); //台架测试任务回复消息定时器回调函数
 
+  void RackTestReplyDetailOnTimer(const ros::TimerEvent &event); //台架测试任务细节图所用数据回复消息定时器回调函数
+
   //define function pointer,there is a one-to-one mapping between target function and your defined function
   funCAN_Init_TYPE        funCAN_Init;
   funLINUX_CAN_Open_TYPE  funLINUX_CAN_Open;
@@ -184,6 +187,8 @@ class Canbus : public robot::common::RobotApp{
 
   void publish_rack_speed(); //发布台架测试任务实际车速
 
+  void publis_rack_detail_speed();   //发布台架测试任务细节图所用数据
+
   /**
   * @brief create services
   */
@@ -373,7 +378,12 @@ private:
    localization::localMsg msg_RTK;
 
    int fuzzy_pid_number = 0;
-   double fuzzy_pid[3] = {0};
+   double fuzzy_pid[6] = {0};
+   float set_speed_last = 0;
+
+   const double duration_rack_reply_detail = 0.1;//100ms //台架测试任务细节图所用数据回复时间
+   ros::Timer rack_reply_detail_timer_;  //台架测试任务细节图所用数据回复定时器
+   float set_speed_reply = 0;
 
 
 
@@ -409,6 +419,8 @@ private:
 
   ros::Publisher canRackData_pub;  //发布台架测试任务回复数据包
 
+  ros::Publisher canRackDetailData_pub;  //发布台架测试任务细节图所用数据包
+
   canbus::obdMsg obd; // obd data
 
 };

src/canbus/msg/rackTestReplyDetailMsg.msg

@@ -0,0 +1,3 @@
+uint32 RackTestReplyState
+float64 SetSpeed
+float64 ActualSpeed

src/canbus/pid.cpp

@@ -286,6 +286,7 @@ void PID::GetOUT()
 
 //模糊PID控制实现函数
 float PID::FuzzyPIDcontroller(float TargetSpeed,float ActualSpeed,float Calculation_Time,
+                              float KP_BaseValue,float KI_BaseValue,float KD_BaseValue,
                               float KP_Factor,float KI_Factor,float KD_Factor)
 {
     float kp = KP_BaseValue;
@@ -361,10 +362,23 @@ float PID::FuzzyPIDcontroller(float TargetSpeed,float ActualSpeed,float Calculat
 
   error_i += erro;//累计误差的和
 
-  if( TargetSpeed == 0 ) //目标速度为0时清空积分的值 或 if( ActualSpeed == 0 )
+
+
+  if( TargetSpeed == 0 && ActualSpeed < 0.05) //目标速度为0时清空积分的值 或 if( ActualSpeed == 0 )
   {
     error_i = 0;
   }
+    kp = kp + detail_kp;
+    ki = ki + detail_ki;
+    kd = kd + detail_kd;
+  out_i = ki * error_i;//积分项输出
+
+  output = kp * erro  + out_i + kd*(erro - erro_pre);//输出
+
+//  LOG(INFO)<< "PID P :"<<kp;
+//  LOG(INFO)<< "PID P erro:"<<erro;
+//  LOG(INFO)<< "PID i :"<<ki;
+//  LOG(INFO)<< "PID i erro:"<<error_i;
 
   //积分限值
   if(ki != 0 && (error_i > (MaxValue  / ki)))
@@ -375,13 +389,6 @@ float PID::FuzzyPIDcontroller(float TargetSpeed,float ActualSpeed,float Calculat
   {
     error_i =  MinValue /ki;
   }
-    kp = kp + detail_kp;
-    ki = ki + detail_ki;
-    kd = kd + detail_kd;
-  out_i = ki * error_i;//积分项输出
-
-  output = kp * erro  + out_i + kd*(erro - erro_pre);//输出
-
 
 
     //输出限值

src/canbus/pid.h

@@ -4,22 +4,22 @@
  *  Created on: Dec 7, 2023
  *      Author: siasun
  */
-
+#include "canbus.h"
 #ifndef PID_H_
 #define PID_H_
 
 //#define Calculation_Time  0.01//10MS
 
-#define KP_BaseValue  0.06
-#define KI_BaseValue  0.000006
-#define KD_BaseValue  0.01
+//#define KP_BaseValue  0.06
+//#define KI_BaseValue  0.000006
+//#define KD_BaseValue  0.01
 
 //#define KP_Factor   6
 //#define KI_Factor   0.008
 //#define KD_Factor   0.001
 
-#define ERROR     120
-#define ERROR_C   240
+#define ERROR     6
+#define ERROR_C   400
 
 #define MaxValue  1.0
 #define MinValue -1.0
@@ -74,6 +74,7 @@ private:
 
 public:
   float FuzzyPIDcontroller(float TargetSpeed,float ActualSpeed,float Calculation_Time,
+                           float KP_BaseValue,float KI_BaseValue,float KD_BaseValue,
                            float KP_Factor,float KI_Factor,float KD_Factor);//模糊PID控制实现函数
   int test();
 

src/common/message.h

@@ -69,6 +69,8 @@ const std::string SmData = "smData";
 const std::string ZSftData = "zsftData";
 
 const std::string RackReplyData = "rackreplydata";
+
+const std::string RackReplyDetailData = "rackreplydetaildata";
 // -----------------topics ----------------------------
 
 // -----------------services ----------------------------

src/localization/localization.cpp

@@ -265,6 +265,7 @@ int Localization::PublishData(NMEA0183 &nmea)
         double speeds = pow(eastSpeed, 2) + pow(northSpeed,2);
         localization.Speed = sqrt(speeds);
         geometry_pub.publish(localization);
+        LOG(INFO)<< "Test Speed:"<<std::setprecision(4)<<localization.Speed;
     }
     catch (ros::Exception ex)
     {
@@ -304,6 +305,9 @@ int Localization::ParaIFDYN(std::string &strIFDYN, IFDYN &ifdyn)
       ifdyn.AngleRateX = atof(nmea_ifdyn[11].c_str());
       ifdyn.AngleRateY = atof(nmea_ifdyn[12].c_str());
       ifdyn.AngleRateZ = atof(nmea_ifdyn[13].c_str());
+      double test = ifdyn.VelHori * SPEED_FACTOR;
+      LOG(INFO)<< "Test VelHori:"<<std::setprecision(4)<<test;
+
   }
   catch(ros::Exception ex)
   {
@@ -357,15 +361,15 @@ int Localization::Analyse_ReadLine(std::string &strSerialResult,  NMEA0183 &nmea
   {
     std::string type = strSerialResult.substr(1,5);
     std::string *str = &strSerialResult;
-    LOG(INFO)<<Name()<<" type:"<<type;
+  //  LOG(INFO)<<Name()<<" type:"<<type;
     if(type.compare("IFDYN") == 0)
     {
-      IFDYN tempIFDYN;
-      if(ParaIFDYN(*str,tempIFDYN) != ROBOT_SUCCESS)
-        return ROBOT_FAILTURE;
-      nmea.ifdyn = tempIFDYN;
-      nmea.ifdyn.print();
-      LOG(INFO)<<Name()<<" para IFDYN end.";
+//      IFDYN tempIFDYN;
+//      if(ParaIFDYN(*str,tempIFDYN) != ROBOT_SUCCESS)
+//        return ROBOT_FAILTURE;
+//      nmea.ifdyn = tempIFDYN;
+//      nmea.ifdyn.print();
+//      LOG(INFO)<<Name()<<" para IFDYN end.";
     }
     else if(type.compare("GPFPD") == 0)
     {
@@ -373,8 +377,8 @@ int Localization::Analyse_ReadLine(std::string &strSerialResult,  NMEA0183 &nmea
       if(ParaGPFPD(*str, tempGPFPD) != ROBOT_SUCCESS)
         return ROBOT_FAILTURE;
       nmea.gpfpd = tempGPFPD;
-      nmea.gpfpd.print();
-      LOG(INFO)<<Name()<<" para GPFPD end.";
+     // nmea.gpfpd.print();
+    //  LOG(INFO)<<Name()<<" para GPFPD end.";
     }
   }
   catch (ros::Exception ex)
@@ -382,7 +386,7 @@ int Localization::Analyse_ReadLine(std::string &strSerialResult,  NMEA0183 &nmea
       LOG(ERROR)<<Name()<< " Analyse_ReadLine  error" << ex.what();
       return ROBOT_FAILTURE;
   }
-  LOG(INFO)<<Name()<<" Analyse_ReadLine end.";
+ // LOG(INFO)<<Name()<<" Analyse_ReadLine end.";
   return ROBOT_SUCCESS;
 
 }