2024年9月5日: SCOUT-zw

zw_pro/.vscode/settings.json

@@ -1,5 +1,78 @@
 {
     "files.associations": {
-        "ostream": "cpp"
-    }
+        "ostream": "cpp",
+        "cctype": "cpp",
+        "clocale": "cpp",
+        "cmath": "cpp",
+        "csignal": "cpp",
+        "cstdarg": "cpp",
+        "cstddef": "cpp",
+        "cstdio": "cpp",
+        "cstdlib": "cpp",
+        "cstring": "cpp",
+        "ctime": "cpp",
+        "cwchar": "cpp",
+        "cwctype": "cpp",
+        "any": "cpp",
+        "array": "cpp",
+        "atomic": "cpp",
+        "hash_map": "cpp",
+        "hash_set": "cpp",
+        "strstream": "cpp",
+        "bit": "cpp",
+        "*.tcc": "cpp",
+        "bitset": "cpp",
+        "chrono": "cpp",
+        "codecvt": "cpp",
+        "complex": "cpp",
+        "condition_variable": "cpp",
+        "cstdint": "cpp",
+        "deque": "cpp",
+        "forward_list": "cpp",
+        "list": "cpp",
+        "map": "cpp",
+        "set": "cpp",
+        "unordered_map": "cpp",
+        "unordered_set": "cpp",
+        "vector": "cpp",
+        "exception": "cpp",
+        "algorithm": "cpp",
+        "functional": "cpp",
+        "iterator": "cpp",
+        "memory": "cpp",
+        "memory_resource": "cpp",
+        "numeric": "cpp",
+        "optional": "cpp",
+        "random": "cpp",
+        "ratio": "cpp",
+        "source_location": "cpp",
+        "string": "cpp",
+        "string_view": "cpp",
+        "system_error": "cpp",
+        "tuple": "cpp",
+        "type_traits": "cpp",
+        "utility": "cpp",
+        "rope": "cpp",
+        "fstream": "cpp",
+        "future": "cpp",
+        "initializer_list": "cpp",
+        "iomanip": "cpp",
+        "iosfwd": "cpp",
+        "iostream": "cpp",
+        "istream": "cpp",
+        "limits": "cpp",
+        "mutex": "cpp",
+        "new": "cpp",
+        "sstream": "cpp",
+        "stdexcept": "cpp",
+        "streambuf": "cpp",
+        "thread": "cpp",
+        "cfenv": "cpp",
+        "cinttypes": "cpp",
+        "typeindex": "cpp",
+        "typeinfo": "cpp",
+        "valarray": "cpp",
+        "variant": "cpp"
+    },
+    "C_Cpp.errorSquiggles": "disabled"
 }

zw_pro/EgoSystem/can_bus.cpp

@@ -1,5 +1,4 @@
 #include "can_bus.h"
-#include "vehicle.h"
 #include <thread>
 #include "message_queue.h"
 #include <math.h>
@@ -216,7 +215,104 @@ void CCanBusSensor::Notify(struct can_frame *date) //上传状态信息
     }
 }
 
