Linux与倍福ADS通信封装与简单控制测试

在倍福中主要调用控制功能块以实现相应的运动，相当于倍福帮我们做了CANopen DSP402格式的通信，调用功能块可以避免直接操作控制字的难度

倍福PLC程序

PROGRAM MAIN
VAR
	axis1:axis_ref;
	axis2:axis_ref;
	jog:MC_Jog;
	power1:MC_Power;
	power2:MC_Power;
	reset1:MC_Reset;
	reset2:MC_Reset;
	ptp_move:MC_MoveAbsolute;
	jog_act_vel:MC_ReadActualVelocity;
	
	jog_for: BOOL;
	jog_back: BOOL;
	power_do: BOOL;
	jog_vel: LREAL;
	exe_reset1: BOOL;
	exe_reset2: BOOL;
	ptp_do: BOOL;
	ptp_pos: LREAL;
	ptp_vel: LREAL;
	axis1_act_val: LREAL;
	ptp_over: BOOL;
END_VAR

jog(
	Axis:= axis1, 
	JogForward:= jog_for, 
	JogBackwards:= jog_back, 
	Mode:= MC_JOGMODE_CONTINOUS, 
	Position:= , 
	Velocity:= jog_vel, 
	Acceleration:= , 
	Deceleration:= , 
	Jerk:= , 
	Done=> , 
	Busy=> , 
	Active=> , 
	CommandAborted=> , 
	Error=> , 
	ErrorID=> );
	
power1(
	Axis:= axis1, 
	Enable:= power_do, 
	Enable_Positive:= TRUE, 
	Enable_Negative:= TRUE, 
	Override:= , 
	BufferMode:= , 
	Options:= , 
	Status=> , 
	Busy=> , 
	Active=> , 
	Error=> , 
	ErrorID=> );
	
power2(
	Axis:= axis2, 
	Enable:= power_do, 
	Enable_Positive:= TRUE, 
	Enable_Negative:= TRUE, 
	Override:= , 
	BufferMode:= , 
	Options:= , 
	Status=> , 
	Busy=> , 
	Active=> , 
	Error=> , 
	ErrorID=> );
	
reset1(
	Axis:= axis1, 
	Execute:= exe_reset1, 
	Done=> , 
	Busy=> , 
	Error=> , 
	ErrorID=> );
	
reset2(
	Axis:= axis2, 
	Execute:= exe_reset2, 
	Done=> , 
	Busy=> , 
	Error=> , 
	ErrorID=> );
	
ptp_move(
	Axis:= axis2, 
	Execute:= ptp_do, 
	Position:= ptp_pos, 
	Velocity:= ptp_vel, 
	Acceleration:= , 
	Deceleration:= , 
	Jerk:= , 
	BufferMode:= , 
	Options:= , 
	Done=> ptp_over, 
	Busy=> , 
	Active=> , 
	CommandAborted=> , 
	Error=> , 
	ErrorID=> );
	
jog_act_vel(
	Axis:= axis1, 
	Enable:= TRUE, 
	Valid=> , 
	Busy=> , 
	Error=> , 
	ErrorID=> , 
	ActualVelocity=> axis1_act_val);

这里主要用到了MC_Jog、MC_MoveAbsolute等功能块，在倍福官网查看使用说明即可

ADS通信封装

针对初版jog_test.cpp编程上的问题，为提高程序的模块性与简洁性，对ADSLib又做了一层封装

myAds.hpp

#ifndef MY_ADS_H
#define MY_ADS_H

#include "AdsLib.h"
#include "AdsNotification.h"
#include "AdsVariable.h"
#include <iostream>
#include <string>
#include <memory>
using namespace std;

class AdsTalker
{
private:
    string m_netid;
    string m_ipv4;
    int m_port;
    AdsDevice m_route;
public:
    typedef shared_ptr<AdsTalker> Ptr;

    bool checkAdsAddr()
    {
        cout << "Default ADS NetID:" << m_route.defaultNetID << endl;
        cout << "Default ADS IPV4:" << m_route.defaultIPV4 << endl;
        cout << "Default ADS Port:" << m_route.defaultPort << endl;
        cout << "Target ADS NetID:" << m_netid << endl;
        cout << "Target ADS IPV4:" << m_ipv4 << endl;
        cout << "Target ADS Port:" << m_port << endl;
        if (m_netid != m_route.defaultNetID || m_ipv4 != m_route.defaultIPV4 || m_port != m_route.defaultPort)
        {
            return false;
        }
        return true;
    }

    AdsTalker(const string netid, const string ipv4, const int port)
    {
        m_netid = netid;
        m_ipv4 = ipv4;
        m_port = port;
        if (!checkAdsAddr())
        {
            cerr << "Target ADS route conflicts with default ADS route,please check!" << endl;
        }
    }

