移动机器人基本运动控制的源代码

C++源代码 功能为移动机器人基本运动控制的源代码

// BehBallTrack.cpp: implementation of the CBehBallTrack class.
//
//////////////////////////////////////////////////////////////////////

#include “stdafx.h“
#include “BehBallTrack.h“

#ifdef _DEBUG
#undef THIS_FILE
static char THIS_FILE[]=__FILE__;
#define new DEBUG_NEW
#endif

//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////

CBehBallTrack::CBehBallTrack（）
{
	m_BehShow = NULL;

	//初始化前置球
	m_ball.ReSet（）;
	m_ball.Ven = TRUE;
	m_ball.Vmin = 250;
	m_ball.Vmax = 700;

	//初始化前置pid
	m_fpid.DeadlineMinus = -3;
	m_fpid.DeadlinePlus = 3;
	m_fpid.IMin = -10000;
	m_fpid.IMax = 10000;
	m_fpid.Kp = 100;
	m_fpid.Ki = 20;
	m_fpid.Kd = 0;

	m_fpid2.DeadlineMinus = -3;
	m_fpid2.DeadlinePlus = 3;

	m_fpid2.Kp = 200;

	m_fpid2.resMax = 400;
	m_fpid2.resMin = -50;
	
	m_cv.SetParam（&m_ball）;

	//初始化全景球
	m_ball2.ReSet（）;
	m_ball2.Ven = TRUE;
	m_ball2.Vmin = 100;
	m_ball2.Vmax = 500;
	
	//初始化全景pid
	m_opid.DeadlineMinus = -3;
	m_opid.DeadlinePlus = 3;
	m_opid.IMin = -1000000;
	m_opid.IMax = 1000000;
	m_opid.Kp = 6000;
	m_opid.Ki = 500;
	m_opid.Kd = 0;
	m_opid.resMax = 3000;
	m_opid.resMin = -3000;

	m_opid2.DeadlineMinus = -1000;
	m_opid2.DeadlinePlus = 10;

	m_opid2.Kp = 10000;

	m_opid2.resMax = 3000;
	m_opid2.resMin = -100;

	m_sv.SetParam（&m_ball2）;

	m_Primary = 0;
}

CBehBallTrack::~CBehBallTrack（）
{

}

void CBehBallTrack::AfterUpdateOverlook（BYTE *pBuffer long lWidth long lHeight double dbTime UINT nState）
{
	if （m_Primary == 1）
	{
		return;
	}
	
	//导入视频进行处理
	m_sv.ImageBuf（lWidthlHeightpBuffer）;
	m_sv.SetCenter（160120）;
	m_sv.Process（1）;

	//判断目标有效坐标
	if （m_ball2.Yen == true）
	{
		Ballx2 = m_ball2.Yx;
		Bally2 = m_ball2.Yy;
		Ballpix2 = m_ball2.Ysum;
	}

	if （m_ball2.Uen == true）
	{
		Ballx2 = m_ball2.Ux;
		Bally2 = m_ball2.Uy;
		Ballpix2 = m_ball2.Usum;
	}

	if （m_ball2.Ven == true）
	{
		Ballx2 = m_ball2.Vx;
		Bally2 = m_ball2.Vy;
		Ballpix2 = m_ball2.Vsum;
	}

	if（Ballpix2 < 1）
	{
		return;
	}

	//计算转向速度
	int terror = （Ballx2 - 160）*100/Bally2;
	int xspeed = m_opid.Calc（terror0）;

	//计算纵向速度
	int yspeed = m_opid2.Calc（Bally2 - 1200）;
	m_pCmd->SetBothMotorsSpeed（yspeed - xspeedyspeed + xspeed）;

	m_strShow.Format（“（%d%d）	l:%d	r:%d	（%d）“Ballx2Bally2yspeed - xspeedyspeed + xspeedm_opid.m_nIntegral）;
	output（&m_strShow）;
}

