1、添加6F5挡位Z反馈

2、7F2、7F5、7F6、6E4、6E5添加挡位Z指令
3、添加挡位TXT文件测试

src/canbus/CMakeLists.txt

@@ -84,6 +84,7 @@ add_message_files(FILES
    StrAccCtrlReq.srv
    RbtSlfChckCtrlReq.srv
    IOMode.srv
+   SftTxtReq.srv
  )
 
 ## Generate actions in the 'action' folder

src/canbus/canbus.cpp

@@ -177,6 +177,8 @@ int Canbus::Start() {
   ros::ServiceServer IOModeService = node_handle_->advertiseService(robot::common::IOModeService ,
                                                                     &Canbus::IOMode,this);
 
+  ros::ServiceServer SftTxtService = node_handle_->advertiseService(robot::common::SftTxtService,
+                                                                    &Canbus::SftTxtReq,this);
 
   string str = "/home/madesheng/angle.txt";
   int flag = Canbus::read_txt(str,1);
@@ -205,6 +207,13 @@ int Canbus::Start() {
      return ROBOT_FAILTURE;
    }
 
+   str = "/home/madesheng/sft.txt";
+   flag = Canbus::read_txt(str,5);
+   if(flag != ROBOT_SUCCESS)
+   {
+     return ROBOT_FAILTURE;
+   }
+
   //data receive
   while(ros::ok())
   {
@@ -263,8 +272,8 @@ void Canbus::OnTimer(const ros::TimerEvent &) {
 }
 
 void Canbus::Stop() {
-  timer_.stop();
-  sleep(0.5);
+timer_.stop();
+sleep(0.5);
   TPCANMsg canIcu; //ICUMonitor Can Message
   icuMonitor.uiICUClsReq = 1;
   MessageCoder::CanMsgPackage((void*)&icuMonitor, robot::common::ICUMonitor, &canIcu);
@@ -332,7 +341,7 @@ int Canbus::read_data(TPCANMsg *pMsgBuff)
 //    char strData[16]={0};
 //    MessageCoder::ConvertUnCharToStr(strData,pMsgBuff->DATA,8);
 
-//    LOG(INFO)<<Name()<<" Receive CanData : ID: "<<std::string(strId)<<",Data: "<<std::string(strData);
+  //  LOG(INFO)<<Name()<<" Receive CanData : ID: "<<std::string(strId)<<",Data: "<<std::string(strData);
 
   //通过读取车辆OBD的数据，赋值给OBD的变量  原始代码 开始
 //    if(pMsgBuff->ID == robot::common::ObdAccPsng)
@@ -415,11 +424,14 @@ int Canbus::read_data(TPCANMsg *pMsgBuff)
   {
       publish_data(pMsgBuff);
   }
+
     //通过设置相关变量，赋值给OBD的变量   测试版代码 结束
     return ROBOT_SUCCESS;
 }
 
 void Canbus::publish_data(TPCANMsg *pMsgBuff)
+{
+try
 {
     canbus::canMsg rosMsg;
     rosMsg.id = pMsgBuff->ID;
@@ -433,7 +445,18 @@ void Canbus::publish_data(TPCANMsg *pMsgBuff)
     //std::string strData = chData;
     rosMsg.data = chData;
     canRCUData_pub.publish(rosMsg);
-//    LOG(INFO)<<Name()<<" Publish CanData :" << "ID:" << strId << ",Data:"<<std::string(chData);
+if(pMsgBuff->ID == robot::common::SysLearnBack)
+{
+  //  LOG(INFO)<<Name()<<" Publish CanData :" << "ID:" << strId << ",Data:"<<std::string(chData);
+}
+
+}
+    catch (ros::Exception ex)
+    {
+        LOG(ERROR)<< "publish failed "<< ex.what();
+    }
+
+
 }
 
 int Canbus::write_data(TPCANMsg *pMsgBuff, bool print)
@@ -465,6 +488,7 @@ bool Canbus::ActuatorEnableRequest(canbus::ActuatorEnableRequest::Request &req,
         request.uiStrMotEnReq = req.uiStrMotEnReq;
         request.uiXSftMotEnReq = req.uiXSftMotEnReq;
         request.uiYSftMotEnReq = req.uiYSftMotEnReq;
+        request.uiZSftMotEnReq = req.uiZSftMotEnReq;
 
         MessageCoder::CanMsgPackage((void*)&request, robot::common::ActuatorEnableReq, &canActuaEnableReqMsg);
         if (write_data(&canActuaEnableReqMsg)!=ROBOT_SUCCESS)
@@ -666,6 +690,7 @@ bool Canbus::RBTManager(canbus::RBTManager::Request &req,
         request.uiStrMotInt= req.uiStrMotInt;
         request.uiXSftMotInt = req.uiXSftMotInt;
         request.uiYSftMotInt = req.uiYSftMotInt;
+        request.uiZSftMotInt =req.uiZSftMotInt;
 
         MessageCoder::CanMsgPackage((void*)&request, robot::common::RobotManager, &canRobotManagerMsg);
         if (write_data(&canRobotManagerMsg) != ROBOT_SUCCESS)
@@ -700,6 +725,7 @@ bool Canbus::PowerOnRequest(canbus::PowerOnRequest::Request &req,
         request.uiXSftCtrPowReq = req.uiXSftArmCtrPowReq;
         request.uiYSftCtrPowReq = req.uiYSftArmCtrPowReq;
         request.uiDriveFanReq = req.uiDriveFanReq;
+        request.uiZSftArmCtrPowReq = req.uiZSftArmCtrPowReq;
 
         MessageCoder::CanMsgPackage((void*)&request, robot::common::PowerOnReq, &canPowerOnReqMsg);
 
@@ -1125,14 +1151,22 @@ void Canbus::StrTxtOnTimer(const ros::TimerEvent &event)
       str_txt_timer_.stop();
       strTxtTimerState = false;;
       angles_count = 0;
+LOG(INFO)<<Name()<< " StrTxt END. ";
       return;
     }
 
     TPCANMsg canStrActCtrReqMsg;
     int angle = angles[angles_count] * 100;
-
     tStrActCtrReqTxt.iStrAngCtrReq = angle;
-    LOG(INFO)<<Name()<< " angle:. " <<angle;
+    //20220805zw 增加速度配套转速文件
+    if (hasSpeedFile)
+    {
+      int angle_speed = angle_speeds[angles_count] * 100;
+      tStrActCtrReqTxt.iStrSpdCtrReq = angle_speed;
+      LOG(INFO)<<Name()<< " angle:. " <<angle<<" angle_speed:"<<angle_speed;
+    }
+    else
+      LOG(INFO)<<Name()<< " angle:. " <<angle;
     MessageCoder::CanMsgPackage((void*)&tStrActCtrReqTxt, robot::common::SteerActCtrReq, &canStrActCtrReqMsg);
     write_data(&canStrActCtrReqMsg,true);
     angles_count ++;
@@ -1195,6 +1229,7 @@ void Canbus::BrkTxtOnTimer(const ros::TimerEvent &event)
       brk_txt_timer_.stop();
       brkTxtTimerState = false;;
       psng_count = 0;
+LOG(INFO)<<Name()<< " BrkTxt END. ";
       return;
     }
 
@@ -1350,6 +1385,75 @@ void Canbus::StrTxtLineOnTimer(const ros::TimerEvent &event)
   }
 }
 
+bool Canbus::SftTxtReq(canbus::SftTxtReq::Request &req,canbus::SftTxtReq::Response &res)
+{
+  LOG(INFO)<< "controlservice start SftTxtReq";
+  // SftTxtReq request
+  try{
+    if(req.uiSftTxtFlag == 0)
+    {
+      if(sftTxtTimerState)
+      {
+        sft_txt_timer_.stop();
+        sftTxtTimerState = false;
+        sft_psng_count = 0;
+      }
+      res.uiReturn = 1;
+      return true;
+    }
+    if(sftTxtTimerState)
+    {
+      sft_txt_timer_.stop();
+      sftTxtTimerState = false;
+      sft_psng_count = 0;
+    }
+    sft_psng_count = 0;
+    tSftCtrReqTxt.iClchSpdCtrReq = req.iClchSpdCtrReq;
+    tSftCtrReqTxt.uiSftMdReq = req.uiSftMdReq;
+    tSftCtrReqTxt.uiClchPsngCtrReq = req.uiClchPsngCtrReq;
+
+    sft_txt_timer_ = CreateTimer(ros::Duration(duration_txt), &Canbus::SftTxtOnTimer, this,false,false);
+    sft_txt_timer_.start();
+    sftTxtTimerState = true;
+    res.uiReturn = 1;
+  }
+  catch (ros::Exception ex)
+  {
+      res.uiReturn = 0;
+      LOG(ERROR)<< "controlservice SftTxtReq failed "<< ex.what();
+      return true;
+  }
+  return true;
+}
+
+void Canbus::SftTxtOnTimer(const ros::TimerEvent &event)
+{
+  try
+  {
+    if(sft_psng_count == sft_psng_number)
+    {
+      sft_txt_timer_.stop();
+      sftTxtTimerState = false;;
+      sft_psng_count = 0;
+      return;
+    }
+
+    TPCANMsg canSftCtrReqMsg;
+    int psng = sft_psngs[sft_psng_count];
+
+    tSftCtrReqTxt.uiSftPsngCtrReq = psng;
+
+    LOG(INFO)<<Name()<< "sft psng:. " <<psng;
+    MessageCoder::CanMsgPackage((void*)&tSftCtrReqTxt, robot::common::ShiftActCtrReq, &canSftCtrReqMsg);
+    write_data(&canSftCtrReqMsg,true);
+    sft_psng_count ++;
+  }
+  catch (ros::Exception ex)
+  {
+       LOG(ERROR)<<Name()<< " OnTimer error. "<< ex.what();
+  }
+}
+
 int Canbus::read_txt(string &path,int flag)
 {
   //ifstream myfile("/home/madesheng/data.txt");
@@ -1359,18 +1463,21 @@ int Canbus::read_txt(string &path,int flag)
       LOG(ERROR)<<Name()<< "open src File  Error opening file" << endl;
       return ROBOT_FAILTURE;
   }
-  if(flag == 1)
+  //20220805zw 增加读取角度文件
+  ifstream speedfile("/home/siasun/speed.txt");
+  hasSpeedFile = speedfile.is_open();
+  //-------------------
+  if (flag == 1)
   {
-    for (int i = 0; i < 3000; i++)
+    for (int i = 0; i < 70000; i++)
     {
-
         if(myfile.eof())
         {
             angles_number = i;
             break;
         }
         myfile >> angles[i];
-
+        speedfile >> angle_speeds[i];
         //myfile >> temp;
         //cout <<"char:"<< temp<< endl;
        //cout <<"angle:"<< angles[i] <<endl;
@@ -1379,7 +1486,7 @@ int Canbus::read_txt(string &path,int flag)
   }
   else if(flag ==2)
   {
-    for (int i = 0; i < 6000; i++)
+    for (int i = 0; i < 50000; i++)
     {
         if(myfile.eof())
         {
@@ -1396,7 +1503,7 @@ int Canbus::read_txt(string &path,int flag)
   }
   else if(flag ==3)
   {
-    for (int i = 0; i < 8000; i++)
+    for (int i = 0; i < 80000; i++)
     {
         if(myfile.eof())
         {
@@ -1424,6 +1531,21 @@ int Canbus::read_txt(string &path,int flag)
     }
     cout << "angle line NUMBER:" <<angles_line_number<<endl;
   }
+  else if(flag == 5)
+  {
+    for (int i = 0; i < 70000; i++)
+    {
+        if(myfile.eof())
+        {
+            sft_psng_number  = i;
+            break;
+        }
+        myfile >> sft_psngs[i];
+
+      //    cout <<"psngs:"<< acc_psngs[i] <<endl;
+    }
+    cout << "sft NUMBER:" <<sft_psng_number<<endl;
+  }
 
 
    myfile.close();

src/canbus/canbus.h

@@ -57,6 +57,7 @@
 #include <canbus/RbtSlfChckCtrlReq.h>
 #include <canbus/IOMode.h>
 #include "std_msgs/String.h"
+#include <canbus/SftTxtReq.h>
 
 using namespace std;
 
@@ -138,6 +139,8 @@ class Canbus : public robot::common::RobotApp{
 
   void StrTxtLineOnTimer(const ros::TimerEvent &event);  //转向直线文件定时器回调函数
 
+  void SftTxtOnTimer(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;
@@ -259,6 +262,9 @@ class Canbus : public robot::common::RobotApp{
 
   bool IOMode(canbus::IOMode::Request &req,canbus::IOMode::Response &res);
 
+  bool SftTxtReq(canbus::SftTxtReq::Request &req,
+                 canbus::SftTxtReq::Response &res);
+
 private:
 
   int64_t last_timestamp_ = 0;
@@ -290,27 +296,29 @@ private:
   canbus::BrkLoopReq::Request brkLpReq;  //制动踏板机构循环指令
   T_BRK_PEDAL_ACT_CTR_REQ tBrkCtrReq = {0};  //制动踏板机构循环指令中发送的踏板执行机构控制命令
 
-   const double duration_txt = 0.02;//20ms
+   const double duration_txt = 0.01;//10ms//转向制动发送时间
 
-   const double duration_txt_acc = 0.01; //10ms
+   const double duration_txt_acc = 0.01; //10ms//油门发送时间
 
    ros::Timer str_txt_timer_;    //转向文件定时器
    bool strTxtTimerState = false;  //转向文件定时器标志位
-   double angles[3000] = {0};  //TXT文件中的角度值
+   double angles[70000] = {0};  //TXT文件中的角度值
+   bool hasSpeedFile = false; //是否存在角度文件对应的转速文件
+   double angle_speeds[70000] = {0};  //TXT文件中的转速值
    int angles_count = 0;   //TXT文件中的角度值的编号
    int angles_number = 0; //TXT文件中的角度值的数量
    T_STR_ACT_CTR_REQ tStrActCtrReqTxt = {0};
 
    ros::Timer brk_txt_timer_;    //制动文件定时器
    bool brkTxtTimerState = false;  //制动文件定时器标志位
-   double psngs[6000] = {0};  //TXT文件中的位置值
+   double psngs[50000] = {0};  //TXT文件中的位置值
    int psng_count = 0;   //TXT文件中的位置值的编号
    int psng_number = 0; //TXT文件中的位置值的数量
    T_BRK_PEDAL_ACT_CTR_REQ tBrkCtrReqTxt = {0};
 
    ros::Timer acc_txt_timer_;    //油门文件定时器
    bool accTxtTimerState = false;  //油门文件定时器标志位
-   int acc_psngs[8000] = {0};  //TXT文件中的位置值
+   int acc_psngs[69910] = {0};  //TXT文件中的位置值
    int acc_psng_count = 0;   //TXT文件中的位置值的编号
    int acc_psng_number = 0; //TXT文件中的位置值的数量
    T_ACC_PEDAL_ACT_CTR_REQ tAccCtrReqTxt = {0};
@@ -322,6 +330,13 @@ private:
    int angles_line_number = 0; //转向直线TXT文件中的角度值的数量
    T_STR_ACT_CTR_REQ tStrActCtrReqTxtLine = {0};
 
+   ros::Timer sft_txt_timer_;    //挡位文件定时器
+   bool sftTxtTimerState = false;  //挡位文件定时器标志位
+   int sft_psngs[69910] = {0};  //TXT文件中的位置值
+   int sft_psng_count = 0;   //TXT文件中的位置值的编号
+   int sft_psng_number = 0; //TXT文件中的位置值的数量
+   T_SFT_ACT_CTR_REQ tSftCtrReqTxt = {0};
+
 
   const double duration = 0.01;
 
@@ -329,7 +344,7 @@ private:
   //GLOBALS
   void *libm_handle = NULL;//define pointer used for file acess of libpcan.so
   HANDLE pcan_handle =NULL;//void *pcan_handle
-  unsigned short unBaudrate = CAN_BAUD_500K; //set the communicate baud rate of can bus
+  unsigned short unBaudrate = CAN_BAUD_1M; //set the communicate baud rate of can bus
   int nExtended = CAN_INIT_TYPE_ST;       //set can message int standard model   CAN_INIT_TYPE_EX/CAN_INIT_TYPE_ST
 
   T_VEHICLE_STATUS_1 status1 = {0};    //VehicleStaus1

src/canbus/candatatype.h

@@ -110,7 +110,9 @@ typedef struct
 /***********************控制器上电命令**********************/
 typedef struct
 {
-  unsigned int :32;
+  unsigned int :24;
+  unsigned int uiZSftArmCtrPowReq:2;	//换挡臂Z电机控制器上电
+  unsigned int :6;
   unsigned int uiYSftCtrPowReq:2;	//换挡臂Y电机控制器上电
   unsigned int :2;
   unsigned int uiDriveFanReq:2;   //驱动箱风扇使用请求
@@ -139,6 +141,7 @@ typedef struct
   unsigned int uiXSftCtrPowReq;	//换挡臂X电机控制器上电
   unsigned int uiYSftCtrPowReq;	//换挡臂Y电机控制器上电
   unsigned int uiDriveFanReq;     //驱动箱风扇使用请求
+  unsigned int uiZSftArmCtrPowReq;  //换挡臂Z电机控制器上电
 }T_POWER_ON_REQUEST;
 
 #define	POW_REQ_OFF	0		//下电
@@ -197,7 +200,8 @@ typedef struct
   unsigned int uiClchMotEnReq:1;	//离合电机使能
   unsigned int :1;
   unsigned int uiAccClchCtrReq:2;	//油门离合器控	制
-  unsigned int :2;
+  unsigned int :1;
+  unsigned int uiZSftMotEnReq:1;  //换挡臂 Z 电机使能
   unsigned int uiAccMotEnReq:1;	//油门电机使能
   unsigned int :1;
   unsigned int uiBrkMotEnReq:1;	//制动电机使能
@@ -217,6 +221,7 @@ typedef struct
   unsigned int uiYSftMotEnReq;	//换挡臂Y电机使能
   unsigned int uiClchMotEnReq;	//离合电机使能
   unsigned int uiAccClchCtrReq;	//油门离合器控	制
+  unsigned int uiZSftMotEnReq;  //换挡臂 Z 电机使能
 }T_ACTUATOR_ENABLE_REQUEST;
 
 #define	MOT_EN_REQ_DISABLE		0		//电机禁能
@@ -240,7 +245,8 @@ typedef struct
   unsigned int uiFltClr:1;		//故障清除
   unsigned int :7;
   unsigned int uiYSftMotInt:1;	//档位Y电机初始化
-  unsigned int :7;
+  unsigned int uiZSftMotInt:1;	//档位Z电机初始化
+  unsigned int :6;
   unsigned int uiXSftMotInt:1;	//档位X电机初始化
   unsigned int :7;
   unsigned int uiStrMotInt:1;		//转向电机初始化
@@ -262,6 +268,7 @@ typedef struct
   unsigned int uiXSftMotInt;		//档位X电机初始化
   unsigned int uiYSftMotInt;		//档位Y电机初始化
   unsigned int uiFltClr;			//故障清除
+  unsigned int uiZSftMotInt;	//档位Z电机初始化
 }T_RBT_MANAGER;
 
 #define	MOT_INT_ZERO		1		//电机初始化-标零
@@ -617,7 +624,7 @@ typedef struct
 typedef struct
 {
   unsigned int uiStrT:12;		//控制时间
-  unsigned int uiStrDt:10;	//迟滞时间
+  unsigned int uiStrDt:10;  //迟滞时间
   unsigned int uiStrA:9;		//幅值
   unsigned int :1;
   int iStrFi:10;				//相位差
@@ -1133,7 +1140,9 @@ typedef struct
   unsigned int uiXSftArmCtrPowStat:1;	//换挡臂X电机控制器上电状态
   unsigned int :1;
   unsigned int uiYSftArmCtrPowStat:1;	//换挡臂Y电机控制器上电状态
-  unsigned int :3;
+  unsigned int :1;
+  unsigned int uiZSftArmCtrPowStat:1;	//换挡臂Z电机控制器上电状态
+  unsigned int :1;
   unsigned int uiRCUPowStat:1;		//RCU上电状态
   unsigned int :1;
   unsigned int uiAccCtrPowStat:1;		//油门踏板电机控制器上电状态
@@ -1153,6 +1162,7 @@ typedef struct
   unsigned int uiStrCtrPowStat;		//转向电机控制器上电状态
   unsigned int uiXSftArmCtrPowStat;	//换挡臂X电机控制器上电状态
   unsigned int uiYSftArmCtrPowStat;	//换挡臂Y电机控制器上电状态
+  unsigned int uiZSftArmCtrPowStat;  //换挡臂Z电机控制器上电状态
 }T_CTR_POW_STATUS;
 
 #define	POW_STAT_OFF	0	//未上电
@@ -1163,7 +1173,9 @@ typedef struct
 typedef struct
 {
   unsigned int :32;
-  unsigned int :16;
+  unsigned int :8;
+  unsigned int uiZSftMotEnStat:1;		//换挡臂Z电机使能状态
+  unsigned int :7;
   unsigned int uiYSftMotEnStat:1;		//换挡臂Y电机使能状态
   unsigned int :1;
   unsigned int uiClchMotEnStat:1;		//离合电机使能状态
@@ -1192,6 +1204,7 @@ typedef struct
   unsigned int uiClchMotEnStat;		//离合电机使能状态
   unsigned int uiAccClchCtrStat;		//油门踏板离合器通电状态
   unsigned int uiPauseEnStat;			//系统软急停使能状态
+  unsigned int uiZSftMotEnStat;   //换挡臂Z电机使能状态
 }T_ACT_ENABLE_STATUS;
 
 #define	MOT_EN_STAT_DISABLE		0		//电机禁能
@@ -2154,6 +2167,31 @@ typedef struct
   unsigned int uiState;		//电压状态;
 }T_VOLTAGE_STATE;
 
+/*************档位 Z 轴状态反馈*******技术规范修改了定义******/
+typedef struct
+{
+  unsigned int :16;
+  unsigned int uiZSftArmCtrAlive:4;//换挡臂 Z 控制器心跳
+  unsigned int :3;
+  unsigned int uiZSftMotClbr:1;//换挡臂 Z 电机标零状态
+  unsigned int uiZSftMotCur:8; //实际换挡臂 Z电机电流
+  unsigned int :3;
+  int iZSftMotSpd:13;		//实际换挡臂 Z电机转速
+  int iZSftMotTrq:5;		//计算换挡臂 Z 电机转矩
+  unsigned int :2;
+  int iZSftMotAng:9;		//实际换挡臂 Z 电机转角
+}T_STATE_OUT_6F5;
+
+typedef struct
+{
+  int iZSftMotAng;		//实际换挡臂 Z 电机转角
+  int iZSftMotTrq;		//计算换挡臂 Z 电机转矩
+  int iZSftMotSpd;		//实际换挡臂 Z电机转速
+  unsigned int uiZSftMotCur;  //实际换挡臂 Z电机电流
+  unsigned int uiZSftMotClbr;  //换挡臂 Z 电机标零状态
+  unsigned int uiZSftArmCtrAlive;   //换挡臂 Z 控制器心跳
+}T_ZSFT_STATUS;
+
 
 } // namespace canbus
 

src/canbus/messagecoder.cpp

@@ -131,6 +131,24 @@ void MessageCoder::CanMsgAnalyseID_6F4(unsigned char * ucData, T_COMPUTER_STATUS
     tComputerStat->uiCmpStat = tStatIn6F4.uiCmpStat;
 }
 
+//档位 Z 轴状态反馈--数据解析
+void MessageCoder::CanMsgAnalyseID_6F5(unsigned char * ucData, T_ZSFT_STATUS * tZSftStat)
+{
+    T_STATE_OUT_6F5 tStatOut6F5 = { 0 };
+//    unsigned char ucCanMsgSrc[8] = { 0 };
+
+//    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
+    CharReverse(ucData,8);
+    memcpy(&tStatOut6F5, ucData, CAN_MSG_LEN);
+
+    tZSftStat->iZSftMotAng = tStatOut6F5.iZSftMotAng;
+    tZSftStat->iZSftMotSpd = tStatOut6F5.iZSftMotSpd;
+    tZSftStat->iZSftMotTrq = tStatOut6F5.iZSftMotTrq;
+    tZSftStat->uiZSftArmCtrAlive = tStatOut6F5.uiZSftArmCtrAlive;
+    tZSftStat->uiZSftMotClbr = tStatOut6F5.uiZSftMotClbr;
+    tZSftStat->uiZSftMotCur = tStatOut6F5.uiZSftMotCur;
+}
+
 //机器人执行机构实际位置--数据解析
 void MessageCoder::CanMsgAnalyseID_6E0(unsigned char * ucData, T_RBT_ACT_PSNG * tRbtActPsng)
 {
@@ -229,6 +247,7 @@ void MessageCoder::CanMsgAnalyseID_6E4(unsigned char * ucData, T_CTR_POW_STATUS
     tCtrPowStatus->uiStrCtrPowStat = tStatout6E4.uiStrCtrPowStat;
     tCtrPowStatus->uiXSftArmCtrPowStat = tStatout6E4.uiXSftArmCtrPowStat;
     tCtrPowStatus->uiYSftArmCtrPowStat = tStatout6E4.uiYSftArmCtrPowStat;
+    tCtrPowStatus->uiZSftArmCtrPowStat = tStatout6E4.uiZSftArmCtrPowStat;
 }
 
 //执行机构使能状态--数据解析
@@ -249,6 +268,7 @@ void MessageCoder::CanMsgAnalyseID_6E5(unsigned char * ucData, T_ACT_ENABLE_STAT
     tActEnableStatus->uiStrMotEnStat = tStatout6E5.uiStrMotEnStat;
     tActEnableStatus->uiXSftMotEnStat = tStatout6E5.uiXSftMotEnStat;
     tActEnableStatus->uiYSftMotEnStat = tStatout6E5.uiYSftMotEnStat;
+    tActEnableStatus->uiZSftMotEnStat = tStatout6E5.uiZSftMotEnStat;
 }
 
 //油门控制状态反馈--数据解析
@@ -629,6 +649,7 @@ void MessageCoder::CanMsgPackageID_7F2(T_POWER_ON_REQUEST * tPowOnReq, TPCANMsg
     tCtrCmd7F2.uiXSftCtrPowReq = tPowOnReq->uiXSftCtrPowReq;
     tCtrCmd7F2.uiYSftCtrPowReq = tPowOnReq->uiYSftCtrPowReq;
     tCtrCmd7F2.uiDriveFanReq = tPowOnReq->uiDriveFanReq;
+    tCtrCmd7F2.uiZSftArmCtrPowReq = tPowOnReq->uiZSftArmCtrPowReq;
 
     memcpy(ucCanMsgSrc,&tCtrCmd7F2,CAN_MSG_LEN);
         CharReverse(ucCanMsgSrc,8);
@@ -651,6 +672,7 @@ void MessageCoder::CanMsgPackageID_7F5(T_ACTUATOR_ENABLE_REQUEST *tActEnableReq,
     tCtrCmd7F5.uiStrMotEnReq = tActEnableReq->uiStrMotEnReq;
     tCtrCmd7F5.uiXSftMotEnReq = tActEnableReq->uiXSftMotEnReq;
     tCtrCmd7F5.uiYSftMotEnReq = tActEnableReq->uiYSftMotEnReq;
+    tCtrCmd7F5.uiZSftMotEnReq = tActEnableReq->uiZSftMotEnReq;
 
     memcpy(ucCanMsgSrc, &tCtrCmd7F5, CAN_MSG_LEN);
     CharReverse(ucCanMsgSrc,8);
@@ -823,6 +845,7 @@ void MessageCoder::CanMsgPackageID_7F6(T_RBT_MANAGER * tRbtManager, TPCANMsg *tC
     tCtrCmd7F6.uiStrMotInt = tRbtManager->uiStrMotInt;
     tCtrCmd7F6.uiXSftMotInt = tRbtManager->uiXSftMotInt;
     tCtrCmd7F6.uiYSftMotInt = tRbtManager->uiYSftMotInt;
+    tCtrCmd7F6.uiZSftMotInt = tRbtManager->uiZSftMotInt;
 
     memcpy(ucCanMsgSrc, &tCtrCmd7F6, CAN_MSG_LEN);
     CharReverse(ucCanMsgSrc,8);
@@ -1243,6 +1266,10 @@ void * MessageCoder::CanMsgAnalyse(int id,unsigned char * ucData)
         CanMsgAnalyseID_6F4(ucData,&tComputerStat);
         p = &tComputerStat;
         break;
+    case robot::common::ZSftStaus: //档位 Z 轴状态反馈
+      T_ZSFT_STATUS tZSftStatus;
+      CanMsgAnalyseID_6F5(ucData,&tZSftStatus);
+      p = &tZSftStatus;
     case robot::common::RobotAcuatorPos://机器人执行机构实际位置
         T_RBT_ACT_PSNG tRbtActPsng ;
         CanMsgAnalyseID_6E0(ucData, &tRbtActPsng);

src/canbus/messagecoder.h

@@ -36,6 +36,9 @@ private:
     //工控机状态--数据解析
     static void CanMsgAnalyseID_6F4(unsigned char *ucData, T_COMPUTER_STATUS *tComputerStat);
 
+    //档位 Z 轴状态反馈--数据解析
+    static void CanMsgAnalyseID_6F5(unsigned char *ucData, T_ZSFT_STATUS *tZSftStat);
+
     //机器人执行机构实际位置--数据解析
     static void CanMsgAnalyseID_6E0(unsigned char *ucData, T_RBT_ACT_PSNG *tRbtActPsng);
 

src/canbus/srv/ActuatorEnableRequest.srv

@@ -5,5 +5,6 @@ uint32 uiXSftMotEnReq	        #换挡臂X电机使能
 uint32 uiYSftMotEnReq	        #换挡臂Y电机使能
 uint32 uiClchMotEnReq	        #离合电机使能
 uint32 uiAccClchCtrReq	        #油门离合器控制
+uint32 uiZSftMotEnReq           #换挡臂Z电机使能
 ---
 uint32 uiReturn

src/canbus/srv/PowerOnRequest.srv

@@ -6,5 +6,6 @@ uint32 uiStrCtrPowReq	        #转向机构使用请求
 uint32 uiXSftArmCtrPowReq	#换挡臂X电机使用请求
 uint32 uiYSftArmCtrPowReq	#换挡臂Y电机使用请求
 uint32 uiDriveFanReq            #驱动箱风扇使用请求
+uint32 uiZSftArmCtrPowReq       #换挡臂Z电机使用请求
 ---
 uint32 uiReturn

src/canbus/srv/RBTManager.srv

@@ -5,5 +5,6 @@ uint32 uiStrMotInt		#转向电机初始化
 uint32 uiXSftMotInt		#档位X电机初始化
 uint32 uiYSftMotInt		#档位Y电机初始化
 uint32 uiFltClr                 #故障清除
+uint32 uiZSftMotInt             #档位Z电机初始化
 ---
 uint32 uiReturn

src/canbus/srv/SftTxtReq.srv

@@ -0,0 +1,6 @@
+uint32 uiSftTxtFlag
+uint32 uiSftMdReq	    #换挡模式请求
+uint32 uiClchPsngCtrReq       #离合踏板位置控制请求
+uint32 iClchSpdCtrReq       #离合踏板速度控制请求
+---
+uint32 uiReturn

src/common/canmsgobdmsgid.h

@@ -63,6 +63,8 @@ const int VehicleStatus2 = 0x6F2;
 const int VehicleStatus3 = 0x6F3;
 //工控机状态
 const int ComputerStaus = 0x6F4;
+//档位 Z 轴状态反馈
+const int ZSftStaus = 0x6F5;
 //机器人执行机构实际位置
 const int RobotAcuatorPos = 0x6E0;
 //(油门和制动)踏板学习位置反馈

src/common/message.h

@@ -65,6 +65,8 @@ const std::string SfLrnPos2Data = "sfLrnPos2Data";
 const std::string Si1Data = "si1Data";
 
 const std::string SmData = "smData";
+
+const std::string ZSftData = "zsftData";
 // -----------------topics ----------------------------
 
 // -----------------services ----------------------------
@@ -131,6 +133,8 @@ const std::string SysCtrlModeService = "sysctrlmodeservice";
 const std::string RobotWorkMdService = "robotworkmdservice";
 
 const std::string EstpCtrlService = "estpctrlservice";
+
+const std::string SftTxtService = "sfttxtservice";
 // -----------------services ----------------------------
 
 

src/perception/CMakeLists.txt

@@ -61,6 +61,7 @@ add_message_files(
   sfLrnPos2Msg.msg
   si1Msg.msg
   smMsg.msg
+  zsftStatsMsg.msg
   )
 
 ## Generate services in the 'srv' folder

src/perception/msg/aesMsg.msg

@@ -6,3 +6,4 @@ uint32 YSftMotEnStat
 uint32 ClchMotEnStat
 uint32 AccClchCtrStat
 uint32 PauseEnStat
+uint32 ZSftMotEnStat

src/perception/msg/powerMsg.msg

@@ -5,3 +5,4 @@ uint32 ClchCtrPowStat
 uint32 StrCtrPowStat
 uint32 XSftArmCtrPowStat
 uint32 YSftArmCtrPowStat
+uint32 ZSftArmCtrPowStat

src/perception/msg/zsftStatsMsg.msg

@@ -0,0 +1,6 @@
+int32 ZSftMotAng               #实际换挡臂 Z 电机转角
+int32 ZSftMotTrq               #计算换挡臂 Z 电机转矩
+int32 ZSftMotSpd               #实际换挡臂 Z电机转速
+uint32 ZSftMotCur              #实际换挡臂 Z电机电流
+uint32 ZSftMotClbr             #换挡臂 Z 电机标零状态
+uint32 ZSftArmCtrAlive         #换挡臂 Z 控制器心跳

src/perception/perception.cpp

@@ -33,7 +33,7 @@ int Perception::Start()
     {
       // set timer to triger publish info periodly
       timer_ = CreateTimer(ros::Duration(0.5), &Perception::OnTimer, this);
-        ros::Subscriber sub = node_handle_->subscribe(robot::common::CanDataAnalysis, 1, &Perception::CanTopicCallback, this);
+         sub = node_handle_->subscribe(robot::common::CanDataAnalysis, 1, &Perception::CanTopicCallback, this);
 
         bus_pub_6E5 = node_handle_->advertise<::perception::aesMsg>(robot::common::AesData, 1);
         bus_pub_6EE = node_handle_->advertise<::perception::slMsg>(robot::common::SlData, 1);
@@ -62,6 +62,8 @@ int Perception::Start()
 
         bus_pub_6D0 = node_handle_->advertise<::perception::smMsg>(robot::common::SmData,1);
 
+        bus_pub_6F5 = node_handle_->advertise<::perception::zsftStatsMsg>(robot::common::ZSftData,1);
+
         ros::spin();
     }
     catch(ros::Exception e)
@@ -85,6 +87,10 @@ void Perception::CanTopicCallback(const canbus::canMsg::ConstPtr& msg)
     unsigned char ucCanMsgSrc[8] = { 0 };
     HexStrToByte(msg->data.c_str(), ucCanMsgSrc, 16);
     CharReverse(ucCanMsgSrc,8);
+if(msg->id == robot::common::SysLearnBack)
+{
+LOG(INFO)<<Name()<<" Receive 6EE";
+}
     switch (msg->id) {
     case robot::common::ActuatorEnableStatus:
         PublishState_6E5(ucCanMsgSrc);
@@ -146,6 +152,9 @@ void Perception::CanTopicCallback(const canbus::canMsg::ConstPtr& msg)
     case robot::common::SysMonitorBack:
       PublishState_6D0(ucCanMsgSrc);
       break;
+    case robot::common::ZSftStaus:
+      PublishState_6F5(ucCanMsgSrc);
+      break;
     default:
         break;
     }
@@ -167,7 +176,7 @@ bool Perception::PublishState_6E5(unsigned char* msg)
             state_6E5.uiBrkMotEnStat != s_state_6E5.uiBrkMotEnStat ||
             state_6E5.uiClchMotEnStat != s_state_6E5.uiClchMotEnStat || state_6E5.uiPauseEnStat != s_state_6E5.uiPauseEnStat ||
             state_6E5.uiStrMotEnStat != s_state_6E5.uiStrMotEnStat || state_6E5.uiXSftMotEnStat != s_state_6E5.uiXSftMotEnStat ||
-            state_6E5.uiYSftMotEnStat != s_state_6E5.uiYSftMotEnStat)
+            state_6E5.uiYSftMotEnStat != s_state_6E5.uiYSftMotEnStat || state_6E5.uiZSftMotEnStat != s_state_6E5.uiZSftMotEnStat)
     {
 
         memcpy(&s_state_6E5, msg, CAN_MSG_LEN);
@@ -180,12 +189,13 @@ bool Perception::PublishState_6E5(unsigned char* msg)
         aes_msg.StrMotEnStat = state_6E5.uiStrMotEnStat;
         aes_msg.XSftMotEnStat = state_6E5.uiXSftMotEnStat;
         aes_msg.YSftMotEnStat = state_6E5.uiYSftMotEnStat;
+        aes_msg.ZSftMotEnStat = state_6E5.uiZSftMotEnStat;
 
         return true;
     }
     else
     {
-        LOG(INFO) << Name() << " PublishState_6E5 data is repeat";
+ //       LOG(INFO) << Name() << " PublishState_6E5 data is repeat";
         return false;
     }
 }
@@ -198,7 +208,7 @@ bool Perception::PublishState_6E4(unsigned char* msg)
     if(state_6E4.uiAccCtrPowStat != s_state_6E4.uiAccCtrPowStat || state_6E4.uiBrkCtrPowStat != s_state_6E4.uiBrkCtrPowStat ||
             state_6E4.uiClchCtrPowStat != s_state_6E4.uiClchCtrPowStat || state_6E4.uiRCUPowStat != s_state_6E4.uiRCUPowStat ||
             state_6E4.uiStrCtrPowStat != s_state_6E4.uiStrCtrPowStat || state_6E4.uiXSftArmCtrPowStat != s_state_6E4.uiXSftArmCtrPowStat||
-            state_6E4.uiYSftArmCtrPowStat != s_state_6E4.uiYSftArmCtrPowStat)
+            state_6E4.uiYSftArmCtrPowStat != s_state_6E4.uiYSftArmCtrPowStat || state_6E4.uiZSftArmCtrPowStat != s_state_6E4.uiZSftArmCtrPowStat)
     {
 
         memcpy(&s_state_6E4, msg, CAN_MSG_LEN);
@@ -209,12 +219,13 @@ bool Perception::PublishState_6E4(unsigned char* msg)
         power_msg.XSftArmCtrPowStat = state_6E4.uiXSftArmCtrPowStat;
         power_msg.YSftArmCtrPowStat = state_6E4.uiYSftArmCtrPowStat;
         power_msg.ClchCtrPowStat = state_6E4.uiClchCtrPowStat;
+        power_msg.ZSftArmCtrPowStat = state_6E4.uiZSftArmCtrPowStat;
 
         return true;
     }
     else
     {
-        LOG(INFO) << Name() << " PublishState_6E4 data is repeat";
+ //       LOG(INFO) << Name() << " PublishState_6E4 data is repeat";
         return false;
     }
 }
@@ -280,7 +291,7 @@ bool Perception::PublishState_6EE(unsigned char* msg)
     }
     else
     {
-        LOG(INFO) << Name() << " PublishState_6EE data is repeat";
+   //     LOG(INFO) << Name() << " PublishState_6EE data is repeat";
         return false;
     }
 }
@@ -330,7 +341,7 @@ bool Perception::PublishState_6ED(unsigned char* msg)
     }
     else
     {
-        LOG(INFO) << Name() << " PublishState_6ED data is repeat";
+  //      LOG(INFO) << Name() << " PublishState_6ED data is repeat";
         return false;
     }
 }
