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_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;