void CBehBallTrack::AfterUpdateVideoSample（BYTE *pBuffer long lWidth long lHeight double dbTime UINT nState）
{
	//摄像头图像祯更新
	m_cv.ImageBuf（lWidthlHeightpBuffer）;
	m_cv.FindSplash（TRUE）;

	if （m_ball.Yen == false && m_ball.Uen == false && m_ball.Ven == false）
	{
		m_pCmd->Brake（1）;
		return;
	}

	//判断目标有效坐标
	if （m_ball.Yen == true）
	{
		Ballx = m_ball.Yx;
		Bally = m_ball.Yy;
		Ballpix = m_ball.Ysum;
	}

	if （m_ball.Uen == true）
	{
		Ballx = m_ball.Ux;

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----

     文件        116  2007-06-18 17:39  Voyager IIA 基本功能DEMO\aa.voy

     文件       4142  2007-08-31 08:59  Voyager IIA 基本功能DEMO\BehBallTrack.cpp

     文件       1252  2007-07-20 11:54  Voyager IIA 基本功能DEMO\BehBallTrack.h

     文件      15749  2006-09-24 22:48  Voyager IIA 基本功能DEMO\CaptureVideo.cpp

     文件       5778  2006-09-22 13:55  Voyager IIA 基本功能DEMO\CaptureVideo.h

     文件       4269  2007-07-09 12:34  Voyager IIA 基本功能DEMO\Catching.cpp

     文件        753  2006-08-30 09:10  Voyager IIA 基本功能DEMO\Catching.h

     文件       2049  2007-07-19 13:55  Voyager IIA 基本功能DEMO\Compasee.cpp

     文件       1347  2007-07-19 13:52  Voyager IIA 基本功能DEMO\Compasee.h

     文件       2720  2007-06-22 11:50  Voyager IIA 基本功能DEMO\CradleDlg.cpp

     文件       1478  2007-06-08 09:31  Voyager IIA 基本功能DEMO\CradleDlg.h

     文件        120  2007-08-04 15:19  Voyager IIA 基本功能DEMO\Defalt.voy

     文件       5814  2009-10-28 17:03  Voyager IIA 基本功能DEMO\Demo.cpp

     文件       1303  2007-06-22 09:13  Voyager IIA 基本功能DEMO\Demo.h

     文件       4176  2007-05-09 10:04  Voyager IIA 基本功能DEMO\Finding.cpp

     文件        748  2007-05-09 10:04  Voyager IIA 基本功能DEMO\Finding.h

     文件       1467  2007-07-19 13:48  Voyager IIA 基本功能DEMO\IBehavior.h

     文件       1616  2007-07-13 11:28  Voyager IIA 基本功能DEMO\IntPID.cpp

     文件        807  2007-07-12 14:18  Voyager IIA 基本功能DEMO\IntPID.h

     文件        749  2006-09-11 15:29  Voyager IIA 基本功能DEMO\IPhy.h

     文件      10565  2007-01-13 18:48  Voyager IIA 基本功能DEMO\NetCmt.cpp

     文件       1764  2006-09-22 12:33  Voyager IIA 基本功能DEMO\NetCmt.h

     文件       1622  2007-02-09 13:42  Voyager IIA 基本功能DEMO\NetSend.cpp

     文件        727  2006-09-10 20:01  Voyager IIA 基本功能DEMO\NetSend.h

     文件       6903  2006-09-09 23:30  Voyager IIA 基本功能DEMO\PieSensor.cpp

     文件       1857  2006-09-01 11:05  Voyager IIA 基本功能DEMO\PieSensor.h

     文件       3615  2006-08-10 09:51  Voyager IIA 基本功能DEMO\ReadMe.txt

     文件        120  2009-11-02 18:25  Voyager IIA 基本功能DEMO\Release\Defalt.voy

     文件     811008  2007-08-31 08:59  Voyager IIA 基本功能DEMO\Release\Voyager 1.14.exe

     文件     480054  2006-09-09 23:29  Voyager IIA 基本功能DEMO\res\bk.bmp

............此处省略50个文件信息