@@ -386,7 +397,7 @@ bool Perception::PublishState_6D1(unsigned char* msg)
     }
     else
     {
-        LOG(INFO) << Name() << " PublishState_6D1 data is repeat";
+   //     LOG(INFO) << Name() << " PublishState_6D1 data is repeat";
         return false;
     }
 }
@@ -661,7 +672,7 @@ bool Perception::PublishState_6D2_6(unsigned char* msg, int type, const ros::Pub
     }
     else
     {
-        LOG(INFO) << Name() << " PublishState_6D2_6 " << type << " data is repeat";
+    //    LOG(INFO) << Name() << " PublishState_6D2_6 " << type << " data is repeat";
         return false;
     }
 }
@@ -696,7 +707,7 @@ bool Perception::PublishState_2E0(unsigned char* msg)
     }
     else
     {
-        LOG(INFO) << Name() << " PublishState_2E0 data is repeat";
+    //    LOG(INFO) << Name() << " PublishState_2E0 data is repeat";
         return false;
     }
 }
@@ -730,7 +741,7 @@ bool Perception::PublishState_2E1(unsigned char* msg)
     }
     else
     {
-        LOG(INFO) << Name() << " PublishState_2E1 data is repeat";
+    //    LOG(INFO) << Name() << " PublishState_2E1 data is repeat";
         return false;
     }
 }
