PDO(Process Data Object),一般用于承载电机位置、速度,imu状态等对实时性要求高的时时变化的信息
-EtherCAT PDO 对应的 C 结构体,必须是 packed 的。(防止内存内的数据错乱导致数据发疯)
SDO(Service Data Object),一般用于承载一些配置信息,大多数上电之后就不变了
ESC(EtherCAT Slave Controller)
专用硬件
负责 on-the-fly 处理
EtherCAT主站发送一个超大的帧,然后从站自己从之中获取自己想要的数据用于处理
工作计数器(Working Counter,WKC)
能够实现实时性的核心在于“on the fly”(边走边处理),而不需要等待整个数据帧接收完毕
OD全称Object Dictionary(对象字典),是 CANopen(Controller Area Network open,控制器局域网开放)协议的一个核心概念,它是一种可读可写的存储器,用于存储==控制器和设备的参数、状态等==信息。OD是一个逻辑概念,是CANopen协议中定义的一个寄存器集合,提供了对设备参数、控制数据、状态等的访问和配置。
在CANopen协议中,每个设备都有自己的OD,可以通过CAN网络进行访问。OD中的每个对象都有一个独立的标识符(包括索引和子索引),OD的结构为一个键值对集合,其中键是对象的标识符,值是对象的数据。OD中的对象包括了各种类型的数据,例如状态标志、PID参数、采样率等等。
通过访问OD对象,可以读取或写入设备的参数和状态信息,从而实现对设备的配置和控制。与设备的寄存器、内存等类似,OD也需要根据具体的设备类型和协议规范进行配置和使用。
ecat状态机有五种状态,分别对应的是 (OP即为OPERATION)
init :
preOP :
safeOP
OP
Boot
在OP阶段我们即可正常接收和发送PDO数据
[!NOTE]
主站定时发送一个数据帧,传过每一个从站,每个从站处理时间都差不多,都是硬件级别的,整帧的往返时间都差不多,因此EtherCAT的通信延迟是确定的
·实现EtherCAT从站的配置和管理
·实现错误处理和故障诊断
·实现周期性任务的调度和执行
·与其他ROS功能包进行集成,如控制器和传感器驱动程序、导航和运动控制等
SOEM_INTERFACE_RSL相当于对原生的SOEM包做了封装,提供了更加适合我们使用的接口,我们想要使用ECAT读取以及发送数据一般使用SOEM_INTERFACE_RSL里面提供的接口来实现
bus与slave的关系:bus是整个通讯过程的管理者,负责打开网卡,获取数据,发送数据等,可以把它理解为快递员;而slave相当于真实的从站在代码世界里面的抽象,可以将它理解为一个仓库,快递员需要知道自己要去哪一个仓库,仓库长什么样,里面的货物(数据)大小是多少等等
我们需要做一个slave的数字孪生,具体为定义一个继承自soem_interface_rsl::EthercatSlaveBase的类,在里面重写以下几个函数:
以及将所需要读出和写入的数据结构体,以及在startup里面将指令和读取数据的结构体大小给到pdoInfo_.rxPdoSize,如下所示:
const std::string name_;
PdoInfo pdoInfo_;
rm_ecat::standard::TxPdo reading_;
rm_ecat::standard::RxPdo command_;
我们做初始化操作的时候,需要明确一个概念,就是我们操作的对象是BUS而不是SLAVE,我们所需要做的是让BUS能够识别到对应的SLAVE配置,例如执行以下代码:
ecat_bus->addSlave(slave);
ecat_bus->startup(true);
拿到了SLAVE的信息并执行了一系列的操作之后,此时应该能够成功进入到SAVE_OP状态,我们想要拿到PDO的数据的话,还需要手动将状态设为OP,例如:
ecat_bus->setState(soem_interface_rsl::ETHERCAT_SM_STATE::OPERATIONAL);
ecat_bus->waitForState(soem_interface_rsl::ETHERCAT_SM_STATE::OPERATIONAL,0,0);
最后,我们调用updataRead和updateWrite方法即可从预先定义的结构体中获取对应数据或者让程序从结构体中发送数据了,例如:
ecat_bus->updateRead();
ecat_bus->updateWrite();
完整代码示例如下所示(自行添加头文件):
namespace soem_interface_examples {
ExampleSlave::ExampleSlave(const std::string& name, soem_interface_rsl::EthercatBusBase* bus, const uint32_t address)
: soem_interface_rsl::EthercatSlaveBase(bus, address), name_(name) {
}
bool ExampleSlave::startup() {
bus_->waitForState(soem_interface_rsl::ETHERCAT_SM_STATE::PRE_OP);
pdoInfo_.rxPdoSize_ = sizeof(command_);
pdoInfo_.txPdoSize_ = sizeof(reading_);
return true;
}
void ExampleSlave::updateRead() {
bus_->readTxPdo(address_, reading_);
ROS_INFO("data:%d",reading_.can0MotorPositions_[1]);
}
void ExampleSlave::updateWrite() {
command_.can0MotorCommnads_[1]=300;
bus_->writeRxPdo(address_, command_);
}
void ExampleSlave::shutdown() {
// Do nothing
}
} // namespace soem_interface_examples
void init_ecat(std::shared_ptr<soem_interface_rsl::EthercatBusBase> & ecat_bus,std::shared_ptr<soem_interface_examples::ExampleSlave> & slave)
{
ecat_bus->addSlave(slave);
ecat_bus->startup(true);
ROS_INFO("%d",soem_interface_rsl::EthercatBusBase::busIsAvailable("enp2s0"));
ROS_INFO("%d",ecat_bus->getNumberOfSlaves());
ecat_bus->waitForState(soem_interface_rsl::ETHERCAT_SM_STATE::SAFE_OP,0,0);
ecat_bus->setState(soem_interface_rsl::ETHERCAT_SM_STATE::OPERATIONAL);
ecat_bus->waitForState(soem_interface_rsl::ETHERCAT_SM_STATE::OPERATIONAL,0,0);
}
void read_data(std::shared_ptr<soem_interface_rsl::EthercatBusBase> & ecat_bus,std::shared_ptr<soem_interface_examples::ExampleSlave> & slave,rm_ecat::standard::TxPdo &tx_pdo)
{
ecat_bus->doBusMonitoring(false);
ecat_bus->updateRead();
ecat_bus->updateWrite();
}
int main(int argc,char *argv[])
{
ros::init(argc,argv,"test_ecat");
ros::NodeHandle nh;
rm_ecat::standard::TxPdo tx_pdo;
auto ecat_bus=std::make_shared<soem_interface_rsl::EthercatBusBase>("enp2s0");
auto slave=std::make_shared<soem_interface_examples::ExampleSlave>("test_ecat",ecat_bus.get(),1);
init_ecat(ecat_bus,slave);
ros::Rate rate(20);
while (ros::ok())
{
read_data(ecat_bus,slave,tx_pdo);
rate.sleep();
}
}