做无人驾驶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_TIME
enum { IDD = IDD_ABOUTBOX };
#endif
protected:
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;
}
else
m_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;
else
b = 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 THEN
da2 := da * 2.0;
ma2 := ma * 2.0;
ELSE
da2 := da;
ma2 := ma;
END_IF;
(* 加速度的绝对值 *)
IF a < 0.0 THEN
ab := -a;
ELSE
ab := 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 THEN
a := 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;
else
sda := 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 THEN
v := mv;
ELSIF v < 0.0 THEN
v := 0.0;
END_IF;
(* 急停后输出为0 *)
IF 前驱mm/s = 0.0 and sv = 0.0 THEN
v := 0.0;
END_IF;