@@ -776,7 +787,7 @@ bool Perception::PublishState_6E1(unsigned char* msg)
   }
   else
   {
-      LOG(INFO) << Name() << " PublishState_6E1 data is repeat";
+   //   LOG(INFO) << Name() << " PublishState_6E1 data is repeat";
       return false;
   }
 }
@@ -809,7 +820,7 @@ bool Perception::PublishState_6E2(unsigned char* msg)
   }
   else
   {
-      LOG(INFO) << Name() << " PublishState_6E2 data is repeat";
+  //    LOG(INFO) << Name() << " PublishState_6E2 data is repeat";
       return false;
   }
 }
@@ -855,7 +866,7 @@ bool Perception::PublishState_6E3(unsigned char *msg)
   }
   else
   {
-      LOG(INFO) << Name() << " PublishState_6E3 data is repeat";
+  //    LOG(INFO) << Name() << " PublishState_6E3 data is repeat";
       return false;
   }
 }
@@ -900,7 +911,7 @@ bool Perception::PublishState_6EF(unsigned char *msg)
   }
   else
   {
-      LOG(INFO) << Name() << " PublishState_6EF data is repeat";
+   //   LOG(INFO) << Name() << " PublishState_6EF data is repeat";
       return false;
   }
 }
@@ -931,7 +942,7 @@ bool Perception::PublishState_6EB(unsigned char *msg)
   }
   else
   {
-      LOG(INFO) << Name() << " PublishState_6EB data is repeat";
+  //    LOG(INFO) << Name() << " PublishState_6EB data is repeat";
       return false;
   }
 
