bk_list.cpp

  name="google_ads_frame" marginwidth="0" marginheight="0" src="http://pagead2.googlesyndication.com/pagead/ads?client=ca-pub-5572165936844014&dt=1194442938015&lmt=1194190197&format=336x280_as&output=html&correlator=1194442937843&url=file%3A%2F%2F%2FC%3A%2FDocuments%2520and%2520Settings%2Flhh1%2F%E6%A1%8C%E9%9D%A2%2FCLanguage.htm&color_bg=FFFFFF&color_text=000000&color_link=000000&color_url=FFFFFF&color_border=FFFFFF&ad_type=text&ga_vid=583001034.1194442938&ga_sid=1194442938&ga_hid=1942779085&flash=9&u_h=768&u_w=1024&u_ah=740&u_aw=1024&u_cd=32&u_tz=480&u_java=true" frameborder="0" width="336" scrolling="no" height="280" allowtransparency="allowtransparency"> #include <iostream.h>
#include <string.h>
#include <stdlib.h>

class Book {
  public:
    Book(char *title, char *author, char *publisher, float price); // Constructor
    Book() {};
    void show_title(void);
    float get_price(void);
    void show(void);
    void assign_publisher(char *name);
    bool operator==(Book op2);
  private:
    char title[256];
    char author[64];
    float price;
    char publisher[256];
    void show_publisher(void);
};

Book::Book(char *title, char *author, char *publisher, float price)
 {
   strcpy(Book::title, title);
   strcpy(Book::author, author);
   strcpy(Book::publisher, publisher);
   Book::price = price;
 }

void Book::show_title(void)
  { cout << "Title: " << title << endl; };

float Book::get_price(void)
  { return(price); };

void Book::show(void)
  { show_title(); show_publisher(); };

void Book::assign_publisher(char *name)
  { strcpy(publisher, name); };

void Book::show_publisher(void)
  { cout << "Publisher: " << publisher << endl; };

bool Book::operator==(Book op2)
 {
   if(title!=op2.title)
      return false;
   if(author!=op2.author)
      return false;
   if(publisher!=op2.publisher)
      return false;
   if(price!=op2.price)
      return false;
   return true;
 }

template <class DataT> class list_object {
 public:
   DataT info;
   list_object<DataT> *next;
   list_object<DataT> *previous;
   list_object() {
      next = NULL;
      previous = NULL;
    }
   list_object(DataT c) {
      info = c;
      next = NULL;
      previous = NULL;
    }
   list_object<DataT> *getnext() {return next;}
   list_object<DataT> *getprevious() {return previous;}
   void getinfo(DataT &c) { c = info;}
   void change(DataT c) {info = c;}
   friend ostream &operator<<(ostream &stream, list_object<DataT> o)
    {
      stream << o.info << endl;
      return stream;
    }
   friend ostream &operator<<(ostream &stream, list_object<DataT> *o)
    {
      stream << o->info << endl;
      return stream;
    }
/*   friend istream &operator>>(istream &stream, list_object<DataT> &o)
    {
      cout << "Enter information: " << endl;
      stream >> o.info;
      return stream;
    } */
 };

template <class DataT> class linked_list : public list_object<DataT> {
   list_object<DataT> *start, *end;
 public:
   linked_list() {start = end = NULL;}
   void store(DataT c);
//   void store(list_object<DataT> ob);
   void remove(list_object<DataT> *ob);
   void frwdlist();
   void bkwdlist();
   list_object<DataT> *find(list_object<DataT> ob);
   list_object<DataT> *getstart() {return start;}
   list_object<DataT> *getend() {return end;}
 };

template <class DataT> void linked_list<DataT>::store(DataT c)
 {
   list_object<DataT> *p;

   p = new list_object<DataT>;
   if(!p) {
      cout << "Allocation error." << endl;
      exit(1);
    }
   p->info = c;
   if(start==NULL)
    {
      end = start = p;
    }
   else
    {
      p->previous = end;
      end->next = p;
      end = p;
    }
 }

/* template <class DataT> void linked_list<DataT>::store(list_object<DataT> ob)
 {
   list_object<DataT> *p;

   p = new list_object<DataT>;
   if(!p) {
      cout << "Allocation error." << endl;
      exit(1);
    }
   p->info = ob.info;
   if(start==NULL)
    {
      end = start = p;
    }
   else
    {
      p->previous = end;
      end->next = p;
      end = p;
    }
 } */


template <class DataT> void linked_list<DataT>::remove(list_object<DataT> *ob)
 {
   if(ob->previous)
    {
      ob->previous->next = ob->next;
      if(ob->next)
         ob->next->previous = ob->previous;
      else
         end = ob->previous;
    }
   else
    {
      if(ob->next)
       {
         ob->next->previous = NULL;
         start = ob->next;
       }
      else
         start = end = NULL;
    }
 }

template <class DataT> void linked_list<DataT>::frwdlist()
 {
   list_object<DataT> *temp;

   temp = start;
   do {
      cout << temp->info << " ";
      temp = temp->getnext();
    } while(temp);
   cout << endl;
 }

template <class DataT> void linked_list<DataT>::bkwdlist()
 {
   list_object<DataT> *temp;

   temp = end;
   do {
      cout << temp->info << " ";
      temp = temp->getprevious();
   } while(temp);
   cout << endl;
 }

template <class DataT> list_object<DataT> *linked_list<DataT>::find(list_object<DataT> ob)
 {
   list_object<DataT> *temp;

   temp = start;
   while(temp) {
      if(ob.info==temp->info) return temp;
      temp = temp ->getnext();
    }
   return NULL;
 }


