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;