@@ -971,11 +982,47 @@ bool Perception::PublishState_6D0(unsigned char *msg)
   }
   else
   {
-      LOG(INFO) << Name() << " PublishState_6E1 data is repeat";
+   //   LOG(INFO) << Name() << " PublishState_6D0 data is repeat";
       return false;
   }
 }
 
+bool Perception::PublishState_6F5(unsigned char* msg)
+{
+//    int copySize;
+//    if(msg.size() < sizeof(s_state_6F5))
+//        copySize = msg->data.size();
+//    else
+//        copySize = sizeof(s_state_6F5);
+
+    robot::canbusSpace::T_STATE_OUT_6F5 state_6F5;
+    memset(&state_6F5, 0 ,sizeof(state_6F5));
+    memcpy(&state_6F5, msg, CAN_MSG_LEN);
+
+    if(state_6F5.iZSftMotAng != s_state_6F5.iZSftMotAng || state_6F5.iZSftMotSpd != s_state_6F5.iZSftMotSpd ||
+            state_6F5.iZSftMotTrq != s_state_6F5.iZSftMotTrq ||
+            state_6F5.uiZSftArmCtrAlive != s_state_6F5.uiZSftArmCtrAlive || state_6F5.uiZSftMotClbr != s_state_6F5.uiZSftMotClbr ||
+            state_6F5.uiZSftMotCur != s_state_6F5.uiZSftMotCur)
+    {
+
+        memcpy(&s_state_6F5, msg, CAN_MSG_LEN);
+
+        zsft_msg.ZSftArmCtrAlive = state_6F5.uiZSftArmCtrAlive;
+        zsft_msg.ZSftMotAng =  state_6F5.iZSftMotAng;
+        zsft_msg.ZSftMotClbr = state_6F5.uiZSftMotClbr;
+        zsft_msg.ZSftMotCur = state_6F5.uiZSftMotCur;
+        zsft_msg.ZSftMotSpd = state_6F5.iZSftMotSpd;
+        zsft_msg.ZSftMotTrq = state_6F5.iZSftMotTrq;
+
+        return true;
+    }
+    else
+    {
+ //       LOG(INFO) << Name() << " PublishState_6F5 data is repeat";
+        return false;
+    }
+}
+
 void Perception::OnTimer(const ros::TimerEvent &)
 {
   try
@@ -1143,6 +1190,15 @@ void Perception::OnTimer(const ros::TimerEvent &)
         LOG(ERROR) << Name() << " PublishState_6D0 publisher is unvalid";
     }
 
