倍福：驱动电机两种控制方式对比

Prejudices

于 2024-06-06 10:35:17 发布

阅读量350

点赞数 4

分类专栏：倍福文章标签：倍福电机控制

本文链接：https://blog.csdn.net/Solititude/article/details/139493182

版权

倍福专栏收录该内容

10 篇文章 2 订阅

订阅专栏

驱动电机两种控制方式对比

在这里插入图片描述

验证用修改Override值控制电机转速的实际效果

规定反复启动Mc_MoveVelocity功能块为方法一，修改Override值为方法二

control_drive.cpp

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

AdsTalker::Ptr adsCtrl;
AidTool::Ptr aidTool;

#define MC_Positive_Direction 1
#define MC_Negative_Direction 3;
const double rotate_reduce = 100.0;
const double drive_reduce = 50.0;
const double tire_diameter = 0.155;
const double twincat_ratio = 360.0;
const double V_MAXSPEED = 0.2;

void cmd_callback(const geometry_msgs::Twist::ConstPtr &cmd_msg)
{
    //通过不断重新启动Mc_MOveVelocity功能块更改速度
    adsCtrl->drive_move_vel_val = (cmd_msg->linear.x / (M_PI * tire_diameter)) * rotate_reduce * twincat_ratio;
    adsCtrl->drive_move_vel_do = false;
    adsCtrl->drive_halt = false;
    cout << aidTool->toc() << " " << adsCtrl->drive_move_vel_val << " " << adsCtrl->drive_act_vel << endl; 
    if (adsCtrl->drive_move_vel_val > 0)
    {
        adsCtrl->drive_move_vel_direction = MC_Positive_Direction;
        adsCtrl->drive_move_vel_do = true;
    }
    else if (adsCtrl->drive_move_vel_val < 0)
    {
        adsCtrl->drive_move_vel_val = -adsCtrl->drive_move_vel_val;
        adsCtrl->drive_move_vel_direction = MC_Negative_Direction;
        adsCtrl->drive_move_vel_do = true;
    }
    else
    {
        adsCtrl->drive_halt = true;
        if (adsCtrl->drive_halt_over)
        {
            adsCtrl->drive_halt = false;
        }
    }

    //控制速度比率从而控制电机速度
    // adsCtrl->drive_move_vel_do = false;
    // double velocity_ratio = cmd_msg->linear.x / V_MAXSPEED;
    // if (cmd_msg->linear.x < 0)
    // {
    //     adsCtrl->drive_move_vel_direction = MC_Negative_Direction;
    //     adsCtrl->drive_move_vel_do = true;
    //     adsCtrl->drive_vel_ratio = - velocity_ratio * 100;
    // }
    // else if (cmd_msg->linear.x > 0)
    // {
    //     adsCtrl->drive_move_vel_direction = MC_Positive_Direction;
    //     adsCtrl->drive_move_vel_do = true;
    //     adsCtrl->drive_vel_ratio = velocity_ratio * 100;
    // }
    // else
    // {
    //     adsCtrl->drive_move_vel_ratio = 0.0;
    //     adsCtrl->drive_halt = true;
    //     if (adsCtrl->drive_halt_over)
    //     {
    //         adsCtrl->drive_halt = false;
    //     }
    // }
    // cout << aidTool->toc() << " " << adsCtrl->drive_move_vel_val * velocity_ratio << " " << adsCtrl->drive_act_vel << endl; 
}

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);
    aidTool = make_shared<AidTool>();

    if (adsCtrl->motor_enable)
    {
        cout << "Motor enabled!" << endl;
    }
    else
    {
        cout << "Motor disabled!" << endl;
        adsCtrl->motor_enable = !adsCtrl->motor_enable;
    }
    // adsCtrl->drive_move_vel_do = true;
    // adsCtrl->drive_vel_ratio = 0.0;
    // adsCtrl->drive_move_vel_val = (V_MAXSPEED / (M_PI * tire_diameter)) * rotate_reduce * twincat_ratio;

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

    ros::spin();

    return 0;
}

