倍福控制电机耦合运动

倍福控制电机耦合运动

电机耦合运动是为了解除车轮机械结构造成的耦合，有点用魔法打败魔法的意思

在倍福里主要用到两个功能块：MC_GearIn和MC_GearOut

这里Master是转向电机，Slave是驱动电机，设置好合适的Ratio即可，其余参数为默认值

驱动电机的耦合跟随如果不decouple的话，会一直处于耦合状态，无法执行后续的Jog运动

Linux耦合控制

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

AdsTalker::Ptr adsCtrl;

const int maxspeed = 10800;

void cmd_callback(const geometry_msgs::Twist::ConstPtr &cmd_msg)
{
    adsCtrl->ptp_pos = cmd_msg->angular.z / M_PI * 2 * 360 * 5;
    double diff = fabs(adsCtrl->ptp_pos - adsCtrl->ptp_act_pos);
    if (diff > 5)
    {
        int count = 0;
        while(!adsCtrl->couple_over && count <= 30)
        {
            adsCtrl->ptp_do = false;
            adsCtrl->couple_do = false;
            ++count;
            adsCtrl->couple_do = true; 
        }
        adsCtrl->decouple_do = false;
        adsCtrl->ptp_do = true;
    }

    if (adsCtrl->ptp_over)
    {
        adsCtrl->ptp_do = false;
        adsCtrl->decouple_do = true;
        adsCtrl->couple_do = false;
    }

    adsCtrl->jog_vel = maxspeed * cmd_msg->linear.x;
    if (adsCtrl->jog_vel > 0)
    {
        adsCtrl->jog_vel = abs(adsCtrl->jog_vel);
        adsCtrl->jog_for = true;
        adsCtrl->jog_back = false;
    }
    else if (adsCtrl->jog_vel < 0)
    {
        adsCtrl->jog_vel = abs(adsCtrl->jog_vel);
        adsCtrl->jog_for = false;
        adsCtrl->jog_back = true;
    }
    else
    {
        adsCtrl->jog_vel = 0.0;
        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->ptp_vel = 360;
    adsCtrl->jog_for = false;
    adsCtrl->jog_back = false;
    adsCtrl->ptp_do = false;
    adsCtrl->couple_do = false;
    adsCtrl->decouple_do = false;

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

    ros::spin();

    return 0;
}

• main主要是通信的建立以及一些控制变量的初始化，控制逻辑与实现均在回调函数中完成
• 车轮优先执行转向动作，转向电机工作前先将驱动电机耦合，由于PLC内部周期的问题，可能会发生耦合失败的现象，因此重复执行耦合命令，设定阀值30次，直至成功将驱动电机耦合
• 转向完成后驱动电机解除耦合，开始正常工作，执行Jog等运动