    AdsVariable<bool> motor_enable{m_route, "MAIN.power_do"};
    AdsVariable<double> jog_vel{m_route, "MAIN.jog_vel"};
    AdsVariable<bool> jog_for{m_route, "MAIN.jog_for"};
    AdsVariable<bool> jog_back{m_route, "MAIN.jog_back"};
    AdsVariable<double> ptp_pos{m_route, "MAIN.ptp_pos"};
    AdsVariable<bool> ptp_do{m_route, "MAIN.ptp_do"};
    AdsVariable<double> ptp_vel{m_route, "MAIN.ptp_vel"};
    AdsVariable<bool> ptp_over{m_route, "MAIN.ptp_over"};

    ~AdsTalker(){};
};

#endif

这里AdsDevice本身是没有无参构造函数的，因此只能修改源码添加预设通信值和无参构造函数

AdsDevice.h

struct AdsDevice
{
    std::string defaultNetID = "169.254.254.142.1.1";
    std::string defaultIPV4 = "169.254.254.142";
    uint16_t defaultPort = AMSPORT_R0_PLC_TC3;

    AdsDevice();

    AdsDevice(const std::string &ipV4, AmsNetId netId, uint16_t port);

    void initAdsDevice(const std::string &ipV4, AmsNetId netId, uint16_t port);

• 本来想通信信息不符合预设值后面也能通过函数更改，所以想在源码中加一个initAdsDevice()函数，但是发现AdsDevice类中的成员变量实在比较难以初始化，因此悻悻作罢
• 只是添加了检查期望通信地址与预设通信地址之间的检查，若错误则打印提示信息
• 这样的好处就是所有需要控制的变量全部放在hpp文件中即可，减少主程序中的冗余和杂乱

jog_test.cpp

#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include "myAds.hpp"

const int maxspeed = 10800;

AdsTalker::Ptr adsCtrl;

void cmd_callback(const geometry_msgs::Twist::ConstPtr &cmd_msg)
{
    cout << "cmd_msg->linear.x:" << cmd_msg->linear.x << endl;
    adsCtrl->jog_vel = maxspeed * abs(cmd_msg->linear.x);
    if (cmd_msg->linear.x > 0)
    {
        adsCtrl->jog_for = true;
        adsCtrl->jog_back = false;
    }
    else if (cmd_msg->linear.x < 0)
    {
        adsCtrl->jog_for = false;
        adsCtrl->jog_back = true;
    }
    else
    {
        adsCtrl->jog_for = false;
        adsCtrl->jog_back = false;
    }
}

int main(int argc, char *argv[])
{
    string netid = "169.254.254.142.1.1";
    string ipv4 = "169.254.254.142";
    adsCtrl = make_shared<AdsTalker>(netid, ipv4, AMSPORT_R0_PLC_TC3);

    if (adsCtrl->motor_enable)
    {
        cout << "Motor enabled!" << endl;
    }
    else
    {
        cout << "Motor disabled!" << endl;
        adsCtrl->motor_enable = !adsCtrl->motor_enable;
    }
    adsCtrl->jog_for = false;
    adsCtrl->jog_back = false;

    ros::init(argc, argv, "ads_jog");
    ros::NodeHandle nh;
    ros::Subscriber cmd_sub = nh.subscribe("/cmd_vel", 1, cmd_callback);

    ros::spin();

    return 0;
}

ptp_test.cpp

#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include "myAds.hpp"

AdsTalker::Ptr adsCtrl;

void cmd_callback(const geometry_msgs::Twist::ConstPtr &cmd_msg)
{
    adsCtrl->ptp_pos = cmd_msg->angular.z / M_PI * 2 * 360;
    adsCtrl->ptp_do = true;
    if(adsCtrl->ptp_over)
    {
        adsCtrl->ptp_do = false;
    }
}

int main(int argc, char *argv[])
{
    string netid = "169.254.254.142.1.1";
    string ipv4 = "169.254.254.142";
    adsCtrl = make_shared<AdsTalker>(netid, ipv4, AMSPORT_R0_PLC_TC3);

    if (adsCtrl->motor_enable)
    {
        cout << "Motor enabled!" << endl;
    }
    else
    {
        cout << "Motor disabled!" << endl;
        adsCtrl->motor_enable = !adsCtrl->motor_enable;
    }
    adsCtrl->ptp_vel = 180;

    ros::init(argc, argv, "ads_ptp");
    ros::NodeHandle nh;
    ros::Subscriber cmd_sub = nh.subscribe("/cmd_vel", 1, cmd_callback);

    ros::spin();

    return 0;
}