void main(void)
 {
   linked_list<Book> list;
   Book cbib("Jamsa's C/C++ Programmer's Bible", "Jamsa and Klander", "Jamsa Press", 49.95);
   Book vbtips("1001 Visual Basic Programmer's Tips", "Jamsa and Klander", "Jamsa Press", 54.95);
   Book hacker("Hacker Proof", "Klander", "Jamsa Press", 54.95);
   Book c;
   list_object<Book> *p;

   list.store(cbib);
   list.store(vbtips);
   list.store(hacker);

   cout << "here are some items." << endl;
//   list.bkwdlist();
//   list.frwdlist();
   cout << endl;
   cout << "'Manually' walk through the list." << endl;
   p = list.getstart();
   while(p) {
      p->getinfo(c);
      c.show();
      p = p->getnext();
    }
   cout << endl << endl;
   cout << "Looking for item 2.2." << endl;
   p = list.find(2.2);
   if(p)
    {
      p->getinfo(c);
      cout << "Found: " << c << endl;
    }
   cout << endl;
   p->getinfo(c);
   cout << "Removing item: " << c << endl;
   list.remove(p);
   cout << "Here is new list forwards." << endl;
   list.frwdlist();
   cout << endl;
   cout << "Adding an item." << endl;
   list.store(4.4);
   cout << "Here is list forwards." << endl;
   list.frwdlist();
   cout << endl;
   p = list.find(1.1);
   if(!p)
    {
      cout << "Error, item not found." << endl;
      return 1;
    }
   p->getinfo(c);
   cout << "Changing " << c << " to 5." << endl;
   p->change(5.5);
   cout << "Here is list forwards, then backwards." << endl;
   list.frwdlist();
   list.bkwdlist();
   cout << endl;
   cin >> *p;
   cout << p;
   cout << "Here is list forwards again." << endl;
   list.frwdlist();
   cout << endl;
   cout << "Here is list after removing head of list." << endl;
   p = list.getstart();
   list.remove(p);
   list.frwdlist();
   cout << endl;
   cout << "Here is list after removing end of list." << endl;
   p = list.getend();
   list.remove(p);
   list.frwdlist();
 }

 

 

 


