倍福控制电机耦合运动
电机耦合运动是为了解除车轮机械结构造成的耦合,有点用魔法打败魔法的意思
在倍福里主要用到两个功能块: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等运动