/*********************************************************************
*
* Software License Agreement
(BSD License
)
*
* Copyright
(c
) 2
024, 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 = 8
000;
double wheelRadius =
0.1175;
double reduceRatio = 42.31;
double bevelGearRatio = 16.
0 / 28.
0;
double wheelTrack =
0.526;
uint8_t LR_Max_velo = 1
00; // 左右电机最大速度
int MaxSpeedValue = 256; //
0xff 反向速度参数最大值
int MaxMotorV = 5
00; // 速度比 电机最大速度脉冲数/电机最大速度转速数 转速比例系数33.33
:1
int NodeID_R =
0x6
02;
int NodeID_L =
0x6
01;
int NodeID_F =
0x67B;
uint8_t Left_enable[3] = {
0x
01,
0x
02}; // 左轮激活
uint8_t Right_enable[3] = {
0x
01,
0x
01}; // 右轮激活
uint8_t Query_speed[8] = {
0x4
0,
0x6C,
0x6
0,
0x
00,
0x
00,
0x
00,
0x
00,
0x
00}; // 速度查询命令 velocity actual value
uint8_t Query_error_code[8] = {
0x4
0,
0x3f,
0x6
0,
0x
00,
0x
00,
0x
00,
0x
00,
0x
00}; // 故障码查询命令 error code
uint8_t Query_Position[8] = {
0x4
0,
0x64,
0x6
0,
0x
00,
0x
00,
0x
00,
0x
00,
0x
00}; // 位置查询命令 position actual value
uint8_t Query_current[8] = {
0x4
0,
0x78,
0x6
0,
0x
00,
0x
00,
0x
00,
0x
00,
0x
00}; // 电流查询命令 current actual value
uint8_t Bus_enable[3] = {
0x
00,
0x
01,
0x
00}; // 总线激活
uint8_t Velocity_mode[8] = {
0x2f,
0x6
0,
0x6
0,
0x
00,
0x
03,
0x
00,
0x
00,
0x
00}; // 设置工作模式 速度环模式
uint8_t Control_disenable[8] = {
0x2b,
0x4
0,
0x6
0,
0x
00,
0x
06,
0x
00,
0x
00,
0x
00}; // 控制字去使能
uint8_t Control_1ock_enable[8] = {
0x2b,
0x4
0,
0x6
0,
0x
00,
0x
07,
0x
00,
0x
00,
0x
00}; // 控制字锁住
uint8_t Control_enable[8] = {
0x2b,
0x4
0,
0x6
0,
0x
00,
0x
0f,
0x
00,
0x
00,
0x
00}; // 控制字加上使能
uint8_t Speed_increase[8] = {
0x23,
0x83,
0x6
0,
0x
00,
0xE8,
0x
03,
0x
00,
0x
00}; // 电机加速度值
uint8_t Speed_decrease[8] = {
0x23,
0x84,
0x6
0,
0x
00,
0xE8,
0x
03,
0x
00,
0x
00}; // 电机减速度值
uint8_t Query_voltage[8] = {
0x4
0,
0x79,
0x6
0,
0x
00,
0x
00,
0x
00,
0x
00,
0x
00}; // 电压查询命令 dc link circuit voltage
uint8_t mcu_state[1] = {
0x
05};
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
(4
000); // 端口号
server_addr.sin_addr.s_addr = inet_addr
("192.168.1.5
0"
); // 绑定到指定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.5
0, port 4
000" << 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 bu
ffer_size = 1
024;
uint8_t bu
ffer[bu
ffer_size];
stFaultPacket fault_data;
while
(true
) {
int
bytes_received = recvfrom
(sockfd, bu
ffer, bu
ffer_size,
0,
(struct sockaddr *
)&client_addr, &addr_len
);
if
(bytes_received <
0) {
perror
("recvfrom failed"
);
continue;
}
// 解析数据
if
(bu
ffer[
0] ==
0xAA
) { // 假设Header为
0xAA
memcpy
(&fault_data, bu
ffer, 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
(1
0001
); // 数据接收端口
server_addr.sin_addr.s_addr = inet_addr
("192.168.1.5
0"
); // 绑定到指定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.5
0, port 1
0001" << 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 bu
ffer_size = 2
048;
uint8_t bu
ffer[bu
ffer_size];
stUDPFBPacket_Pos packet_data;
while
(true
) {
int
bytes_received = recvfrom
(sockfd, bu
ffer, bu
ffer_size,
0,
(struct sockaddr *
)&client_addr, &addr_len
);
if
(bytes_received <
0) {
perror
("recvfrom failed"
);
continue;
}
// 解析数据
// if
(bu
ffer[
0] ==
0xAA
) { // 假设Header为
0xAA
// memcpy
(&packet_data, bu
ffer, sizeof
(packet_data
));
// printArmFeedbackData
(packet_data, client_addr, addr_len
); // 传递 client_addr 和 addr_len
// }
// 假设Header为
0xAA
memcpy
(&packet_data, bu
ffer, 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
(5
02
0); // 底盘数据端口
// server_addr.sin_addr.s_addr = INADDR_ANY; // 绑定到所有可用的网络接口
server_addr.sin_addr.s_addr = inet_addr
("192.168.1.5
0"
);
// 绑定套接字
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 swap
BytesIfNeeded
(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 swap
BytesIfNeeded
(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 bu
ffer_size = 1
024;
uint8_t bu
ffer[bu
ffer_size];
struct ChassisData chassis_data;
while
(true
) {
int
bytes_received = recvfrom
(sockfd, bu
ffer, bu
ffer_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
(bu
ffer[
0] ==
0xAA && bu
ffer[1] ==
0x8
0) {
memcpy
(&chassis_data, bu
ffer, sizeof
(chassis_data
));
// 转换字节序
swap
BytesIfNeeded
(chassis_data
);
// // 校验数据完整性
// uint8_t receivedCRC = calculateCRC8
(bu
ffer, 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 bu
ffer_size = 1
024;
// uint8_t bu
ffer[bu
ffer_size];
// struct ChassisData chassis_data;
// while
(true
) {
// int
bytes_received = recvfrom
(sockfd, bu
ffer, bu
ffer_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
(bu
ffer[
0] ==
0xAA && bu
ffer[1] ==
0x8
0) {
// memcpy
(&chassis_data, bu
ffer, 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时，返回
0x
00
return
0;
}
if
(direction == 1
) {
// Forward direction
: 0x
00 -
0x64
if
(speed <
0) {
speed =
0;
} else if
(speed > 1
00) {
speed = 1
00;
}
return static_cast<uint8_t>
(speed
);
} else {
// Backward direction
: 0x8
0 -
0xFF (from -1
00 to -
0)
int calculatedValue = 155 +
(1
00 - speed
);
if
(calculatedValue < 1
00) {
calculatedValue = 1
00;
} 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
) /
(6
0 * 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] |=
0x4
0; // 设置Bit 6为1
} else {
send_data[2] &= ~
0x4
0; // 设置Bit 6为
0
}
if
(down
) {
send_data[2] |=
0x8
0; // 设置Bit 7为1
} else {
send_data[2] &= ~
0x8
0; // 设置Bit 7为
0
}
}
void hrstek_control
::backcontrolFlippers
(bool up, bool down
) {
// 控制后翻转臂
if
(up
) {
send_data[7] |=
0x4
0; // 设置Bit 6为1
} else {
send_data[7] &= ~
0x4
0; // 设置Bit 6为
0
}
if
(down
) {
send_data[7] |=
0x8
0; // 设置Bit 7为1
} else {
send_data[7] &= ~
0x8
0; // 设置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 * 1
00));
int rightVelocity = static_cast<int>
(std
::round
(std
::abs
(vRight
) / maxVelocity * 1
00));
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
(8
000);
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
(1
0000);
server_addr.sin_addr.s_addr = inet_addr
("192.168.1.11
0"
);
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] =
0x4
0;
// send_data[4] =calculateMotorControl
(1
0,-1
);
// send_data[5] =calculateMotorControl
(1
0,-1
);
// //
Byte 4
: Left Motor Control
// send_data[4] =
0;
// send_data[4] &= ~
(0xFF <<
0);
// send_data[4] |=
(5
0 <<
0);
// send_data[4] &= ~
(1 << 7
);
// //
Byte 5
: Right Motor Control
// send_data[5] =
0;
// send_data[5] &= ~
(0xFF <<
0);
// send_data[5] |=
(6
0 <<
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
(0x
00-
0x7F
)
send_data[8] =
0;
//
Byte 9
: PTZ focus
(0x
00-
0xFA
: 0-25
0,
0x64
:=1
00)
send_data[9] =
0x64;
// 调用函数计算异或校验值
uint8_t xor_checksum = calculateXORChecksum
(send_data, 1
0);
// std
::cout << "异或校验值为
: 0x" << std
::hex << static_cast<int>
(xor_checksum
) << std
::dec << std
::endl;
//
Byte 1
0: CRC
send_data[1
0] = 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
(3
00000);
}
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 < 5
00) &&
(Cmd_vel_Receiving_flag_timeout == false
))
{
Cmd_vel_Receiving_flag_cout++;
}
else if
((Cmd_vel_Receiving_flag_cout >= 5
00) &&
(Cmd_vel_Receiving_flag_timeout == false
))
{
cout << "-------------> \n";
// 获取当前时间
std
::time_t currentTime = std
::time
(nullptr
);
// 将当前时间转换为本地时间结构体
std
::tm *localTime = std
::localtime
(¤tTime
);
// 格式化输出时间，精确到毫秒
char bu
ffer[8
0];
std
::strftime
(bu
ffer, sizeof
(bu
ffer
), "%Y-%m-%d %H
:%M
:%S", localTime
);
std
::cout << bu
ffer << '.' << std
::clock
() % 1
000 << std
::endl;
Cmd_vel_Receiving_flag_timeout = true;
usleep
(5
000);
cout << "cmd_vel超时!!!!!!!!! \n";
cout << "<------------- \n";
}
}
else
{
Cmd_vel_Receiving_flag_timeout = false;
Cmd_vel_Receiving_flag_cout =
0;
}
usleep
(1
000);
}
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_25
0K; // 示例参数
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的数据？
本文介绍了一种常见的烧写Flash过程中遇到的BytesMismatch问题，即0x00!=0xFF的情况，并给出了一个简单的解决方案：重启Vivado通常可以解决此问题。
扫一扫
109
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