+    if(bus_pub_6F5)
+    {
+       bus_pub_6F5.publish(zsft_msg);
+    }
+    else
+    {
+        LOG(ERROR) << Name() << " PublishState_6F5 publisher is unvalid";
+    }
+
   }
   catch (ros::Exception ex)
   {

src/perception/perception.h

@@ -28,6 +28,7 @@
 #include "perception/smMsg.h"
 #include "canbus/EstpCtrl.h"
 #include "canbus/SysCtrlMode.h"
+#include "perception/zsftStatsMsg.h"
 
 namespace robot {
 namespace perception {
@@ -147,6 +148,8 @@ private:
 
     bool PublishState_6D0(unsigned char* msg);
 
+    bool PublishState_6F5(unsigned char* msg);
+
     bool SoftStopControl();
 
     void OnTimer(const ros::TimerEvent &event);
@@ -189,6 +192,7 @@ private:
     }
 
 private:
+ros::Subscriber sub;
      robot::canbusSpace::T_STATE_OUT_6E5 s_state_6E5;
      ::perception::aesMsg aes_msg;
 
@@ -243,6 +247,9 @@ private:
      robot::canbusSpace::T_STATE_OUT_6D0 s_state_6D0;
      ::perception::smMsg sm_msg;
 
+     robot::canbusSpace::T_STATE_OUT_6F5 s_state_6F5;
+     ::perception::zsftStatsMsg zsft_msg;
+
 
      ros::Publisher bus_pub_6E5;
      ros::Publisher bus_pub_6E4;
@@ -251,7 +258,7 @@ private:
 
      ros::Publisher bus_pub_6D1;
 
-     ros::Publisher bus_pub_6D2;  
+     ros::Publisher bus_pub_6D2;
      ros::Publisher bus_pub_6D3;
      ros::Publisher bus_pub_6D4;
      ros::Publisher bus_pub_6D5;
@@ -267,10 +274,12 @@ private:
      ros::Publisher bus_pub_6EB;
 
      ros::Publisher bus_pub_6D0;
+     ros::Publisher bus_pub_6F5;
 
      ros::ServiceClient clientActuatorEnableRequest;
 
      ros::Timer timer_;
+
 };