在AidTool里添加了记录程序运行时间的功能，也可以用作时间戳，这里是借鉴了**A-LOAM**

#ifndef MY_TOOLS_H
#define MY_TOOLS_H

#include <stdio.h>
#include <stdlib.h>
#include <iostream>
#include <time.h>
#include <string.h>
#include <memory>
#include <chrono>

using namespace std;

class AidTool
{
private:
    chrono::time_point<chrono::system_clock> start, end;
public:
    typedef shared_ptr<AidTool> Ptr;

    AidTool(){};

    char *getNowTime(void)
    {
        time_t timer;
        struct tm *now_time;
        timer = time(NULL);
        now_time = localtime(&timer);
        return asctime(now_time);
    }

    char *getPreciseTime(void)
    {
        static char timestr[200] = {0};
        struct tm *pTempTm;
        struct timeval time;

        gettimeofday(&time, NULL);
        pTempTm = localtime(&time.tv_sec);
        if (pTempTm != NULL)
        {
            snprintf(timestr, 199, "%04d-%02d-%02d %02d:%02d:%02d.%03ld",
                     pTempTm->tm_year + 1900, pTempTm->tm_mon + 1,
                     pTempTm->tm_mday, pTempTm->tm_hour, pTempTm->tm_min,
                     pTempTm->tm_sec, time.tv_usec / 1000);
        }
        return timestr;
    }

    void tic()
    {
        start = chrono::system_clock::now();
    }

    double toc()
    {
        end = chrono::system_clock::now();
        chrono::duration<double> elapsed_seconds = end - start;
        return elapsed_seconds.count() * 1000;
    }

    ~AidTool(){};
};

#endif

记录分别在下面几个txt，02效果不太好，没有用

2022-10-19-moorn01.txt（beckhoff_drive_mod1_00.csv）
2022-10-19-moorn02.txt
2022-10-19-moorn03.txt（beckhoff_drive_mod2_00.csv）

可以在Excel中绘制，但效果不太好，上下分别为方法一和二

在这里插入图片描述

所以在MATLAB里绘图，beckhoff_drivemod_comp.m

time除1000是因为log里的单位是ms，绘图时会比较大，以s为单位更合适

data1 = csvread('beckhoff_drive_mod1_00.csv',2,0);
time1 = data1(:,1) / 1000.0;
target_velocity1 = data1(:,2);
actual_velocity1 = data1(:,3);

figure(1);
plot(time1,target_velocity1,'g');
xlabel('时间/s');
ylabel('转速/度/s');
title('电机控制方式一');
grid on;
xlim([0,35]);
ylim([-15000,13000]);
set(gca,'xtick',0:2.5:35);
set(gca,'ytick',-15000:2000:13000);
hold on;
plot(time1,actual_velocity1,'r');
legend('目标转速','实际转速');
hold on;

data2 = csvread('beckhoff_drive_mod2_00.csv',2,0);
time2 = data2(:,1) / 1000.0;
target_velocity2 = data2(:,2);
actual_velocity2 = data2(:,3);

figure(2);
plot(time2,target_velocity2,'g');
xlabel('时间/s');
ylabel('转速/度/s');
title('电机控制方式二');
grid on;
xlim([0,30]);
ylim([-15000,15000]);
set(gca,'xtick',0:2.5:30);
set(gca,'ytick',-15000:2000:15000);
hold on;
plot(time2,actual_velocity2,'r');
legend('目标转速','实际转速');
hold on;

效果如下，上下分别为方法一和二

在这里插入图片描述

两种方法对目标速度的跟随都比较好，方法一更利于实现转向速度的补偿，因此仍采用方法一控制驱动电机

当前的独轮车控制代码，control_test.cpp

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

AdsTalker::Ptr adsCtrl;
AidTool::Ptr aidTool;

#define MC_Positive_Direction 1
#define MC_Negative_Direction 3;
const double rotate_reduce = 100.0;
const double drive_reduce = 50.0;
const double tire_diameter = 0.155;
const double twincat_ratio = 360.0;

mutex mut;