-/*-----------------小车控制------------------*/
+
+/*------------------小车控制起-------------------*/
+
+void CCanBusSensor::order(cannet_frame* frames){
+    
+        cannet_frame& frame = frames[0];          
+        Sendframe[0].can_id = 0x421;
+        Sendframe[0].can_dlc = frame.dlc;
+
+        memcpy(Sendframe[0].data,order_on,frame.dlc);
+        order_flag = true; 
+
+        if(!order_flag){
+        cout<<"order_flag: false"<<endl;
+        }
+}
+
+
+void CCanBusSensor::toggleLamp(cannet_frame* frames){
+
+        cannet_frame& frame = frames[0];           
+        Sendframe[0].can_id = 0x121;
+        Sendframe[0].can_dlc = frame.dlc;
+
+        switch (frame.data[0])
+                {
+                    case 0x00:
+                        memcpy(Sendframe[0].data,headlamp_off,frame.dlc); //前后车灯关
+                        std::cout<<"前车灯关,后车灯关 "<<std::endl;
+                    break;
+                
+                    case 0x10:
+                        memcpy(Sendframe[0].data,headlamp_on,frame.dlc); //前车灯开
+                        std::cout<<"前车灯开,后车灯关 "<<std::endl;
+                    break;
+                    
+                    case 0x08:
+                        memcpy(Sendframe[0].data,taillamp_on,frame.dlc); //后车灯开
+                        std::cout<<"前车灯关,后车灯开 "<<std::endl;
+                    break;
+                }
+}
+
+void CCanBusSensor::car_stop(cannet_frame* frames){
+
+        cannet_frame& frame = frames[0];           
+        Sendframe[0].can_id = 0x111;
+        Sendframe[0].can_dlc = frame.dlc;
+
+        if(frame.data[2]==0x00){
+            memcpy(Sendframe[0].data,stop,frame.dlc); //急停
+            
+            stop_flag = true; 
+            if(stop_flag){
+            std::cout<<"stop_flag: true"<<std::endl;
+            }
+        }
+}
+
+
+void CCanBusSensor::move_run(cannet_frame* frames){
+    
+        cannet_frame& frame = frames[0];
+        int32_t canid = frame.canid;
+
+        Sendframe[0].can_id = 0x111;
+        Sendframe[0].can_dlc = frame.dlc;
+
+        if(frame.data[4]>0x02 && frame.data[4]<=0x7F){
+            Sendframe[0].data = move_forward; //油门前进
+            run_flag = true;
+        }
+        else if(frame.data[5]>0x02 && frame.data[5]<=0x7F){
+            memcpy(Sendframe[0].data,stop,frame.dlc); //刹车后退
+            run_flag = true;
+        }
+
+}
+
+void CCanBusSensor::move_rot(cannet_frame* frames){
+
+        cannet_frame& frame = frames[0];
+        int32_t canid = frame.canid;
+
+        Sendframe[1].can_id = 0x111;
+        Sendframe[1].can_dlc = frame.dlc;
+
+        if(frame.data[0] != 0x01){
+            memcpy(Sendframe[0].data,move_rotation,frame.dlc); //转弯
+            rot_flag = true;
+        }
+
+}
+
+
+
+/*------------------小车控制止-------------------*/
+
 
 void CCanBusSensor::OnMessage(cannet_frame* frames, int32_t count)  //下发控制指令
 {
@@ -232,26 +328,80 @@ void CCanBusSensor::OnMessage(cannet_frame* frames, int32_t count)  //下发控
         switch (canid)
         {
             case 0x181:
-                Sendframe[0].can_id = 0x121;
+                // Sendframe[0].can_id = 0x421;
                 Sendframe[0].can_dlc = frame.dlc;
 
-                // Hand_Throttle = make_int16(frame.data[1],frame.data[0]); //手油门
-                // Foot_Throttle = make_int16(frame.data[3],frame.data[2]); //脚油门
-                // Brake = make_int16(frame.data[5],frame.data[4]);         //刹车
-
                 // memcpy(Sendframe[0].data, frame.data, frame.dlc);
+                // Hand_Throttle = make_int16(frame.data[1],frame.data[0]);
+                // Foot_Throttle = make_int16(frame.data[3],frame.data[2]);
+                // Brake = make_int16(frame.data[5],frame.data[4]);
+                // cout<<"receive 181: "<<Hand_Throttle<<" "<<Foot_Throttle<<" "<<Brake<<endl;
+
+                // 指令模式
+                // memcpy(Sendframe[0].data,order_on,frame.dlc); 
+                
+                CCanBusSensor::order(&frame);
+                CCanBusSensor::toggleLamp(&frame);
+                // CCanBusSensor::move_run(&frame);     //move_run和car
+                // CCanBusSensor::car_stop(&frame);
+
+                // 车灯
+                // Sendframe[0].can_id = 0x121;
+                // memcpy(Sendframe[0].data,headlamp_on,frame.dlc);
+                // switch (frame.data[0])
+                // {
+                //     case 0x00:
+                //         memcpy(Sendframe[0].data,headlamp_off,frame.dlc); //前后车灯关
+                //         std::cout<<"前车灯关,后车灯关 "<<std::endl;
+                //     break;
+                
+                //     case 0x10:
+                //         memcpy(Sendframe[0].data,headlamp_on,frame.dlc); //前车灯开
+                //         std::cout<<"前车灯开,后车灯关 "<<std::endl;
+                //     break;
+                    
+                //     case 0x08:
+                //         memcpy(Sendframe[0].data,taillamp_on,frame.dlc); //后车灯开
+                //         std::cout<<"前车灯关,后车灯开 "<<std::endl;
+                //     break;
+                // }
+                 
+
+                // 前进后退
+                // Sendframe[0].can_id = 0x111;
+                // if(frame.data[4]>0x02 && frame.data[4]<=0x7F){
+                //     memcpy(Sendframe[0].data,move_forward,frame.dlc); //油门前进
+                //     run_flag = true;
+                // }
+                // else if(frame.data[5]>0x02 && frame.data[5]<=0x7F){
+                //     memcpy(Sendframe[0].data,stop,frame.dlc); //刹车后退
+                //     run_flag = true;
+                // }
+
+                // // 急停        
+                // Sendframe[0].can_id = 0x111;
+                // if(frame.data[2]==0x00 && frame.data[3]==0xFF){
+                //     memcpy(Sendframe[0].data,stop,frame.dlc); //急停
+                //     stop_flag = true; 
+                //     if(stop_flag){
+                //         cout<<"stop_flag: true"<<endl;
+                //     }
+                // }
+
 
-        memcpy(Sendframe[0].data,order_on,sizeof(order_on)); 
-        memcpy(Sendframe[0].data,headlamp_on,sizeof(headlamp_on)); 
 
-                if(frame.data[0]==0x10)
-                  memcpy(Sendframe[0].data,headlamp_on,sizeof(headlamp_on)); 
-                else 
-                  memcpy(Sendframe[0].data,headlamp_off,sizeof(headlamp_off));
 
-                     
             break;
 
+            // case 0x0CFDD634:
+
+            //     Sendframe[1].can_dlc = frame.dlc;
+
+            //     CCanBusSensor::order(&frame);
+            //     CCanBusSensor::move_rot(&frame);
+
+            // break;
+
             case 0x182:
                 Sendframe[1].can_id = 0x182;
                 Sendframe[1].can_dlc = frame.dlc;

zw_pro/EgoSystem/can_bus.h

@@ -4,6 +4,10 @@
 #include "../common/iobuffer.h"
 #include "../common/sensor_socket.h"
 
+using namespace std;
+
+
+
 class CMessageQueue;
 class CCanBusSensor
 {
@@ -18,6 +22,13 @@ public:
      void OnMessage(cannet_frame* frames, int32_t count = 4);
      void Emergency();
      
+
+     void order(cannet_frame* frames);  // 指令模式
+     void toggleLamp(cannet_frame* frames);  // 车灯开关  
+     void move_run(cannet_frame* frames); // 车辆前进  
+     void move_rot(cannet_frame* frames);  // 车辆右转  
+     void car_stop(cannet_frame* frames);
+
 private:
      void Run();
      void CanSendProc();
@@ -42,4 +53,24 @@ private:
      int64_t count;
      FeedData _data;
      int model;//0本地,1远程
-};
+
+     /*-------------------小车控制起--------------------*/
+     bool order_flag = false; // 指令模式
+     bool lamp_flag = false; // 车灯状态 
+     bool run_flag = false; // 车辆前进后退
+     bool rot_flag = false; // 车辆转弯
+     bool stop_flag = false; // 急停状态
+
+     unsigned char order_on[8] = {0x01,0x00,0x00,0x00,0x00,0x00,0x00,0x00};        //id:421 指令模式开
+     unsigned char headlamp_on[8] = {0x01,0x01,0x50,0x00,0x00,0x00,0x00,0x00};        //id:121 前车灯常开
+     unsigned char headlamp_off[8] = {0x01,0x00,0x00,0x00,0x00,0x00,0x00,0x00};       //id:121 前车灯常关
+     unsigned char taillamp_on[8] = {0x01,0x00,0x00,0x01,0x50,0x00,0x00,0x00};       //id:121 后车灯常开
+     unsigned char taillamp_off[8] = {0x01,0x00,0x00,0x00,0x00,0x00,0x00,0x00};       //id:121 后车灯常关
+     unsigned char move_forward[8] = {0x00,0x96,0x00,0x00,0x00,0x00,0x00,0x00};    //id:111 以0.15m/s前进
+     unsigned char move_backward[8] = {0x00,0xcc,0x00,0x00,0x00,0x00,0x00,0x00};   //id:111 以0.15m/s后退
+     unsigned char move_rotation[8] = {0x00,0x00,0x00,0xc8,0x00,0x00,0x00,0x00};   //id:111 以0.2rad/s旋转
+     unsigned char stop[8] = {0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00};            //id:111 急停
+     /*-------------------小车控制止--------------------*/
+     
+};
+

zw_pro/EgoSystem/readme.txt

@@ -1,6 +1,6 @@
 舱端初始状态：  181  00 00 FF 00 02 02 67 18                   
 舱端前车灯开：  181  10 00 FF 00 02 02 67 18                  车端前车灯(常开)： 121 01 01 50 00 00 00 00 00
-舱端后车灯开：  181  00 00 FF 00 02 02 67 18                  车端后车灯(常开)： 121 01 00 00 01 50 00 00 00
+舱端后车灯开：  181  08 00 FF 00 02 02 67 18                  车端后车灯(常开)： 121 01 00 00 01 50 00 00 00
 
                                                                  byte[0] byte[1] byte[2] byte[3] byte[4] byte[5] byte[6] byte[7]
 舱端油门：      181 00 00 FF 00 7F 02 67 18            车端前进：111   00      96      00      00      00      00      00      00 （小车以0.15m/s前进）
@@ -8,17 +8,20 @@
 
 油门踏板：0-127 刹车踏板：0-127  
 
-                                                      车端旋转：111   00      00      00      c8      00      00      00      00 （小车以0.2rad/s旋转）
-                                                      车体旋转角速度，单位0.001rad/s,值域【-523，523】   
-                                                      byte[0]:移动速度高八位 byte[1]：移动速度低八位  byte[2]：旋转速度高八位 byte[3]:旋转速度低八位          
+舱端左手柄                                             车端旋转：111   00      00      00      c8      00      00      00      00 （小车以0.2rad/s旋转）
+初始状态： 0CFDD634  01 00 01 00 FF 00 00 FF          车体旋转角速度，单位0.001rad/s,值域【-523，523】   
+左转：    0CFDD634  04 90 01 00 FF 00 00 FF          byte[0]:移动速度高八位 byte[1]：移动速度低八位  byte[2]：旋转速度高八位 byte[3]:旋转速度低八位      
+右转：    0CFDD634  10 86 01 00 FF 00 00 FF                                                   
+                                                          
 
-舱端急停：  181  00 00 00 FF 02 02 67 18                  车端急停：
+舱端急停：  181  00 00 00 FF 02 02 67 18               车端急停：111  00      00      00      c8      00      00      00      00
 
-舱端操纵杆初始状态：0CFDD634h  01 00 01 00 FF 00 00 FF                             
-舱端操纵杆前进：   0CFDD634h  01 00 10 FA FF 00 00 FF                  
-舱端操纵杆后退：   0CFDD634h  01 00 04 FA FF 00 00 FF                  
-舱端操纵杆右转：   0CFDD634h  10 FA 01 00 FF 00 00 FF                 
-舱端操纵杆左转：   0CFDD634h  04 FA 01 00 FF 00 00 FF                     
+舱端右手柄
+舱端操纵杆初始状态：0CFDD634  01 00 01 00 FF 00 00 FF                             
+舱端操纵杆前进：   0CFDD634  01 00 10 FA FF 00 00 FF                  
+舱端操纵杆后退：   0CFDD634  01 00 04 FA FF 00 00 FF                  
+舱端操纵杆右转：   0CFDD634  10 FA 01 00 FF 00 00 FF                 
+舱端操纵杆左转：   0CFDD634  04 FA 01 00 FF 00 00 FF                     
 
                                       
                                                                          

zw_pro/EgoSystem/vehicle.cpp

@@ -1,4 +0,0 @@
-#include "vehicle.h"
-#include <iostream>  
-#include <string>  
-

zw_pro/EgoSystem/vehicle.h

@@ -1,36 +0,0 @@
-#ifndef VEHICLE_H
-#define VEHICLE_H
-#include <iostream>  
-#include <string>  
-using namespace std;
-
-unsigned char order_on[8] = {0x01,0x00,0x00,0x00,0x00,0x00,0x00,0x00};        //id:421 指令模式开
-unsigned char headlamp_on[8] = {0x01,0x01,0x50,0x00,0x00,0x00,0x00,0x00};        //id:121 前车灯常开
-unsigned char headlamp_off[8] = {0x01,0x00,0x50,0x00,0x00,0x00,0x00,0x00};       //id:121 前车灯常关
-unsigned char tailamp_on[8] = {0x01,0x00,0x00,0x10,0x50,0x00,0x00,0x00};       //id:121 后车灯常关
-unsigned char taillamp_off[8] = {0x01,0x00,0x00,0x00,0x00,0x00,0x00,0x00};       //id:121 后车灯常关
-unsigned char move_forward[8] = {0x00,0x96,0x00,0x00,0x00,0x00,0x00,0x00};    //id:111 以0.15m/s前进
-unsigned char move_backward[8] = {0x00,0xcc,0x00,0x00,0x00,0x00,0x00,0x00};   //id:111 以0.15m/s后退
-unsigned char move_rotation[8] = {0x00,0x00,0x00,0xc8,0x00,0x00,0x00,0x00};   //id:111 以0.2rad/s旋转
-unsigned char stop[8] = {0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00};            //id:111 急停
-
-class Vehicle {  
-private:  
-    bool order_flag; 
-    bool lamp_flag; // 车灯状态 
-    bool stop_flag; // 急停状态
-    std::string direction_tag; // 车辆当前方向  
-  
-public:  
-    Vehicle() : order_flag(true), lamp_flag(false), stop_flag(false), direction_tag("forward") {}  
-  
-    void order(bool order_flag);
-    void toggleLamp(bool lamp_flag);  // 车灯开关  
-    void moveForward(); // 车辆前进  
-    void turnLeft();  // 车辆左转  
-    void turnRight(); // 车辆右转  
-    void moveBackward();  // 车辆后退（简单实现，不改变方向）  
-};  
-
-
-#endif