第五章 机械臂的组装及控制

零差云控关节模组

https://www.zeroerr.cn/download/eRob.html 获取零差云控最新资料
第一章 单模组的使用

第二章 零差云控关节模组内部构造

第三章 IGH及preempt-rt的安装

第四章 命令行读取电机的信息

第五章 机械臂的组装及控制

---

前言

在linux下使用vs code编写c程序，使用GCC进行编译。
没有写关于CIA402的状态机转换程序，只简单写了进OP的程序，这种方式进OP特别慢，并且没写退出OP的程序。

参考自：https://blog.csdn.net/weixin_40293570/article/details/108712655?ops_request_misc=&request_id=&biz_id=102&utm_term=igh+preempt_rt&utm_medium=distribute.pc_search_result.none-task-blog-2_blogsobaiduweb~default-2-108712655.nonecase&spm=1018.2226.3001.4450

一、机械臂组装

图1 电源电压和电流额定值

图2 电源接线方式

图3 ethercat接线方式
建议：通信线使用双绞线，自己做JST接头，一根到底，防止接线不牢固出现通信问题。模组间连线时，in入out出，依次连接，最终端模组不做冗余时只需连接in即可。
组装前先将编码器外接电池安装上进行编码器复位，并将电机转动至编码器0位处，好像目前还不支持置零操作。

二、对两个关节模组控制

IGH-DC模式对两个关节模组进行控制，使用CSP模式。通过每个周期改变目标位置来实现运动。
注：下面的代码只是简单实现了两个电机的转动

#include <errno.h>
#include <signal.h>
#include <stdio.h>
#include <string.h>
#include <sys/resource.h>
#include <sys/time.h>
#include <sys/types.h>
#include <unistd.h>
#include <time.h>
#include <sys/mman.h>
#include <malloc.h>
#include <sched.h> /* sched_setscheduler() */

/****************************************************************************/
#include "../include/ecrt.h"
#include "../include/ectty.h"                                                                                                                   //     ../表示上一级文件夹
/****************************************************************************/

// Application parameters
#define FREQUENCY 1000
#define CLOCK_TO_USE CLOCK_MONOTONIC
#define MEASURE_TIMING
#define CYCLIC_POSITION            8   /*Operation mode for 0x6060:0*/       /*csp模式*/      //位置模式

/****************************************************************************/

#define NSEC_PER_SEC (1000000000L)
#define PERIOD_NS (NSEC_PER_SEC/ FREQUENCY)    /*本次设置周期PERIOD_NS为1ms*/

#define DIFF_NS(A, B) (((B).tv_sec - (A).tv_sec) * NSEC_PER_SEC + \
(B).tv_nsec - (A).tv_nsec)

#define TIMESPEC2NS(T) ((uint64_t) (T).tv_sec * NSEC_PER_SEC + (T).tv_nsec)

/****************************************************************************/

// EtherCAT
static ec_master_t *master = NULL;
static ec_master_state_t master_state = {
   };
static ec_domain_t *domain1 = NULL;
static ec_domain_state_t domain1_state = {
   };

/****************111111111111***********************/
static ec_slave_config_t *sc[2] ;    //根据从站的个数定
static ec_slave_config_state_t sc_state[2] ;

/****************************************************************************/
// process data
static uint8_t *domain1_pd = NULL;
/****************22222222222222******************/
#define ZeroErr        0,0                        /*EtherCAT address on the bus*/
#define ZeroErr1       0,1
 //上面是从站的地址，每个从站只有地址不一样

#define VID_PID          0x5a65726f,0x00029252   //需要通过sudo ethercat cstruct -a 0命令查看电机的ID
 //上面是电机的VID_PID,相同品牌的电机上面两个相同

/**********************以下五条对从站的PDO进行配置***Offsets for PDO entries *******************************/
static struct{
   
    unsigned int operation_mode;
    unsigned int target_position;
    unsigned int digital_outputs;
    unsigned int ctrl_word;
    unsigned int position_actual_value;
    unsigned int digital_inputs;
    unsigned int status_word;
    int actual_current;
    unsigned int actual_speed;
    int  actual_torque;
}offset,offset1;
/******************333333333333333*******************/
//通过定义新的相同的结构体来进行不同从站各状态的定义,来对多个从站进行配置
const static ec_pdo_entry_reg_t domain1_regs[] = {
   
    //电机0
    {
   ZeroErr, VID_PID, 0x607A, 0, &offset.target_position},
    {
   ZeroErr, VID_PID, 0x60FE, 0, &offset.digital_outputs},
    {
   ZeroErr, VID_PID, 0x6040, 0, &offset.ctrl_word},
    {
   ZeroErr, VID_PID, 0x6064, 0, &offset.position_actual_value},
    {
   ZeroErr, VID_PID, 0x60FD, 0, &offset.digital_inputs},
    {
   ZeroErr, VID_PID, 0x6041, 0, &offset.status_word},
    {
   ZeroErr, VID_PID, 0x6078, 0, &offset.actual_current},
    {
   ZeroErr, VID_PID, 0x606c, 0, &offset.actual_speed},
    {
   ZeroErr, VID_PID, 0x6077, 0, &offset.actual_torque},
    //电机1
    {
   ZeroErr1, VID_PID, 0x607A