二自由度机械臂软件系统（补充）达妙电机+创芯can盒底层通信代码测试

• 我这里使用的是达妙的电机+创芯的can盒

一、底层通信demo

• 这个demo只是在一个文件里运行了整套通信功能，发布电机位置到一个topic，然后从一个topic读取位置命令

1、记录一下踩到的坑：

（1） cmakelist中要引入can盒的驱动库，这个驱动库是动态库，所以在编译完成后运行时还要知道其所在的位置，所以要在环境变量中添加。我之前用过这个库，但是当时用了add_library，把一个文件和这个库绑在一起了，成了一个新的静态库，碰巧躲过了这个问题。

set_target_properties(can_motor_test PROPERTIES
    BUILD_RPATH ${LIBRARY_PATH}
    INSTALL_RPATH ${LIBRARY_PATH})

（2） can盒要进行权限的设置

（3）can的接受函数缓存区不能太小，否则读不到数据，也不报错

int reclen = VCI_Receive(VCI_USBCAN2,0,0,rec_data_buff,50,200);

（4）can发送的数据实例有基础参数要设置

	send_data.ID = 1;
	send_data.SendType=1;
	send_data.RemoteFlag=0;
	send_data.ExternFlag=0;
	send_data.DataLen=8;

2、demo代码

#include "controlcan.h"
#include <iostream>
#include <stdio.h>
#include <thread>
#include <mutex>
#include <thread>
#include <chrono>
#include <map>
#include <string>
#include <unistd.h> // 包含 usleep 函数所需的头文件
#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/float64.hpp>

using namespace std;


double pos = 0;
double cmd = 0;
VCI_CAN_OBJ rec_data;
VCI_CAN_OBJ send_data;

std::mutex CanBox_ThreadMutex;  



void CanInit(){
	//1、打开can盒
	int state = 0;
  	state = VCI_OpenDevice(VCI_USBCAN2A,0,0);
	if(state==1)//打开设备
	{
		cout << "open deivce success!\n" <<endl;//打开设备成功
	}else if(state==-1)
	{
		cout << "no can device!\n" <<endl;

	}else{
		printf(">>open deivce fail!\n");//这个就算成功打开，还是为0
		
	}
	//2、初始化两路can口
	
		//2.1 can1初始化
		VCI_INIT_CONFIG config_can;
		config_can.AccCode=0;
		config_can.AccMask=0xFFFFFFFF;
		config_can.Filter=1;//接收所有帧
		config_can.Mode=0;//正常模式

		config_can.Timing0=0x00;/*波特率1000 Kbps*/
		config_can.Timing1=0x14;
		std::cout <<"1000" <<std::endl;

		if(VCI_InitCAN(VCI_USBCAN2,0,0,&config_can)!=1)
		{
			cout << ">>Init CAN fail" <<endl;
			VCI_CloseDevice(VCI_USBCAN2,0);
		}
		else{
			cout << ">>Init CAN success" <<endl;
		}

		if(VCI_StartCAN(VCI_USBCAN2,0,0)!=1)
		{
			// cout << ">>Start CAN" << can_active_list[i].can_index <<"error" <<endl;
			// printf(">>Start CAN1 error\n");
			VCI_CloseDevice(VCI_USBCAN2,0);

		}else{
						cout << ">>start CAN success" <<endl;
			// cout << ">>Start CAN" << can_active_list[i].can_index <<"success" <<endl;
			// printf(">>Start CAN1 success\n");
		}

	for(int i=0;i<10;i++){
		send_data.Data[0] = 0xFF;
		send_data.Data[1] = 0xFF;
		send_data.Data[2] = 0xFF;
		send_data.Data[3] = 0xFF;
		send_data.Data[4] = 0xFF;
		send_data.Data[5] = 0xFF;
		send_data.Data[6] = 0xFF;
		send_data.Data[7] = 0xFC;
		if(VCI_Transmit(VCI_USBCAN2, 0, 0, &send_data, 1) == 1)
		{
			cout<<send_data.ID<<endl;
			printf("set mode success:3\n");	
				
		}else{
			printf("send data fail\n");
		}
	}


	for(int i=0;i<10;i++){

	send_data.Data[0] = 0xFF;
	send_data.Data[1] = 0xFF;
	send_data.Data[2] = 0xFF;
	send_data.Data[3] = 0xFF;
	send_data.Data[4] = 0xFF;
	send_data.Data[5] = 0xFF;
	send_data.Data[6] = 0xFF;
	send_data.Data[7] = 0xFE;
	if(VCI_Transmit(VCI_USBCAN2, 0, 0, &send_data, 1) == 1)
	{
		printf("set zero success:3\n");	
			
	}else{
		printf("send data fail\n");
	}

	}

}
uint16_t float_to_uint(float v,float v_min,float v_max,uint32_t width){
	float temp;
	int32_t utemp;
	temp = ((v-v_min)/(v_max-v_min))*((float)width);
	utemp = (int32_t)temp;
	if(utemp < 0)
	utemp = 0;
	if(utemp > width)
	utemp = width;
	

    
	return utemp;
}