bool rotateHome()
{
    lock_guard<mutex> lock(mut);
    adsCtrl->home_vel = 0.05 * twincat_ratio * rotate_reduce;

    int count = 0;
    while (!adsCtrl->couple_over && count <= 10)
    {
        adsCtrl->home_run = false;
        adsCtrl->couple_do = false;
        ++count;
        adsCtrl->couple_do = true;
    }
    adsCtrl->decouple_do = false;

    adsCtrl->home_do = true;
    while (!adsCtrl->rotate_act_vel)
    {
        adsCtrl->home_run = false;
        adsCtrl->home_run = true;
        usleep(1000);
    }

    while (!adsCtrl->home_over)
    {
        cout << "Tire is homing..." << endl;
    }
    if (adsCtrl->home_over)
    {
        cout << "Tire succeeds to back home!" << endl;
        adsCtrl->home_do = false;
        return true;
    }
    mut.unlock();
    return false;
}

void cmd_callback(const geometry_msgs::Twist::ConstPtr &cmd_msg)
{
    //判断是否为回零命令
    if (cmd_msg->linear.z == -1.0)
    {
        if (abs(adsCtrl->rotate_act_pos) > 10.0)
        {
            if (!rotateHome())
            {
                ROS_ERROR("Tire fails to back home!");
                return;
            }
        }
    }

    lock_guard<mutex> lock(mut);
    adsCtrl->ptpRel_pos = (cmd_msg->angular.z / (M_PI * 2)) * twincat_ratio * rotate_reduce;
    adsCtrl->drive_move_vel_val = (cmd_msg->linear.x / (M_PI * tire_diameter)) * rotate_reduce * twincat_ratio;
    //转向电机控制
    adsCtrl->ptpRel_do = false;
    if (adsCtrl->ptpRel_pos != 0)
    {
        adsCtrl->ptpRel_do = true;
    }
    if (adsCtrl->ptpRel_over)
    {
        adsCtrl->ptpRel_do = false;
    }

    //驱动电机控制
    adsCtrl->drive_move_vel_do = false;
    adsCtrl->drive_halt = false;
    adsCtrl->rotate_statusCheck = false;
    adsCtrl->rotate_statusCheck = true;
    if (fabs(adsCtrl->rotate_act_vel) > 60)
    {
        if (!adsCtrl->rotate_isDecelerating)
        {
            adsCtrl->drive_move_vel_val = adsCtrl->drive_move_vel_val + adsCtrl->rotate_act_vel / 2;
        }
    }
    cout << aidTool->getPreciseTime() << "  drive target velocity:" << adsCtrl->drive_move_vel_val << "; drive actual velocity:" << adsCtrl->drive_act_vel << "; rotate actual velocity:" << adsCtrl->rotate_act_vel << endl;
    if (adsCtrl->drive_move_vel_val > 0)
    {
        adsCtrl->drive_move_vel_direction = MC_Positive_Direction;
        adsCtrl->drive_move_vel_do = true;
    }
    else if (adsCtrl->drive_move_vel_val < 0)
    {
        adsCtrl->drive_move_vel_val = -adsCtrl->drive_move_vel_val;
        adsCtrl->drive_move_vel_direction = MC_Negative_Direction;
        adsCtrl->drive_move_vel_do = true;
    }
    else
    {
        adsCtrl->drive_halt = true;
        if (adsCtrl->drive_halt_over)
        {
            adsCtrl->drive_halt = false;
        }
    }
    mut.unlock();
}

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);
    aidTool = make_shared<AidTool>();

    if (adsCtrl->motor_enable)
    {
        cout << "Motor enabled!" << endl;
    }
    else
    {
        cout << "Motor disabled!" << endl;
        adsCtrl->motor_enable = !adsCtrl->motor_enable;
    }
    adsCtrl->ptp_vel = 0.1 * twincat_ratio * rotate_reduce;
    adsCtrl->drive_vel_ratio = 100.0;

    if (!rotateHome())
    {
        ROS_ERROR("Tire fails to back home!");
        return -1;
    }

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

    ros::spin();

    return 0;
}