S型加减速程序，C++，PLC，AGV

做无人驾驶AGV项目，起动和停车冲击非常大，参考牛人的博文https://blog.csdn.net/Septembernine/article/details/53125828，写了一段S型加减速程序，也称抛物线加减速，七段加减速，实际应用效果不错，分享给大家，代码中有比较详细的注释。

图形

MFC++


// DLGDlg.cpp: 实现文件
//#include "pch.h"
#include "framework.h"
#include "DLG.h"
#include "DLGDlg.h"
#include "afxdialogex.h"#ifdef _DEBUG
#define new DEBUG_NEW
#endif// 对话框数据
#ifdef AFX_DESIGN_TIMEenum { IDD = IDD_ABOUTBOX };
#endifprotected:virtual void DoDataExchange(CDataExchange* pDX);    // DDX/DDV 支持// 实现
protected:DECLARE_MESSAGE_MAP()
};CAboutDlg::CAboutDlg() : CDialogEx(IDD_ABOUTBOX)
{
}void CAboutDlg::DoDataExchange(CDataExchange* pDX)
{CDialogEx::DoDataExchange(pDX);
}BEGIN_MESSAGE_MAP(CAboutDlg, CDialogEx)
END_MESSAGE_MAP()// CDLGDlg 对话框CDLGDlg::CDLGDlg(CWnd* pParent /*=nullptr*/): CDialogEx(IDD_DLG_DIALOG, pParent), m_fVelocity(600), m_nTimer(0)
{m_mtx = D2D1::Matrix3x2F::Scale(0.2f, -0.2f)  // 把中心移到左下角* D2D1::Matrix3x2F::Translation( 50, 380 )   // Y方向反过来;m_hIcon = AfxGetApp()->LoadIcon(IDR_MAINFRAME);
}void CDLGDlg::DoDataExchange(CDataExchange* pDX)
{CDialogEx::DoDataExchange(pDX);DDX_Text(pDX, IDC_EDIT1, m_fVelocity);DDX_Control(pDX, IDC_EDIT1, m_pEdit1);
}BEGIN_MESSAGE_MAP(CDLGDlg, CDialogEx)ON_WM_SYSCOMMAND()ON_WM_PAINT()ON_WM_QUERYDRAGICON()ON_BN_CLICKED(IDOK, &CDLGDlg::OnBnClickedOk)ON_BN_CLICKED(IDC_BUTTON1, &CDLGDlg::OnBnClickedButton1)ON_BN_CLICKED(IDC_BUTTON2, &CDLGDlg::OnBnClickedButton2)ON_WM_TIMER()
END_MESSAGE_MAP()// CDLGDlg 消息处理程序BOOL CDLGDlg::OnInitDialog()
{CDialogEx::OnInitDialog();// 将“关于...”菜单项添加到系统菜单中。// IDM_ABOUTBOX 必须在系统命令范围内。ASSERT((IDM_ABOUTBOX & 0xFFF0) == IDM_ABOUTBOX);ASSERT(IDM_ABOUTBOX < 0xF000);CMenu* pSysMenu = GetSystemMenu(FALSE);if (pSysMenu != nullptr){BOOL bNameValid;CString strAboutMenu;bNameValid = strAboutMenu.LoadString(IDS_ABOUTBOX);ASSERT(bNameValid);if (!strAboutMenu.IsEmpty()){pSysMenu->AppendMenu(MF_SEPARATOR);pSysMenu->AppendMenu(MF_STRING, IDM_ABOUTBOX, strAboutMenu);}}// 设置此对话框的图标。  当应用程序主窗口不是对话框时，框架将自动//  执行此操作SetIcon(m_hIcon, TRUE);            // 设置大图标SetIcon(m_hIcon, FALSE);        // 设置小图标//ShowWindow(SW_MAXIMIZE);//ShowWindow(SW_MINIMIZE);// TODO: 在此添加额外的初始化代码return TRUE;  // 除非将焦点设置到控件，否则返回 TRUE
}void CDLGDlg::OnSysCommand(UINT nID, LPARAM lParam)
{if ((nID & 0xFFF0) == IDM_ABOUTBOX){CAboutDlg dlgAbout;dlgAbout.DoModal();}else{CDialogEx::OnSysCommand(nID, lParam);}
}// 如果向对话框添加最小化按钮，则需要下面的代码
//  来绘制该图标。  对于使用文档/视图模型的 MFC 应用程序，
//  这将由框架自动完成。void CDLGDlg::OnPaint()
{if (IsIconic()){CPaintDC dc(this); // 用于绘制的设备上下文SendMessage(WM_ICONERASEBKGND, reinterpret_cast<WPARAM>(dc.GetSafeHdc()), 0);// 使图标在工作区矩形中居中int cxIcon = GetSystemMetrics(SM_CXICON);int cyIcon = GetSystemMetrics(SM_CYICON);CRect rect;GetClientRect(&rect);int x = (rect.Width() - cxIcon + 1) / 2;int y = (rect.Height() - cyIcon + 1) / 2;// 绘制图标dc.DrawIcon(x, y, m_hIcon);}else{CPaintDC dc(this); // 用于绘制的设备上下文// X坐标dc.MoveTo(transfrom(-50, 0));dc.LineTo(transfrom(2600, 0));// Y坐标dc.MoveTo(transfrom(0, -600));dc.LineTo(transfrom(0, 1600));// 设写速度dc.MoveTo(transfrom(0, m_fVelocity));dc.LineTo(transfrom(20, m_fVelocity));}
}//当用户拖动最小化窗口时系统调用此函数取得光标
//显示。
HCURSOR CDLGDlg::OnQueryDragIcon()
{return static_cast<HCURSOR>(m_hIcon);
}int nn = 0;
float b2 = 3;
float b3 = b2 * 1;
float bb = b2;
float b = bb;
float aa = 0;
float xx = 0;
float yy = 0;
float am = 50;// 设置速度
void CDLGDlg::OnBnClickedOk()
{UpdateData();m_pEdit1.SetFocus();m_pEdit1.SetSel(0xffff0000);// 设写速度CDC* pdc = GetDC();pdc->MoveTo(transfrom(xx, m_fVelocity));pdc->LineTo(transfrom(xx + 2000, m_fVelocity));pdc->MoveTo(transfrom(xx, m_fVelocity/2));pdc->LineTo(transfrom(xx + 2000, m_fVelocity/2));ReleaseDC(pdc);}// 坐标变换
CPoint CDLGDlg::transfrom(float x, float y)
{D2D1_POINT_2F p;p.x = x;p.y = y;D2D1_POINT_2F p1 = m_mtx.TransformPoint(p);return CPoint(p1.x, p1.y);
}// 启动/暂停
void CDLGDlg::OnBnClickedButton1()
{m_pEdit1.SetFocus();m_pEdit1.SetSel(0xffff0000);if(m_nTimer){KillTimer(m_nTimer);m_nTimer = 0;}elsem_nTimer = SetTimer(100, 300, 0);
}// 初始化
void CDLGDlg::OnBnClickedButton2()
{Invalidate();if (m_nTimer){KillTimer(m_nTimer);m_nTimer = 0;}xx = 0;yy = 0;aa = 0;b = b2;
}// 画图
void CDLGDlg::OnTimer(UINT_PTR nIDEvent)
{CDC* pdc = GetDC();// 上一点坐标float xx0 = xx, yy0 = yy, aa0 = aa;// 按当前加速度计算到加速度为0时的速度变化，加一个当前加速度，想当于4舍5入float sdv = m_fVelocity - yy;         // 设定速度变化量//if (sdv < 0) // 快速减速//{//    bb = b3;// b = bb;//}float aa1 = aa < 0 ? -aa : aa;           // 绝对值float aa2 = aa1 / 2;float cdv = aa * aa1 / bb / 2 + aa;        // 到抛物线顶点的距离，加一个当前加速度，想当于4舍5入if (aa != 0 || yy != m_fVelocity)  // 调整中{if (sdv >= -b3 && sdv <= b3 && aa >= -b3 && aa <= b3)    // 加速度和速度都小于5时，结束{aa = 0;yy = m_fVelocity;}else{if (cdv + aa2 < sdv)       // 计算顶点在设定值的上面，加速度加大b = bb;else if (cdv - aa2 > sdv) // 计算顶点在设定值的下面，加速度减小b = -bb;elseb = 0;                 // 计算顶点和设定值接近，加速度不变if (sdv > 0 && yy < 100)aa = b;else if ((b > 0 && aa < am) || (b < 0 && aa > -am)) // 加速度在正负50范围内aa += b;yy += aa; // 速度变化}}xx += 20;// 加速度曲线pdc->MoveTo(transfrom(xx0, aa0 * 5));pdc->LineTo(transfrom(xx, aa*5));// 速度曲线pdc->MoveTo(transfrom(xx0, yy0));pdc->LineTo(transfrom(xx, yy));ReleaseDC(pdc);CDialogEx::OnTimer(nIDEvent);
}

PLC功能块

(* 抛物线加减速 *)if a = 0.0 and v = sv THEN (* 调整结束 *)RETURN;
END_if;(* 设定速度与输出速度差值 *)
sdv := sv - v; (* 如果是减速，参数加大3倍，快速减 *)
IF sdv < 0.0 THENda2 := da * 2.0;ma2 := ma * 2.0;
ELSEda2 := da;ma2 := ma;
END_IF;(* 加速度的绝对值 *)
IF a < 0.0 THENab := -a;
ELSEab := a;
END_IF;(* 到抛物线顶点的距离，加一个当前加速度，想当于4舍5入*)
cdv := a * ab / da2 / 2.0 + a;(*   加速度和速度都小于5时，结束 *)
if sdv >= -da2 and sdv <= da2 and a >= -da2 and a <= da2 THENa := 0.0;v := sv;RETURN;
END_IF;(* 加加速或减加速 *)
if cdv + ab  /  2.0 < sdv then      (* 计算顶点在设定值的上面，加速度加大 *)sda := da2;
elsif  cdv - ab / 2.0 > sdv then         (*  计算顶点在设定值的下面，加速度减小 *)sda := -da2;
elsesda := 0.0;                    (* 计算顶点和设定值接近，加速度不变 *)
end_if;(* 新加速度 *)
IF sdv > 0.0 AND v < 30.0 THEN        (* 开始用均加速，减少冲击 *)a := da2;
elsif (sda > 0.0 and a < ma2) OR  (sda < 0.0 and a > -ma2) THEN     (* 加速度在正负50范围内 *)a := a + sda;
END_IF;(* 新速度 *)
v := v + a;       (* 速度变化 *) (* 速度范围 *)
IF v > mv THENv := mv;
ELSIF v < 0.0 THENv := 0.0;
END_IF;(* 急停后输出为0 *)
IF 前驱mm／s = 0.0 and sv = 0.0 THENv := 0.0;
END_IF;

S型加减速程序，C++，PLC，AGV相关推荐

STM32步进电机S型加减速算法
简单说明一下硬件资源,需要用到STM32两个定时器,TIM1产生PWM脉冲并对脉冲个数计数,TIM2开启定时中断用于算法的实现.采用CubeMX+Hal库配置,这里不做详细介绍,重点介绍S型加减速算法 ...
STM32-步进电机S型加减速控制
基于STM32的步进电机S型加减速控制算法 STM32简介 STM32代表ARM Cortex-M内核的32位微控制器.专为要求高性能.低成本.低功耗的嵌入式应用专门设计的: STM32系列的内核主要 ...
基于带约束S型加减速曲线的空间直线插补与空间圆弧插补算法（Matlab）
写在前面学习代码都记录在个人github上,欢迎关注~ Matlab机器人工具箱版本9.10 在前面的博文中: 基于抛物线过渡(梯形加减速)的空间直线插补算法与空间圆弧插补算法(Matlab) 基于 ...
七段S型加减速算法--多轴时间同步方案调研
七段S型加减速算法--时间同步基本概念问题提出参考资料参考一: 链接1 链接2 链接3 参考二实现方案后续基本概念 7段S型加减速算法(7 segments S-curve veloci ...
步进电机的S型加减速算法
电机能运行的速度远远大于启动速度(即最大匀速速度),那么怎么平稳的运行到最大速度就是S型加减速曲线的作用 1.Qt 1.1.S加减速的计算 static float Freq[10][1000]; s ...
步进电机s型加减速计算工具_21个有关伺服电机的问题想当工程师的你一定得知道...
工业机器人电动伺服系统的一般结构为三个闭环控制,即电流环.速度环和位置环.一般情况下,对于交流伺服驱动器,可通过对其内部功能参数进行人工设定而实现位置控制.速度控制.转矩控制等多种功能. 1.如何正确 ...
matlab拟合s型加减速曲线,运动控制系统s曲线加减速的实现方法
运动控制系统s曲线加减速的实现方法 [专利摘要]一种运动控制系统S曲线加减速的实现方法,1)S曲线加减速的实现条件:S曲线加减速控制过程的基本限定条件公式如公式(6):完整的S曲线加减速过程应满足公式 ...
CUBEMX教程—— STM32F407实现多步进电机型加减速全过程
cubemx配置图先配置定时器,选择定时器8,内部时钟源,通道1,2,3,4 时钟配置,查看手册可以知道TIM8连接在APB2,定时器8时钟频率为168M. 定时器8参数配置定时器8GPIO设置 ...
步进电机s型加减速计算工具_步进电机噪音和振动的原因分析及应对策略
不正确地驱动步进电机很容易导致电机发出"嗡嗡"的噪声和很大的振动. 当驱动步进电机时,如果发现步进电机处于静止状态时,其内部都发出很明显的噪音,有点类似线圈快速变化那种,一般是由于 ...

S型加减速程序，C++，PLC，AGV

S型加减速程序，C++，PLC，AGV相关推荐

最新文章

热门文章