2024年9月11日: SCOUT-zw

zw_pro/EgoSystem/.vscode/settings.json

@@ -1,5 +1,7 @@
 {
     "files.associations": {
-        "ostream": "cpp"
+        "ostream": "cpp",
+        "atomic": "cpp",
+        "cstdint": "cpp"
     }
 }

zw_pro/EgoSystem/can_bus.cpp

@@ -1,7 +1,7 @@
 #include "can_bus.h"
 #include <thread>
 #include "message_queue.h"
-
+#include <iomanip>
 
 
 #define	GET_BIT(x, bit)	((x & (1 << bit)) >> bit)
@@ -235,7 +235,7 @@ bool CCanBusSensor::car_stop(cannet_frame* frame){
         switch (carframe.data[2])
         {
             case 0x00:
-                *id_111 = *car_stop;
+                std::copy(id_init,id_init+8,id_111);
                 stop_flag = true;
                 // std::cout<<"car stop!"<<std::endl;
             break;
@@ -278,96 +278,138 @@ void CCanBusSensor::toggleLamp(cannet_frame* frame){
 int CCanBusSensor::gearselection(cannet_frame* frame){
     
     cannet_frame& carframe = frame[0];
+    
     switch (carframe.data[5])
     {
         case 0x10:
             return 1;
-        break;
 
         case 0x00:
             return 2;
-        break;
 
         case 0x40:
             return 3;
-        break;
     }
 }
 
-void CCanBusSensor::gear_act(int gear){
 
-    switch (gear)
-    {
-        case 1:
-            id_633[0] = 
-    }
+void CCanBusSensor::runSpeed(unsigned char input, unsigned char &byte0, unsigned char &byte1) {
+    // 将输入值从 0x00 - 0xFF 映射到 0x0000 - 0x05DC (0 - 1500)
+    unsigned int mapped_value = static_cast<unsigned int>(input-2) * 1500 / 0x7F;
+    cout<<"run speed: "<<mapped_value<<endl;
+    // 提取高八位和低八位
+    byte0 = static_cast<unsigned char>((mapped_value >> 8) & 0xFF); // 高八位
+    byte1 = static_cast<unsigned char>(mapped_value & 0xFF);        // 低八位
+}
+
+void CCanBusSensor::backSpeed(unsigned char input, unsigned char &byte0, unsigned char &byte1) {
+    // 将输入值从 0x00 - 0xFF 映射到 -1500 到 0 的范围
+    int mapped_value = static_cast<int>(input-2) * -1500 / 0x7F;
+    cout<<"back speed: "<<mapped_value<<endl;
 
+    // 将负数调整为 0xFA24 ( -1500) 到 0x0000 的正数范围
+    unsigned short adjusted_value = static_cast<unsigned short>(mapped_value + 0x10000);
+
+    // 提取高八位和低八位
+    byte0 = static_cast<unsigned char>((adjusted_value >> 8) & 0xFF); // 高八位
+    byte1 = static_cast<unsigned char>(adjusted_value & 0xFF);        // 低八位
 }
 
+
 void CCanBusSensor::move_run(cannet_frame* frame){
     
-        cannet_frame& carframe = frame[0];     
+    cannet_frame& carframe = frame[0];     
         
-        switch (gearselection)
+    if(!(carframe.data[5]>0x02 && carframe.data[5]<=0x7F))
+    {
         if(carframe.data[4]>0x02 && carframe.data[4]<=0x7F)
         {
-           
-            // std::copy(move_forward,move_forward+8,Sendframe[6].data); //油门前进
-            // memcpy(Sendframe[3].data,move_forward,carframe.dlc);
-            id_111[0] = 0x00;
-            id_111[1] = 0x96;
-
-            run_flag = true;
+            CCanBusSensor::runSpeed(carframe.data[4],id_111[0],id_111[1]);
             std::cout<<"油门前进"<<std::endl;
         }
-        else  if(carframe.data[5]>0x02 && carframe.data[5]<=0x7F)
+        else
         {
-            // std::copy(move_backward,move_backward+8,Sendframe[2].data); //刹车后退
-            // std::copy(stop_on,stop_on+8,Sendframe[2].data); //刹车停止
-            // memcpy(Sendframe[3].data,stop_on,carframe.dlc);
             id_111[0] = 0x00;
             id_111[1] = 0x00;
-
-            run_flag = true;
-            std::cout<<"刹车后退"<<std::endl;
+            std::cout<<"前进油门松开"<<std::endl;
         }
-        // else  memcpy(Sendframe[2].data,stop_on,carframe.dlc);
-        else  { id_111[0] = 0x00;   id_111[1] = 0x00; }
+    }
+    else  
+    {   
+        id_111[0] = 0x00;   
+        id_111[1] = 0x00; 
+        id_111[2] = 0x00;
+        id_111[3] = 0x00;
+        std::cout<<"前进档：刹车"<<std::endl;
+    }
 
 }
 
-void CCanBusSensor::move_rot(cannet_frame* frame){
 
-        cannet_frame& carframe = frame[0];     
+void CCanBusSensor::move_back(cannet_frame* frame){
 
-        switch(carframe.data[0])
+    cannet_frame& carframe = frame[0];
+
+    if(!(carframe.data[5]>0x02 && carframe.data[5]<=0x7F))
+    {
+        if(carframe.data[4]>0x02 && carframe.data[4]<=0x7F)
         {
-            case 0x01:
-                
-                id_111[2] = 0x00;
-                id_111[3] = 0x00;
-               
-                // cout<<"No rotation"<<endl;
-            break;
+            CCanBusSensor::backSpeed(carframe.data[4],id_111[0],id_111[1]);
+            std::cout<<"油门后退"<<std::endl;
+        }
+        else
+        {
+            id_111[0] = 0x00;
+            id_111[1] = 0x00;
+            
+            std::cout<<"后退油门松开"<<std::endl;
+        }
+    }
+    else  
+    {   
+        id_111[0] = 0x00;   
+        id_111[1] = 0x00; 
+        id_111[2] = 0x00;
+        id_111[3] = 0x00;
+        std::cout<<"后退档：刹车"<<std::endl;
+    }
+}
 
-            case 0x04:
 
-                id_111[2] = 0x00;
-                id_111[3] = 0xc8;
+void CCanBusSensor::move_rot(cannet_frame* frame){
 
-                rot_flag = true;
-                // cout<<"turn left"<<endl;
-            break;
+        cannet_frame& carframe = frame[0];
 
-            case 0x10:
+        if(!stop_flag)
+        {
+            switch(carframe.data[0])
+            {
+                case 0x01:
+                    
+                    id_111[2] = 0x00;
+                    id_111[3] = 0x00;
+                    // cout<<"No rotation"<<endl;
+                break;
 
-                id_111[2] = 0xFF;
-                id_111[3] = 0x38;
+                case 0x04:
 
-                rot_flag = true;
-                // cout<<"turn right"<<endl;
-            break;
+                    id_111[2] = 0x00;
+                    id_111[3] = 0xc8;
+                    // cout<<"turn left"<<endl;
+                break;
+
+                case 0x10:
 
+                    id_111[2] = 0xFF;
+                    id_111[3] = 0x38;
+                    // cout<<"turn right"<<endl;
+                break;
+            }
+        }
+        else 
+        {
+            id_111[2] = 0x00;  
+            id_111[3] = 0x00;
         }
 
 }
@@ -381,17 +423,17 @@ void CCanBusSensor::move_rot(cannet_frame* frame){
 void CCanBusSensor::OnMessage(cannet_frame* frames, int32_t count)  //下发控制指令
 {
 
-    for (int32_t i = count; i > 0; i--) //count=4
+    for (int32_t i = 3; i >= 0; i--) //count=4
     {
         cannet_frame& frame = frames[i];
 
         int32_t canid = frame.canid;
-        
+        Sendframe[3].can_dlc = frame.dlc;
         //std::cout << std::hex << canid << std::endl;
         CCanBusSensor::order(&frame);
         switch (canid)
         {
-            case 0x181:
+            case 0x181:       // receive from frame[3]
 
                 // Sendframe[1].can_id = 0x121;
                 // Sendframe[1].can_dlc = frame.dlc;
@@ -406,33 +448,60 @@ void CCanBusSensor::OnMessage(cannet_frame* frames, int32_t count)  //下发控
                 if(!CCanBusSensor::car_stop(&frame))
                 {
                     // cout<<"car ends stopping!"<<endl;    
-                    CCanBusSensor::move_run(&frame);     //move_run和car
+                    switch (gear)
+                    {
+                        case 1:
+                            CCanBusSensor::move_run(&frame);
+                            cout<<"case 1:前进档"<<endl;
+                        break;
+
+                        case 2:
+                            id_111[0] = 0x00;
+                            id_111[1] = 0x00;
+                            cout<<"case 2:空档"<<endl;
+                        break;
+
+                        case 3:
+                            CCanBusSensor::move_back(&frame);
+                            cout<<"case 3:后退档"<<endl;
+                        break;
+
+                        default:
+                            cout<<"gear error"<<endl;
+                        break;
+                    }
                 }
                 
             break;
 
             case 0x0CFDD633:  // receive from frame[3]
             
-                if(!CCanBusSensor::car_stop(&frame))
+                if(!(CCanBusSensor::car_stop(&frame)))
                 {
                     // cout<<"car ends stopping!"<<endl;  
-                    int gear = CCanBusSensor::gearselection(&frame);
-                    CCanBusSensor::gear_act(gear);
+                    gear = CCanBusSensor::gearselection(&frame);
+                    cout<<"档位："<<gear<<endl;
                     CCanBusSensor::move_rot(&frame);
                 }
                 
 
             break;
 
-
             default:
             break;
         }
     } 
     Sendframe[3].can_id = 0x111;
-    Sendframe[3].can_dlc = frame.dlc;
-    memcpy(Sendframe[3].data,id_111,frame.dlc);
-    cout<<"id_111: "<<static_cast<int>(id_111[0])<<static_cast<int>(id_111[1])<<static_cast<int>(id_111[2])<<static_cast<int>(id_111[3])<<endl;
+    // Sendframe[3].can_dlc = frame.dlc;
+    // memcpy(Sendframe[3].data,id_111,sizeof(id_111));
+    std::copy(id_111,id_111+8,Sendframe[3].data);
+    
+    std::cout << "Array contents: ";
+    for (unsigned char value : id_111) {
+        std::cout << "0x" << std::hex << std::setw(2) << std::setfill('0') << static_cast<int>(value) << " ";
+    }
+    std::cout << std::endl;
+    // cout<<"id_111: "<<static_cast<int>(id_111[0])<<static_cast<int>(id_111[1])<<static_cast<int>(id_111[2])<<static_cast<int>(id_111[3])<<endl;
 }
 
 void CCanBusSensor::SendStatusToMSG()

zw_pro/EgoSystem/can_bus.h

@@ -28,20 +28,22 @@ public:
      void order(cannet_frame* frame);  // 指令模式
      void toggleLamp(cannet_frame* frame);  // 车灯开关  
      void move_run(cannet_frame* frame); // 车辆前进  
+     void move_back(cannet_frame* frame); // 车辆后退  
      void move_rot(cannet_frame* frame);  // 车辆右转  
      bool car_stop(cannet_frame* frame);
      int gearselection(cannet_frame* frame); // 1:前进档 2:空挡 3:后退档
-     void gear_act(int gear);
+     void runSpeed(unsigned char input, unsigned char &byte0, unsigned char &byte1); //座舱速度与小车速度线性对应
+     void backSpeed(unsigned char input, unsigned char &byte0, unsigned char &byte1);
 
      bool order_flag ; // 指令模式
      bool lamp_flag ; // 车灯状态 
      bool run_flag ; // 车辆前进后退
-     bool rot_flag = false; // 车辆转弯
+     bool rot_flag ; // 车辆转弯
      bool stop_flag ; // 急停状态
      int gear; //档位
 
+     unsigned char id_init[8] = {0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00};
      unsigned char id_111[8] = {0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00};
-     unsigned char id_633[8] = {0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00};
 /*------------------小车控制参数和函数止------------------*/
 
 private:

zw_pro/EgoSystem/readme.txt

@@ -7,6 +7,7 @@
 舱端刹车：      181 00 00 FF 00 02 7F 67 18            车体行进速度，单位mm/s,值域【-1500，1500】     用十六进制表示：-1500：0xFA24    1500:0x05DC
 
 油门踏板：0-127 刹车踏板：0-127  
+舱端急停：  181  00 00 00 FF 02 02 67 18               车端急停：111  00      00      00      00      00      00      00      00
 
 舱端左手柄                                             车端旋转：111   00      00      00      c8      00      00      00      00 （小车以0.2rad/s旋转）
 初始状态： 0CFDD633  01 00 01 00 FF 00 00 FF          车体旋转角速度，单位0.001rad/s,值域【-523，523】    用十六进制表示：-523：0xFDF5   523:0x020B
@@ -16,7 +17,6 @@
 空档：     0CFDD633  01 00 01 00 FF 00 00 FF  
 后退档：   0CFDD633  01 00 01 00 FF 40 00 FF                                           
 
-舱端急停：  181  00 00 00 FF 02 02 67 18               车端急停：111  00      00      00      00      00      00      00      00
 
 舱端右手柄
 舱端操纵杆初始状态：0CFDD634  01 00 01 00 FF 00 00 FF