/*********************************************************************
*
* 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(¤tTime);
// 格式化输出时间，精确到毫秒
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的数据？
本文介绍了一个Windows应用程序的实现过程，包括窗口类注册、主窗口创建、消息循环处理等关键步骤，并展示了如何动态添加菜单项及其加速键，以及响应不同的菜单选项。
扫一扫
1万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