float uint_to_float(uint16_t utemp, float v1, float v2) {
    float v = static_cast<float>((utemp-v1)/v1*v2);
    // float v = (temp-v1)/v1*v2;

    return v;
}

void thread_send_callback(){
	while(1){
	// std::cout << "==================send thread" << std::endl;
	// ROS_INFO("++++++++++send_callback++++++++++");
		uint16_t s_p_int = float_to_uint(cmd, -1433, 1433, 65535);
		uint16_t s_v_int = float_to_uint(0, -1719, 1719, 4096);
		uint16_t  s_t_int = float_to_uint(0, -10, 10, 4096);

		uint16_t s_Kp_int = 0;
		uint16_t s_Kd_int = 0;
		
		//位置模式
		// cout << "位置" << endl;
		s_Kp_int = float_to_uint(1, 0 , 500, 4096);
		s_Kd_int = float_to_uint(1, 0 , 5, 4096);
		

		send_data.Data[0] = s_p_int>>8;
		send_data.Data[1] = s_p_int&0xFF;
		send_data.Data[2] = s_v_int>>4;
		send_data.Data[3] = ((s_v_int&0xF)<<4) + (s_Kp_int >>8);
		send_data.Data[4] = s_Kp_int &0xFF;
		send_data.Data[5] = s_Kd_int>>4;
		send_data.Data[6] = ((s_Kd_int &0xF)<<4) + (s_t_int >>8);
		send_data.Data[7] = s_t_int&0xFF;
			
		CanBox_ThreadMutex.lock();

		if(VCI_Transmit(VCI_USBCAN2, 0, 0, &send_data, 1) == 1)
		{
			// printf("send data success:3\n");	
				
		}else{
			printf("send data fail\n");
		}

		


		CanBox_ThreadMutex.unlock();
			usleep(1);
				
		}
	
}

void thread_receive_callback(){
	while(1){
		// std::cout << "===============receive thread" << std::endl;
			VCI_CAN_OBJ* rec_data_buff = new VCI_CAN_OBJ[50];
		

		CanBox_ThreadMutex.lock();
		int reclen = VCI_Receive(VCI_USBCAN2,0,0,rec_data_buff,50,200);
		// cout << "len:"<<reclen<<"size:"<<sizeof(rec_data_buff)<<endl;
		
		if(reclen > 0)
		{
			printf("rec data success:%d\n",reclen);	
		}else if(reclen ==0){
			// printf("zero:3\n");	
				
		}
		else{
			// std::cout << can_index << std::endl;
			// printf("rec data fail\n");
		}
		
		usleep(1);	//让线程停止1毫秒，防止较快的线程总是占用硬件


	// can_motor_ref[i].id = rec_data_buff.Data[0];

	uint16_t utemp = (static_cast<uint16_t>(rec_data_buff[0].Data[1]) << 8) | rec_data_buff[0].Data[2];
	pos = uint_to_float( utemp,32768,1433)/1;
	std::cout << "pos:"<<pos<<std::endl;


		CanBox_ThreadMutex.unlock();
		delete[] rec_data_buff;
		
	}
}


class PubSubNode : public rclcpp::Node
{
public:
  PubSubNode() : Node("pub_sub_node"), pos_(0.0)
  {
    // 创建发布者
    publisher_ = this->create_publisher<std_msgs::msg::Float64>("pos", 10);
    timer_ = this->create_wall_timer(
      std::chrono::milliseconds(500),
      std::bind(&PubSubNode::publish_pos, this)
    );

    // 创建订阅者
    subscription_ = this->create_subscription<std_msgs::msg::Float64>(
      "cmd", 10,
      std::bind(&PubSubNode::cmd_callback, this, std::placeholders::_1)
    );
  }

private:
  void publish_pos()
  {
    auto message = std_msgs::msg::Float64();
    message.data = pos;
    RCLCPP_INFO(this->get_logger(), "Publishing: '%f'", pos);
    publisher_->publish(message);
    // pos_ += 1.0; // 示例逻辑，增加 pos
  }

  void cmd_callback(const std_msgs::msg::Float64::SharedPtr msg)
  {
	cmd = msg->data;
    RCLCPP_INFO(this->get_logger(), "Received cmd: '%f'", msg->data);
	
  }

  rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr publisher_;
  rclcpp::Subscription<std_msgs::msg::Float64>::SharedPtr subscription_;
  rclcpp::TimerBase::SharedPtr timer_;
  double pos_;
};


int main(int argc, char *argv[]){
	//1、配置参数
	send_data.ID = 1;
	send_data.SendType=1;
	send_data.RemoteFlag=0;
	send_data.ExternFlag=0;
	send_data.DataLen=8;
	//2、初始化
	CanInit();
	//3、启动线程
	std::thread thread_send(thread_send_callback);
	std::thread thread_receive(thread_receive_callback);
	
	thread_send.detach();
	thread_receive.detach();


	rclcpp::init(argc, argv);
	rclcpp::spin(std::make_shared<PubSubNode>());
	rclcpp::shutdown();	


	return 0;
}