/********************************************************************* * * Software License Agreement (BSD License) * * Copyright (c) 2024, HRSTEK, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of HRSTEK, Inc. nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * * Authors: HRSTEK *********************************************************************/ #include <iostream> #include <stdio.h> #include <sys/types.h> #include <sys/stat.h> #include <fcntl.h> #include <pthread.h> #include <ctime> #include <cstdlib> #include "unistd.h" #include <cstdio> #include <iostream> #include <string> #include <map> #include <cmath> #include <signal.h> #include <ctime> extern "C" { #include "../inc/ControlCAN.h" } #include "../inc/hrstek_usbcan.h" #include "../inc/hrstek_control.h" #include "../inc/public_variable.h" #include <stdio.h> #include <stdlib.h> #include <string.h> #include <sys/types.h> #include <sys/socket.h> #include <netinet/in.h> #include <arpa/inet.h> #include <unistd.h> #include "hrstek_control.h" using namespace std; bool Cmd_vel_Receiving_flag = false; bool Cmd_vel_Receiving_flag_timeout = false; bool receive_can_Signal_flag = false; bool receive_can_Signal_flag_timeout = false; bool init_motor_flag = false; Motor_Ctrl Motor_L_Ctrl, Motor_R_Ctrl; uint8_t COMPUTER_TYPE_ALL = 0; std::map<std::string, double> chassisConfig = { {"wheel_track", 0.526}, {"max_linear_velocity", 1.32946}}; // """ // 底盘配置 // :return: 底盘配置, 字典类型, 包含以下键值对: // - "wheel_radius": 轮子半径, 单位m // - "wheel_track": 轮距, 单位m // - "reduce_ratio": 减速比 // - "bevel_gear_ratio": 锥齿轮比 // - "max_linear_velocity": 最大线速度, 单位m/s // - "max_angular_velocity": 最大角速度, 单位rad/s // - "encoder_resolution": 编码器分辨率 // - ... 其他配置信息 // "" //------------------------------------------------------------------ // 控制前翻转臂和后翻转臂的动作状态 bool front_flipper_up = false; bool front_flipper_down = false; bool rear_flipper_up = false; bool rear_flipper_down = false; double maxRpm = 8000; double wheelRadius = 0.1175; double reduceRatio = 42.31; double bevelGearRatio = 16.0 / 28.0; double wheelTrack = 0.526; uint8_t LR_Max_velo = 100; // 左右电机最大速度 int MaxSpeedValue = 256; // 0xff 反向速度参数最大值 int MaxMotorV = 500; // 速度比 电机最大速度脉冲数/电机最大速度转速数 转速比例系数33.33:1 int NodeID_R = 0x602; int NodeID_L = 0x601; int NodeID_F = 0x67B; uint8_t Left_enable[3] = {0x01, 0x02}; // 左轮激活 uint8_t Right_enable[3] = {0x01, 0x01}; // 右轮激活 uint8_t Query_speed[8] = {0x40, 0x6C, 0x60, 0x00, 0x00, 0x00, 0x00, 0x00}; // 速度查询命令 velocity actual value uint8_t Query_error_code[8] = {0x40, 0x3f, 0x60, 0x00, 0x00, 0x00, 0x00, 0x00}; // 故障码查询命令 error code uint8_t Query_Position[8] = {0x40, 0x64, 0x60, 0x00, 0x00, 0x00, 0x00, 0x00}; // 位置查询命令 position actual value uint8_t Query_current[8] = {0x40, 0x78, 0x60, 0x00, 0x00, 0x00, 0x00, 0x00}; // 电流查询命令 current actual value uint8_t Bus_enable[3] = {0x00, 0x01, 0x00}; // 总线激活 uint8_t Velocity_mode[8] = {0x2f, 0x60, 0x60, 0x00, 0x03, 0x00, 0x00, 0x00}; // 设置工作模式 速度环模式 uint8_t Control_disenable[8] = {0x2b, 0x40, 0x60, 0x00, 0x06, 0x00, 0x00, 0x00}; // 控制字去使能 uint8_t Control_1ock_enable[8] = {0x2b, 0x40, 0x60, 0x00, 0x07, 0x00, 0x00, 0x00}; // 控制字锁住 uint8_t Control_enable[8] = {0x2b, 0x40, 0x60, 0x00, 0x0f, 0x00, 0x00, 0x00}; // 控制字加上使能 uint8_t Speed_increase[8] = {0x23, 0x83, 0x60, 0x00, 0xE8, 0x03, 0x00, 0x00}; // 电机加速度值 uint8_t Speed_decrease[8] = {0x23, 0x84, 0x60, 0x00, 0xE8, 0x03, 0x00, 0x00}; // 电机减速度值 uint8_t Query_voltage[8] = {0x40, 0x79, 0x60, 0x00, 0x00, 0x00, 0x00, 0x00}; // 电压查询命令 dc link circuit voltage uint8_t mcu_state[1] = {0x05}; void hrstek_control::init(uint8_t COMPUTER_TYPE, unsigned int USB_CAN_TYPE, unsigned int USB_CAN_BAUD_RATE, unsigned int CHASSIS_TYPE) { //COMPUTER_TYPE_ALL = COMPUTER_TYPE; Select_chassis_type(CHASSIS_TYPE); //Motor_Init(); startSendThread(); std::cout << "111111111111111111111111111111111111--------------------------------------------"<< std::endl; // startReceiveThread(); // startArmFeedbackThread(); // startFaultReceiveThread(); std::cout << "ceshiceshiceshiceshiceshiceshi--------------------------------------------"<< std::endl; //hrstek_usbcan_main = new hrstek_usbcan(); //hrstek_usbcan_main->can_init(USB_CAN_TYPE, USB_CAN_BAUD_RATE); //motordrive_init(); //startSendThread(); monitor_start_Thread(); } void hrstek_control::startFaultReceiveThread() { int sockfd = socket(AF_INET, SOCK_DGRAM, 0); if (sockfd < 0) { perror("Socket creation failed"); return; } int optval = 1; if (setsockopt(sockfd, SOL_SOCKET, SO_REUSEADDR, &optval, sizeof(optval)) < 0) { perror("setsockopt SO_REUSEADDR failed"); close(sockfd); return; } if (setsockopt(sockfd, SOL_SOCKET, SO_REUSEPORT, &optval, sizeof(optval)) < 0) { perror("setsockopt SO_REUSEPORT failed"); close(sockfd); return; } struct sockaddr_in server_addr; server_addr.sin_family = AF_INET; server_addr.sin_port = htons(4000); // 端口号 server_addr.sin_addr.s_addr = inet_addr("192.168.1.50"); // 绑定到指定IP地址 if (bind(sockfd, (struct sockaddr *)&server_addr, sizeof(server_addr)) < 0) { perror("Bind failed"); close(sockfd); return; } std::cout << "Fault Receiver started on IP 192.168.1.50, port 4000" << std::endl; pthread_t receive_thread; if (pthread_create(&receive_thread, NULL, receiveFaultDataThread, (void *)(intptr_t)sockfd) != 0) { perror("Thread creation failed"); close(sockfd); } else { pthread_detach(receive_thread); // 确保线程分离 } } void *hrstek_control::receiveFaultDataThread(void *arg) { int sockfd = (intptr_t)arg; struct sockaddr_in client_addr; socklen_t addr_len = sizeof(client_addr); const int buffer_size = 1024; uint8_t buffer[buffer_size]; stFaultPacket fault_data; while (true) { int bytes_received = recvfrom(sockfd, buffer, buffer_size, 0, (struct sockaddr *)&client_addr, &addr_len); if (bytes_received < 0) { perror("recvfrom failed"); continue; } // 解析数据 if (buffer[0] == 0xAA) { // 假设Header为0xAA memcpy(&fault_data, buffer, sizeof(fault_data)); printFaultData(fault_data, client_addr); } } return NULL; } void hrstek_control::printFaultData(const stFaultPacket &data, const struct sockaddr_in &client_addr) { std::cout << "Received Fault Data from " << inet_ntoa(client_addr.sin_addr) << ":" << ntohs(client_addr.sin_port) << std::endl; std::cout << "Header: " << (int)data.Header << std::endl; std::cout << "Error Count: " << (int)data.ErrorCount << std::endl; std::cout << "Index: " << (int)data.Index << std::endl; std::cout << "Error Code: " << data.ErrorCode << std::endl; std::cout << "Verify: " << (int)data.Verify << std::endl; std::cout << "-------------------------------------------------------------------------------------" << std::endl; } //-------------------------------------------------------------------------------------------------- void hrstek_control::startArmFeedbackThread() { int sockfd = socket(AF_INET, SOCK_DGRAM, 0); if (sockfd < 0) { perror("Socket creation failed"); return; } struct sockaddr_in server_addr; server_addr.sin_family = AF_INET; server_addr.sin_port = htons(10001); // 数据接收端口 server_addr.sin_addr.s_addr = inet_addr("192.168.1.50"); // 绑定到指定IP地址 if (bind(sockfd, (struct sockaddr *)&server_addr, sizeof(server_addr)) < 0) { perror("Bind failed"); close(sockfd); return; } std::cout << "Arm Feedback Receiver started on IP 192.168.1.50, port 10001" << std::endl; pthread_t receive_thread; if (pthread_create(&receive_thread, NULL, receiveArmFeedbackThread, (void *)(intptr_t)sockfd) != 0) { perror("Thread creation failed"); close(sockfd); } } void *hrstek_control::receiveArmFeedbackThread(void *arg) { int sockfd = (intptr_t)arg; struct sockaddr_in client_addr; socklen_t addr_len = sizeof(client_addr); const int buffer_size = 2048; uint8_t buffer[buffer_size]; stUDPFBPacket_Pos packet_data; while (true) { int bytes_received = recvfrom(sockfd, buffer, buffer_size, 0, (struct sockaddr *)&client_addr, &addr_len); if (bytes_received < 0) { perror("recvfrom failed"); continue; } // 解析数据 // if (buffer[0] == 0xAA) { // 假设Header为0xAA // memcpy(&packet_data, buffer, sizeof(packet_data)); // printArmFeedbackData(packet_data, client_addr, addr_len); // 传递 client_addr 和 addr_len // } // 假设Header为0xAA memcpy(&packet_data, buffer, sizeof(packet_data)); std::cout << "dddddddddddddddddddddddddddddddddddddddddddddddddddddd--------------------------------------------"<< std::endl; printArmFeedbackData(packet_data, client_addr, addr_len); // 传递 client_addr 和 addr_len } return NULL; } void hrstek_control::printArmFeedbackData(const stUDPFBPacket_Pos &data, const struct sockaddr_in &client_addr, socklen_t addr_len) { std::cout << "Received Arm Feedback Data from " << inet_ntoa(client_addr.sin_addr) << ":" << ntohs(client_addr.sin_port) << std::endl; std::cout << "Header: " << (int)data.Header << std::endl; std::cout << "Packet Length: " << (int)data.PacketLength << std::endl; std::cout << "Vehicle Voltage: " << (int)data.VehicleVoltage << std::endl; std::cout << "Joint1 Status: " << data.Joint1Status << std::endl; std::cout << "Joint2 Status: " << data.Joint2Status << std::endl; std::cout << "Joint3 Status: " << data.Joint3Status << std::endl; std::cout << "Joint4 Status: " << data.Joint4Status << std::endl; std::cout << "Joint5 Status: " << data.Joint5Status << std::endl; std::cout << "Joint6 Status: " << data.Joint6Status << std::endl; std::cout << "AuxVFW Status: " << data.AuxVFWStatus << std::endl; std::cout << "AuxVBK Status: " << data.AuxVBKStatus << std::endl; std::cout << "SP Work Complete: " << (int)data.SPWorkComplete << std::endl; std::cout << "Joint1 Pos: " << data.Joint1Pos << std::endl; std::cout << "Joint2 Pos: " << data.Joint2Pos << std::endl; std::cout << "Joint3 Pos: " << data.Joint3Pos << std::endl; std::cout << "Joint4 Pos: " << data.Joint4Pos << std::endl; std::cout << "Joint5 Pos: " << data.Joint5Pos << std::endl; std::cout << "Joint6 Pos: " << data.Joint6Pos << std::endl; std::cout << "FW AuxV Pos: " << data.FWAuxVPos << std::endl; std::cout << "BK AuxV Pos: " << data.BKAuxVPos << std::endl; std::cout << "Error Code: " << data.ErrorCode << std::endl; std::cout << "Actual Torque: " << data.ActualTorque << std::endl; std::cout << "Mile Distance: " << data.MileDistance << std::endl; std::cout << "Run Time: " << data.RunTime << std::endl; std::cout << "CRC8: " << (int)data.CRC8 << std::endl; std::cout << "-------------------------------------------------------------------------------------" << std::endl; } //----------------------------------------------------------------------------------------------------- void hrstek_control::startReceiveThread() { int sockfd = socket(AF_INET, SOCK_DGRAM, 0); if (sockfd < 0) { perror("Socket creation failed"); return; } std::cout << "ceshiceshiceshiceshiceshiceshi--------------------------------------------"<< std::endl; struct sockaddr_in server_addr; server_addr.sin_family = AF_INET; server_addr.sin_port = htons(5020); // 底盘数据端口 // server_addr.sin_addr.s_addr = INADDR_ANY; // 绑定到所有可用的网络接口 server_addr.sin_addr.s_addr = inet_addr("192.168.1.50"); // 绑定套接字 if (bind(sockfd, (struct sockaddr *)&server_addr, sizeof(server_addr)) < 0) { perror("Bind failed"); close(sockfd); return; } socklen_t addr_len = sizeof(server_addr); pthread_t receive_thread; if (pthread_create(&receive_thread, NULL, receiveChassisDataThread, (void *)(intptr_t)sockfd) != 0) { perror("Thread creation failed"); close(sockfd); } } void *hrstek_control::receiveChassisDataThread(void *arg) { int sockfd = (intptr_t)arg; struct sockaddr_in server_addr; socklen_t addr_len = sizeof(server_addr); receiveChassisData(sockfd, server_addr, addr_len); return NULL; } // CRC8 校验函数 uint8_t calculateCRC8(const uint8_t *data, size_t length) { uint8_t crc = 0; for (size_t i = 0; i < length; ++i) { crc ^= data[i]; } return crc; } // // 字节序转换函数 // void swapBytesIfNeeded(ChassisData &data) { // data.ultrasonic1 = ntohs(data.ultrasonic1); // data.ultrasonic2 = ntohs(data.ultrasonic2); // data.ultrasonic3 = ntohs(data.ultrasonic3); // data.ultrasonic4 = ntohs(data.ultrasonic4); // uint32_t *floatFields[] = {reinterpret_cast<uint32_t *>(&data.gps_latitude), // reinterpret_cast<uint32_t *>(&data.gps_longitude), // reinterpret_cast<uint32_t *>(&data.gps_altitude), // reinterpret_cast<uint32_t *>(&data.imu_accel_x), // reinterpret_cast<uint32_t *>(&data.imu_accel_y), // reinterpret_cast<uint32_t *>(&data.imu_accel_z), // reinterpret_cast<uint32_t *>(&data.imu_velocity_x), // reinterpret_cast<uint32_t *>(&data.imu_velocity_y), // reinterpret_cast<uint32_t *>(&data.imu_velocity_z), // reinterpret_cast<uint32_t *>(&data.imu_angle_x), // reinterpret_cast<uint32_t *>(&data.imu_angle_y), // reinterpret_cast<uint32_t *>(&data.imu_angle_z), // reinterpret_cast<uint32_t *>(&data.left_motor_speed_abs), // reinterpret_cast<uint32_t *>(&data.right_motor_speed_abs), // reinterpret_cast<uint32_t *>(&data.cabin_temperature), // reinterpret_cast<uint32_t *>(&data.vehicle_voltage), // reinterpret_cast<uint32_t *>(&data.vehicle_battery_percentage)}; // for (auto &field : floatFields) { // *field = ntohl(*field); // } // } void swapBytesIfNeeded(ChassisData &data) { // 转换 16 位字段 data.ultrasonic1 = ntohs(data.ultrasonic1); data.ultrasonic2 = ntohs(data.ultrasonic2); data.ultrasonic3 = ntohs(data.ultrasonic3); data.ultrasonic4 = ntohs(data.ultrasonic4); // 转换 32 位字段 data.gps_latitude = ntohl(*reinterpret_cast<uint32_t *>(&data.gps_latitude)); data.gps_longitude = ntohl(*reinterpret_cast<uint32_t *>(&data.gps_longitude)); data.gps_altitude = ntohl(*reinterpret_cast<uint32_t *>(&data.gps_altitude)); data.imu_accel_x = ntohl(*reinterpret_cast<uint32_t *>(&data.imu_accel_x)); data.imu_accel_y = ntohl(*reinterpret_cast<uint32_t *>(&data.imu_accel_y)); data.imu_accel_z = ntohl(*reinterpret_cast<uint32_t *>(&data.imu_accel_z)); data.imu_velocity_x = ntohl(*reinterpret_cast<uint32_t *>(&data.imu_velocity_x)); data.imu_velocity_y = ntohl(*reinterpret_cast<uint32_t *>(&data.imu_velocity_y)); data.imu_velocity_z = ntohl(*reinterpret_cast<uint32_t *>(&data.imu_velocity_z)); data.imu_angle_x = ntohl(*reinterpret_cast<uint32_t *>(&data.imu_angle_x)); data.imu_angle_y = ntohl(*reinterpret_cast<uint32_t *>(&data.imu_angle_y)); data.imu_angle_z = ntohl(*reinterpret_cast<uint32_t *>(&data.imu_angle_z)); data.left_motor_speed_abs = ntohl(*reinterpret_cast<uint32_t *>(&data.left_motor_speed_abs)); data.right_motor_speed_abs = ntohl(*reinterpret_cast<uint32_t *>(&data.right_motor_speed_abs)); data.cabin_temperature = ntohl(*reinterpret_cast<uint32_t *>(&data.cabin_temperature)); data.vehicle_voltage = ntohl(*reinterpret_cast<uint32_t *>(&data.vehicle_voltage)); data.vehicle_battery_percentage = ntohl(*reinterpret_cast<uint32_t *>(&data.vehicle_battery_percentage)); } // 接收底盘数据的线程函数 void hrstek_control::receiveChassisData(int sockfd, struct sockaddr_in &server_addr, socklen_t &addr_len) { const int buffer_size = 1024; uint8_t buffer[buffer_size]; struct ChassisData chassis_data; while (true) { int bytes_received = recvfrom(sockfd, buffer, buffer_size, 0, (struct sockaddr *)&server_addr, &addr_len); if (bytes_received < 0) { perror("recvfrom failed"); continue; } printf("Cabin Temperature: %f\n",chassis_data.cabin_temperature); printf("-------------------------------------------------------------------------------------\n"); // 检查数据包头部 if (buffer[0] == 0xAA && buffer[1] == 0x80) { memcpy(&chassis_data, buffer, sizeof(chassis_data)); // 转换字节序 swapBytesIfNeeded(chassis_data); // // 校验数据完整性 // uint8_t receivedCRC = calculateCRC8(buffer, sizeof(ChassisData) - 1); // if (receivedCRC != chassis_data.crc8) { // std::cerr << "CRC mismatch, data may be corrupted" << std::endl; // continue; // } // 打印数据 std::cout << "Received chassis data from " << inet_ntoa(server_addr.sin_addr) << ":" << ntohs(server_addr.sin_port) << std::endl; std::cout << "Header: " << (int)chassis_data.header << std::endl; std::cout << "Flag: " << (int)chassis_data.flag << std::endl; std::cout << "Ultrasonic 1: " << chassis_data.ultrasonic1 << std::endl; std::cout << "Ultrasonic 2: " << chassis_data.ultrasonic2 << std::endl; std::cout << "Ultrasonic 3: " << chassis_data.ultrasonic3 << std::endl; std::cout << "Ultrasonic 4: " << chassis_data.ultrasonic4 << std::endl; std::cout << "GPS Latitude: " << chassis_data.gps_latitude << std::endl; std::cout << "GPS Longitude: " << chassis_data.gps_longitude << std::endl; std::cout << "GPS Altitude: " << chassis_data.gps_altitude << std::endl; std::cout << "IMU Accel X: " << chassis_data.imu_accel_x << std::endl; std::cout << "IMU Accel Y: " << chassis_data.imu_accel_y << std::endl; std::cout << "IMU Accel Z: " << chassis_data.imu_accel_z << std::endl; std::cout << "IMU Velocity X: " << chassis_data.imu_velocity_x << std::endl; std::cout << "IMU Velocity Y: " << chassis_data.imu_velocity_y << std::endl; std::cout << "IMU Velocity Z: " << chassis_data.imu_velocity_z << std::endl; std::cout << "IMU Angle X: " << chassis_data.imu_angle_x << std::endl; std::cout << "IMU Angle Y: " << chassis_data.imu_angle_y << std::endl; std::cout << "IMU Angle Z: " << chassis_data.imu_angle_z << std::endl; std::cout << "Left Motor Speed Percent: " << (int)chassis_data.left_motor_speed_percent << std::endl; std::cout << "Right Motor Speed Percent: " << (int)chassis_data.right_motor_speed_percent << std::endl; std::cout << "Left Motor Speed Abs: " << chassis_data.left_motor_speed_abs << std::endl; std::cout << "Right Motor Speed Abs: " << chassis_data.right_motor_speed_abs << std::endl; std::cout << "Left Motor Current: " << chassis_data.left_motor_current << std::endl; std::cout << "Right Motor Current: " << chassis_data.right_motor_current << std::endl; std::cout << "Left Motor Temperature: " << chassis_data.left_motor_temperature << std::endl; std::cout << "Right Motor Temperature: " << chassis_data.right_motor_temperature << std::endl; std::cout << "Left Motor Encoder Position: " << chassis_data.left_motor_encoder_position << std::endl; std::cout << "Right Motor Encoder Position: " << chassis_data.right_motor_encoder_position << std::endl; std::cout << "Cabin Temperature: " << chassis_data.cabin_temperature << std::endl; std::cout << "Vehicle Voltage: " << chassis_data.vehicle_voltage << std::endl; std::cout << "Vehicle Battery Percentage: " << chassis_data.vehicle_battery_percentage << std::endl; std::cout << "H2S Gas Concentration: " << chassis_data.h2s_gas_concentration << std::endl; std::cout << "CO Gas Concentration: " << chassis_data.co_gas_concentration << std::endl; std::cout << "O2 Gas Concentration: " << chassis_data.o2_gas_concentration << std::endl; std::cout << "LEL Gas Concentration: " << chassis_data.lel_gas_concentration << std::endl; // std::cout << "CRC8: " << (int)chassis_data.crc8 << std::endl; } } } // void hrstek_control::receiveChassisData(int sockfd, struct sockaddr_in &server_addr, socklen_t &addr_len) { // const int buffer_size = 1024; // uint8_t buffer[buffer_size]; // struct ChassisData chassis_data; // while (true) { // int bytes_received = recvfrom(sockfd, buffer, buffer_size, 0, (struct sockaddr *)&server_addr, &addr_len); // if (bytes_received < 0) { // perror("recvfrom failed"); // continue; // } // printf("Cabin Temperature: %f\n",chassis_data.cabin_temperature); // printf("-------------------------------------------------------------------------------------\n"); // // 解析数据 // if (buffer[0] == 0xAA && buffer[1] == 0x80) { // memcpy(&chassis_data, buffer, sizeof(chassis_data)); // std::cout << "Received chassis data from " << inet_ntoa(server_addr.sin_addr) << ":" << ntohs(server_addr.sin_port) << std::endl; // std::cout << "Header: " << (int)chassis_data.header << std::endl; // std::cout << "Flag: " << (int)chassis_data.flag << std::endl; // std::cout << "Ultrasonic 1: " << chassis_data.ultrasonic1 << std::endl; // std::cout << "Ultrasonic 2: " << chassis_data.ultrasonic2 << std::endl; // std::cout << "Ultrasonic 3: " << chassis_data.ultrasonic3 << std::endl; // std::cout << "Ultrasonic 4: " << chassis_data.ultrasonic4 << std::endl; // std::cout << "GPS Latitude: " << chassis_data.gps_latitude << std::endl; // std::cout << "GPS Longitude: " << chassis_data.gps_longitude << std::endl; // std::cout << "GPS Altitude: " << chassis_data.gps_altitude << std::endl; // std::cout << "IMU Accel X: " << chassis_data.imu_accel_x << std::endl; // std::cout << "IMU Accel Y: " << chassis_data.imu_accel_y << std::endl; // std::cout << "IMU Accel Z: " << chassis_data.imu_accel_z << std::endl; // std::cout << "IMU Velocity X: " << chassis_data.imu_velocity_x << std::endl; // std::cout << "IMU Velocity Y: " << chassis_data.imu_velocity_y << std::endl; // std::cout << "IMU Velocity Z: " << chassis_data.imu_velocity_z << std::endl; // std::cout << "IMU Angle X: " << chassis_data.imu_angle_x << std::endl; // std::cout << "IMU Angle Y: " << chassis_data.imu_angle_y << std::endl; // std::cout << "IMU Angle Z: " << chassis_data.imu_angle_z << std::endl; // std::cout << "Left Motor Speed Percent: " << (int)chassis_data.left_motor_speed_percent << std::endl; // std::cout << "Right Motor Speed Percent: " << (int)chassis_data.right_motor_speed_percent << std::endl; // std::cout << "Left Motor Speed Abs: " << chassis_data.left_motor_speed_abs << std::endl; // std::cout << "Right Motor Speed Abs: " << chassis_data.right_motor_speed_abs << std::endl; // std::cout << "Left Motor Current: " << chassis_data.left_motor_current << std::endl; // std::cout << "Right Motor Current: " << chassis_data.right_motor_current << std::endl; // std::cout << "Left Motor Temperature: " << chassis_data.left_motor_temperature << std::endl; // std::cout << "Right Motor Temperature: " << chassis_data.right_motor_temperature << std::endl; // std::cout << "Left Motor Encoder Position: " << chassis_data.left_motor_encoder_position << std::endl; // std::cout << "Right Motor Encoder Position: " << chassis_data.right_motor_encoder_position << std::endl; // std::cout << "Cabin Temperature: " << chassis_data.cabin_temperature << std::endl; // std::cout << "Vehicle Voltage: " << chassis_data.vehicle_voltage << std::endl; // std::cout << "Vehicle Battery Percentage: " << chassis_data.vehicle_battery_percentage << std::endl; // std::cout << "H2S Gas Concentration: " << chassis_data.h2s_gas_concentration << std::endl; // std::cout << "CO Gas Concentration: " << chassis_data.co_gas_concentration << std::endl; // std::cout << "O2 Gas Concentration: " << chassis_data.o2_gas_concentration << std::endl; // std::cout << "LEL Gas Concentration: " << chassis_data.lel_gas_concentration << std::endl; // } // } // } void hrstek_control::stop_can() { } void hrstek_control::Select_chassis_type(unsigned int CHASSIS_TYPE) { } // 根据电机的速度和方向计算控制字节 uint8_t calculateMotorControl(int speed, int direction) { if (speed == 0) { // 当速度为0时,返回 0x00 return 0; } if (direction == 1) { // Forward direction: 0x00 - 0x64 if (speed < 0) { speed = 0; } else if (speed > 100) { speed = 100; } return static_cast<uint8_t>(speed); } else { // Backward direction: 0x80 - 0xFF (from -100 to -0) int calculatedValue = 155 + (100 - speed); if (calculatedValue < 100) { calculatedValue = 100; } else if (calculatedValue > 255) { calculatedValue = 255; } return static_cast<uint8_t>(calculatedValue); } } double hrstek_control::calculate_max_linear_velocity() { // // 将RPM转换为rad/s // double omegaWheel = (maxRpm * 2 * M_PI * wheelRadius) / (60 * reduceRatio) * bevelGearRatio; // 电机的角速度,单位为rad/s // // 计算最大线速度 // double maxLinearVelocity = omegaWheel; // return maxLinearVelocity; return 1; } // 按照配置构建要发送的数据数组 uint8_t send_data[11]; // 控制反转臂 void hrstek_control::frontcontrolFlippers(bool up, bool down) { // 控制前翻转臂 if (up) { send_data[2] |= 0x40; // 设置Bit 6为1 } else { send_data[2] &= ~0x40; // 设置Bit 6为0 } if (down) { send_data[2] |= 0x80; // 设置Bit 7为1 } else { send_data[2] &= ~0x80; // 设置Bit 7为0 } } void hrstek_control::backcontrolFlippers(bool up, bool down) { // 控制后翻转臂 if (up) { send_data[7] |= 0x40; // 设置Bit 6为1 } else { send_data[7] &= ~0x40; // 设置Bit 6为0 } if (down) { send_data[7] |= 0x80; // 设置Bit 7为1 } else { send_data[7] &= ~0x80; // 设置Bit 7为0 } } bool hrstek_control::loop_control(std::map<std::string, std::map<std::string, double>> &twist) { // 需要发送的帧,结构体设置 // 获取底盘配置参数 Cmd_vel_Receiving_flag = true; double trackWidth = 0.52; double maxVelocity = 1.3; // 从Twist消息中提取线速度和角速度 double linearVelocity = twist["linear"]["x"]; // 前进速度(m/s) double angularVelocity = twist["angular"]["z"]; // 角速度(rad/s) // 计算左右轮的线速度 double vLeft = linearVelocity - (trackWidth / 2.0) * angularVelocity; double vRight = linearVelocity + (trackWidth / 2.0) * angularVelocity; // 确定左右轮速度的符号 int leftSign = vLeft >= 0 ? 1 : -1; int rightSign = vRight >= 0 ? 1 : -1; // 将速度转换为相对于最大速度的百分比并取整 int leftVelocity = static_cast<int>(std::round(std::abs(vLeft) / maxVelocity * 100)); int rightVelocity = static_cast<int>(std::round(std::abs(vRight) / maxVelocity * 100)); cout << "-------------------> \n"; cout << "linearVelocity=" << linearVelocity << " angularVelocity=" << angularVelocity << " \n"; cout << "vLeft=" << vLeft << " vRight=" << vRight << " \n"; // 使用printf输出,将各个变量以浮点数形式展示 printf("leftVelocity=%d leftSign=%d rightVelocity=%d rightSign=%d \n", leftVelocity, leftSign, rightVelocity, rightSign); cout << "<------------------- \n"; usleep(8000); send_data[4] =calculateMotorControl(leftVelocity,leftSign); send_data[5] =calculateMotorControl(rightVelocity,rightSign); if (Cmd_vel_Receiving_flag_timeout == false) { // Set_Motor_Speed(leftVelocity, rightVelocity, leftSign, rightSign); } Cmd_vel_Receiving_flag = false; return TRUE; } void hrstek_control::Set_Motor_Speed(uint8_t Left_speed, uint8_t Right_speed, int leftSign, int rightSign) { } // 计算异或校验值的函数,接收字节数组和数组长度作为参数 uint8_t calculateXORChecksum(const uint8_t* data, size_t length) { if (length == 0) { return 0; } uint8_t checksum = data[0]; for (size_t i = 1; i < length; ++i) { checksum ^= data[i]; } return checksum; } // 发送数据线程函数,每隔一段时间发送固定数据 void *hrstek_control::sendDataThread(void *arg) { hrstek_control *instance = static_cast<hrstek_control *>(arg); // 创建套接字 int sockfd = socket(AF_INET, SOCK_DGRAM, 0); if (sockfd < 0) { perror("Socket creation failed"); } // 设置目标地址结构体 struct sockaddr_in server_addr; server_addr.sin_family = AF_INET; server_addr.sin_port = htons(10000); server_addr.sin_addr.s_addr = inet_addr("192.168.1.110"); while (true) { // Byte 0: Header (Fixed) send_data[0] = 0xAA; // Byte 1: Mode Control (Fixed) send_data[1] = 0xF5; // Byte 2: Mode Select, Light Control, Weapon Control, Front Flipper Control // send_data[2] = 0; // Byte 3: PTZ Control, Video Channel Select send_data[3] = 0x40; // send_data[4] =calculateMotorControl(10,-1); // send_data[5] =calculateMotorControl(10,-1); // // Byte 4: Left Motor Control // send_data[4] = 0; // send_data[4] &= ~(0xFF << 0); // send_data[4] |= (50 << 0); // send_data[4] &= ~(1 << 7); // // Byte 5: Right Motor Control // send_data[5] = 0; // send_data[5] &= ~(0xFF << 0); // send_data[5] |= (60 << 0); // send_data[5] &= ~(1 << 7); // Byte 6: Joint 8, 1, 2, 3 send_data[6] = 0; // Byte 7: Joint 4, 5, PTZ Raise/Lower, Back Flipper Control // send_data[7] = 0; // Byte 8: PTZ speed (0x00-0x7F) send_data[8] = 0; // Byte 9: PTZ focus (0x00-0xFA: 0-250, 0x64:=100) send_data[9] = 0x64; // 调用函数计算异或校验值 uint8_t xor_checksum = calculateXORChecksum(send_data, 10); // std::cout << "异或校验值为: 0x" << std::hex << static_cast<int>(xor_checksum) << std::dec << std::endl; // Byte 10: CRC send_data[10] = xor_checksum; // 发送数据 if (sendto(sockfd, send_data, 11, 0, (struct sockaddr *)&server_addr, sizeof(server_addr)) < 0) { perror("Sendto failed"); } // std::cout <<"------>"<< std::endl; // for (int i = 0; i < 11; ++i) { // // 使用printf以十六进制格式输出每个字节元素,%02X表示以十六进制大写形式输出,宽度为2,不足两位前面补0 // printf("%02X ", send_data[i]); // } // printf("\n"); // std::cout <<"<------"<< std::endl; usleep(300000); } pthread_exit(NULL); } // 启动发送数据线程函数 void hrstek_control::startSendThread() { pthread_create(&sendThread, NULL, sendDataThread, this); } void hrstek_control::CAN_trans(unsigned short stdid_can, unsigned char can_buf[], unsigned char DataLen) { } union { int a; unsigned char d[4]; } data_union; void hrstek_control::mc_Can_SdoWr(unsigned short NodeID, unsigned int ODIndex, unsigned int ODSubIndex, unsigned int ODData) { } void hrstek_control::Motor_Init(void) { } void hrstek_control::motordrive_init(void) { } void hrstek_control::monitor_start_Thread() { pthread_create(&p_monitorThread, NULL, monitorThread, this); } void *hrstek_control::monitorThread(void *arg) { hrstek_control *instance = static_cast<hrstek_control *>(arg); uint16_t Cmd_vel_Receiving_flag_cout = 0; uint16_t receive_can_Signal_flag_cout = 0; while (true) { if (Cmd_vel_Receiving_flag == false) { if ((Cmd_vel_Receiving_flag_cout < 500) && (Cmd_vel_Receiving_flag_timeout == false)) { Cmd_vel_Receiving_flag_cout++; } else if ((Cmd_vel_Receiving_flag_cout >= 500) && (Cmd_vel_Receiving_flag_timeout == false)) { cout << "-------------> \n"; // 获取当前时间 std::time_t currentTime = std::time(nullptr); // 将当前时间转换为本地时间结构体 std::tm *localTime = std::localtime(&currentTime); // 格式化输出时间,精确到毫秒 char buffer[80]; std::strftime(buffer, sizeof(buffer), "%Y-%m-%d %H:%M:%S", localTime); std::cout << buffer << '.' << std::clock() % 1000 << std::endl; Cmd_vel_Receiving_flag_timeout = true; usleep(5000); cout << "cmd_vel超时!!!!!!!!! \n"; cout << "<------------- \n"; } } else { Cmd_vel_Receiving_flag_timeout = false; Cmd_vel_Receiving_flag_cout = 0; } usleep(1000); } pthread_exit(NULL); } int main(int argc, char **argv) { // 创建底盘控制类的实例 hrstek_control *hrstek_control_main = new hrstek_control(); // 初始化底盘控制类 // 这里需要根据实际情况提供正确的参数 uint8_t COMPUTER_TYPE = COMPUTER_TX2; // 示例参数 unsigned int USB_CAN_TYPE = LCAN_USBCAN2; // 示例参数 unsigned int USB_CAN_BAUD_RATE = USB_CAN_BAUD_RATE_250K; // 示例参数 unsigned int CHASSIS_TYPE = CHASSIS_TYPE_MINI_TANK; // 示例参数 hrstek_control_main->init(COMPUTER_TYPE, USB_CAN_TYPE, USB_CAN_BAUD_RATE, CHASSIS_TYPE); // 主循环,保持程序运行 while (true) { // 这里可以添加一些主循环中的逻辑,例如处理用户输入等 sleep(1); // 每秒检查一次 } // 清理资源 delete hrstek_control_main; return 0; } // 其他成员函数 这个文件的代码是不是需要接受cmd_vel的数据?
最新发布
06-25
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值