visionview-1.cpp
来自「FIRA 3对3 机器人足球PC端控制程序。」· C++ 代码 · 共 2,053 行 · 第 1/5 页
CPP
2,053 行
//////////////// ROBOT SOCCER PROGRAM //////////////////
//////////////// By Kim Heung Soo //////////////////
//////////////// MIRO 4. 99. 4. 21. //////////////////
// VisionView.cpp : implementation of the CVisionView class
//
#include "stdafx.h"
#include "Vision.h"
#include "VisionDoc.h"
#include "VisionView.h"
#include <math.h>
#include <mmsystem.h>
#include "SettingColor.h"
#include "SetBoundary.h"
#include "SetPixelSize.h"
#include <winsvc.h>
#include "TVicHW32.h"
#include "Comm.h"
#include "Robot.h"
#include "Miro99.h"
PVicHW32Descriptor HW32 = NULL;
#define ZOOM_SCALE 3
#define ZOOM_OFFX 20
#define ZOOM_OFFY 10
#define ZOOM_SIZE 64
#define ZOOM_HALFSIZE 32
#define PARTIAL_IMAGE_SIZE 80//64 // dependent on robot size and its speed !!!!
#define M_PI 3.1415926535897932
#define DELTA 0.00000000001
#define VRAM_ADDRESS 0XE6000000 // dependent on video display adapter
#define DEPTH_PER_PIX 1 // the number of bytes per pixel on screen
#define DISPLAY_RES_X 1024 // display resolution
#define DISPLAY_RES_Y 768
#define BUF_OFFSET_1 DISPLAY_RES_X
#define BUF_OFFSET_2 (DISPLAY_RES_X-DispSizeX)
#define BUF_OFFSET_3 (DISPLAY_RES_X-PARTIAL_IMAGE_SIZE)
CSettingColor* ptrSetColor = new CSettingColor;
CSetBoundary* ptrSetBoundary = new CSetBoundary;
double PositionOfHomeRobot[5][2], PositionOfOpponent[5][2], PositionOfBall[2];
double curr_raw_PositionOfHomeRobot[5][2], curr_raw_PositionOfBall[2], curr_raw_AngleOfHomeRobot[5];
double past_raw_PositionOfHomeRobot[5][2], past_raw_PositionOfBall[2], past_raw_AngleOfHomeRobot[5];
double PredictionOfBall[2];
double PredictionOfBall1[2];
int up, down;
short AngleOfHomeRobot[5];
BOOL FlagHomeRobotFound[5], FlagBallFound, FlagOpponentFound[5];
char LUTcolor[256*256*256];
double ballestimate[2];
double viewvel[3];
double viewtheta;
double angle;
double dist_e;
int field1, field2, field3;
double ball_position_x, ball_position_y;
double robot_position_x, robot_position_y;
double robot_angle;
double pre_position_x, pre_position_y;
double x_up1, y1_up;
double x_down1, y1_down;
double x_center, y_center;
double x_center1, y_center1;
double Test_PositionOfBall[2];
int Ball_Position ;
BYTE command[6]; //for serial communciation
////////////////////////////////////////////////////////////////
int nNormalGame = 0;
int Home1;
int Home2;
int PreBall;
double preball_x, preball_y;
#ifdef _DEBUG
#define new DEBUG_NEW
#undef THIS_FILE
static char THIS_FILE[] = __FILE__;
#endif
/////////////////////////////////////////////////////////////////////////////
// CVisionView
IMPLEMENT_DYNCREATE(CVisionView, CFormView)
BEGIN_MESSAGE_MAP(CVisionView, CFormView)
//{{AFX_MSG_MAP(CVisionView)
ON_BN_CLICKED(IDC_SET_COLOR, OnSetColor)
ON_WM_LBUTTONDOWN()
ON_WM_MOUSEMOVE()
ON_BN_CLICKED(IDC_SET_BOUNDARY, OnSetBoundary)
ON_BN_CLICKED(IDC_LOAD_CONFIGURATION, OnLoadConfiguration)
ON_BN_CLICKED(IDC_SAVE_CONFIGURATION, OnSaveConfiguration)
ON_BN_CLICKED(IDC_CHANGE_COLOR, OnChangeColor)
ON_BN_CLICKED(IDC_SET_PIXEL_SIZE, OnSetPixelSize)
ON_BN_CLICKED(IDC_SET_ROBOT_SIZE, OnSetRobotSize)
ON_BN_CLICKED(IDC_FLAG_FIND_BALL, OnFlagFindBall)
ON_BN_CLICKED(IDC_FLAG_FIND_HGOALIE, OnFlagFindHgoalie)
ON_BN_CLICKED(IDC_FLAG_FIND_HOME1, OnFlagFindHome1)
ON_BN_CLICKED(IDC_FLAG_FIND_HOME2, OnFlagFindHome2)
ON_BN_CLICKED(IDC_FLAG_FIND_HOME3, OnFlagFindHome3)
ON_BN_CLICKED(IDC_FLAG_FIND_HOME4, OnFlagFindHome4)
ON_BN_CLICKED(IDC_COM1, OnCom1)
ON_BN_CLICKED(IDC_COM2, OnCom2)
ON_BN_CLICKED(IDC_LEFT_HOME, OnLeftHome)
ON_BN_CLICKED(IDC_RIGHT_HOME, OnRightHome)
ON_BN_CLICKED(IDC_KICK_OFF, OnKickOff)
ON_BN_CLICKED(IDC_PENALITY_KICK, OnPenalityKick)
ON_BN_CLICKED(IDC_FREE_KICK, OnFreeKick)
ON_BN_CLICKED(IDC_FREE_BALL, OnFreeBall)
ON_BN_CLICKED(IDC_GOALIE_KICK, OnGoalieKick)
ON_BN_CLICKED(IDC_DEFENSE, OnDefense)
ON_BN_CLICKED(IDC_READY_BUTTON, OnReady)
ON_BN_CLICKED(IDC_STOP_GAME_BUTTON, OnStopGame)
ON_BN_CLICKED(IDC_DRAW_CHECK, OnFlagDraw)
ON_BN_CLICKED(IDC_START_GAME_BUTTON, OnStartGame)
ON_BN_CLICKED(IDC_LOAD_VXD_BUTTON, OnLoadVxd)
ON_BN_CLICKED(IDC_CONTINUOUS_GRAB_BUTTON, OnContinuousGrab)
ON_BN_CLICKED(IDC_POS_BALL_RADIO, OnPosBallRadio)
ON_BN_CLICKED(IDC_POS_HGOALIE_RADIO, OnPosHgoalieRadio)
ON_BN_CLICKED(IDC_POS_HOME1_RADIO, OnPosHome1Radio)
ON_BN_CLICKED(IDC_POS_HOME2_RADIO, OnPosHome2Radio)
ON_BN_CLICKED(IDC_POS_HOME3_RADIO, OnPosHome3Radio)
ON_BN_CLICKED(IDC_POS_HOME4_RADIO, OnPosHome4Radio)
ON_BN_CLICKED(IDC_ENABLE_INIT_POS_CHECK, OnEnableInitPosCheck)
ON_BN_CLICKED(IDC_FLAG_FIND_OPPONENT1, OnFlagFindOpponent1)
ON_BN_CLICKED(IDC_FLAG_FIND_OPPONENT2, OnFlagFindOpponent2)
ON_BN_CLICKED(IDC_FLAG_FIND_OPPONENT3, OnFlagFindOpponent3)
ON_BN_CLICKED(IDC_FLAG_FIND_OPPONENT4, OnFlagFindOpponent4)
ON_BN_CLICKED(IDC_FLAG_FIND_OPPONENT5, OnFlagFindOpponent5)
ON_BN_CLICKED(IDC_POS_OPPO1_RADIO, OnPosOppo1Radio)
ON_BN_CLICKED(IDC_POS_OPPO2_RADIO, OnPosOppo2Radio)
ON_BN_CLICKED(IDC_POS_OPPO3_RADIO, OnPosOppo3Radio)
ON_BN_CLICKED(IDC_POS_OPPO4_RADIO, OnPosOppo4Radio)
ON_BN_CLICKED(IDC_POS_OPPO5_RADIO, OnPosOppo5Radio)
ON_BN_CLICKED(IDC_ROBOT_STOP, OnRobotStop)
ON_BN_CLICKED(IDC_KICK_OFF2, OnKickOff2)
ON_BN_CLICKED(IDC_PENALITY_KICK2, OnPenalityKick2)
ON_BN_CLICKED(IDC_FREE_KICK2, OnFreeKick2)
ON_BN_CLICKED(IDC_GOALIE_KICK2, OnGoalieKick2)
ON_BN_CLICKED(IDC_DEFENSE2, OnDefense2)
ON_BN_CLICKED(IDC_FREE_BALL2_1, OnFreeBall2_1)
ON_BN_CLICKED(IDC_FREE_BALL2_2, OnFreeBall2_2)
ON_BN_CLICKED(IDC_FREE_BALL2_3, OnFreeBall2_3)
ON_BN_CLICKED(IDC_FREE_BALL2_4, OnFreeBall2_4)
ON_BN_CLICKED(IDC_DEMO, OnDemo)
//}}AFX_MSG_MAP
END_MESSAGE_MAP()
/////////////////////////////////////////////////////////////////////////////
// CVisionView construction/destruction
CVisionView::CVisionView()
: CFormView(CVisionView::IDD)
{
//{{AFX_DATA_INIT(CVisionView)
m_bFlagFindBall = FALSE;
m_bFlagFindHgoalie = FALSE;
m_bFlagFindHome1 = FALSE;
m_bFlagFindHome2 = FALSE;
m_bFlagFindHome3 = FALSE;
m_bFlagFindHome4 = FALSE;
m_nGame = -1;
m_nSetting = -1;
m_bFlagDraw = FALSE;
m_sInfo = _T("");
m_bEnableInitPos = FALSE;
m_nLeftHome = -1;
m_bFlagFindOpponent1 = FALSE;
m_bFlagFindOpponent2 = FALSE;
m_bFlagFindOpponent3 = FALSE;
m_bFlagFindOpponent4 = FALSE;
m_bFlagFindOpponent5 = FALSE;
//}}AFX_DATA_INIT
// TODO: add construction code here
bSetColor = FALSE;
bBeginZoom = FALSE;
bEndZoom = FALSE;
bSetRobotSize = FALSE;
SelectedRectInZoom = CRect(0, 0, 0, 0);
clientRect.left = clientRect.top = clientRect.right = clientRect.bottom = 0;
PointPhys = NULL;
bPlay = FALSE;
bFlagSendCommand = FALSE;
nInitPosBall = BALL;
m_nLeftHome = 0;
nNormalGame = 0; //////////////////////////////////////////
}
CVisionView::~CVisionView()
{
}
void CVisionView::DoDataExchange(CDataExchange* pDX)
{
CFormView::DoDataExchange(pDX);
//{{AFX_DATA_MAP(CVisionView)
DDX_Check(pDX, IDC_FLAG_FIND_BALL, m_bFlagFindBall);
DDX_Check(pDX, IDC_FLAG_FIND_HGOALIE, m_bFlagFindHgoalie);
DDX_Check(pDX, IDC_FLAG_FIND_HOME1, m_bFlagFindHome1);
DDX_Check(pDX, IDC_FLAG_FIND_HOME2, m_bFlagFindHome2);
DDX_Check(pDX, IDC_FLAG_FIND_HOME3, m_bFlagFindHome3);
DDX_Check(pDX, IDC_FLAG_FIND_HOME4, m_bFlagFindHome4);
DDX_Check(pDX, IDC_COM1,m_bPort1);
DDX_Radio(pDX, IDC_KICK_OFF, m_nGame);
DDX_Check(pDX, IDC_DRAW_CHECK, m_bFlagDraw);
DDX_Text(pDX, IDC_INFO_EDIT, m_sInfo);
DDX_Check(pDX, IDC_ENABLE_INIT_POS_CHECK, m_bEnableInitPos);
DDX_Radio(pDX, IDC_LEFT_HOME, m_nLeftHome);
DDX_Check(pDX, IDC_FLAG_FIND_OPPONENT1, m_bFlagFindOpponent1);
DDX_Check(pDX, IDC_FLAG_FIND_OPPONENT2, m_bFlagFindOpponent2);
DDX_Check(pDX, IDC_FLAG_FIND_OPPONENT3, m_bFlagFindOpponent3);
DDX_Check(pDX, IDC_FLAG_FIND_OPPONENT4, m_bFlagFindOpponent4);
DDX_Check(pDX, IDC_FLAG_FIND_OPPONENT5, m_bFlagFindOpponent5);
//}}AFX_DATA_MAP
}
BOOL CVisionView::PreCreateWindow(CREATESTRUCT& cs)
{
// TODO: Modify the Window class or styles here by modifying
// the CREATESTRUCT cs
return CFormView::PreCreateWindow(cs);
}
/////////////////////////////////////////////////////////////////////////////
// CVisionView diagnostics
#ifdef _DEBUG
void CVisionView::AssertValid() const
{
CFormView::AssertValid();
}
void CVisionView::Dump(CDumpContext& dc) const
{
CFormView::Dump(dc);
}
CVisionDoc* CVisionView::GetDocument() // non-debug version is inline
{
ASSERT(m_pDocument->IsKindOf(RUNTIME_CLASS(CVisionDoc)));
return (CVisionDoc*)m_pDocument;
}
#endif //_DEBUG
/////////////////////////////////////////////////////////////////////////////
// CVisionView message handlers
// 橇饭烙 弊贰滚 包访 檬扁拳
void CVisionView::OnInitialUpdate()
{
CFormView::OnInitialUpdate();
// TODO: Add your specialized code here and/or call the base class
long i;
///////////////////////////////////////////////////////////////////////////////////
// MIL: Code that will be executed when a view is first attached to the document //
///////////////////////////////////////////////////////////////////////////////////
// Get ID's of image buffer, digitizer, display window size.
MilImageBufferId = ((CVisionDoc*)GetDocument())->MilImage;
MilDigitizerId = ((CVisionApp*)AfxGetApp())->MilDigitizer;
NumberOfDigitizer = ((CVisionApp*)AfxGetApp())->NumberOfDigitizer;
DispSizeX = ((CVisionApp*)AfxGetApp())->SizeX;
DispSizeY = ((CVisionApp*)AfxGetApp())->SizeY;
MdigReference(MilDigitizerId, M_HUE_REF, 128);
MdigReference(MilDigitizerId, M_SATURATION_REF, 128);
MdigReference(MilDigitizerId, M_CONTRAST_REF, 128);
MdigReference(MilDigitizerId, M_BRIGHTNESS_REF, 128);
if (MilImageBufferId)
{
// Allocate a display
MdispAlloc(((CVisionApp*)AfxGetApp())->MilSystem, M_DEFAULT, M_DISPLAY_SETUP, M_DEFAULT, &MilDisplayId);
// Do not allow example to run in dual screen mode
if(MdispInquire(MilDisplayId, M_DISPLAY_MODE, M_NULL)!=M_WINDOWED)
{
MessageBox("This example does not run in dual screen mode.",
"MdispMFC example", MB_ICONINFORMATION );
MdispFree(MilDisplayId);
MilDisplayId = (MIL_ID) NULL;
}
if (MilDisplayId)
{
// Disable scroll bars on display
MdispControl(MilDisplayId, M_WINDOW_SCROLLBAR, M_DISABLE);
// Select the buffer from it's display object and given window
MdispSelectWindow(MilDisplayId, MilImageBufferId, m_hWnd);
// Get active multimedia window rectangle
//MMRect.SetRect(0, 0, DispSizeX+364, DispSizeY+252);
MMRect.SetRect(0, 0, DispSizeX+364, DispSizeY+240);
if (!MMRect.IsRectEmpty())
{
// Get major and minor version numbers of Windows
DWORD dwVersion = GetVersion();
DWORD dwWindowsMajorVersion = (DWORD)(LOBYTE(LOWORD(dwVersion)));
DWORD dwWindowsMinorVersion = (DWORD)(HIBYTE(LOWORD(dwVersion)));
// Check if dimension must be adjusted
long PosOffset=0;
// Windows NT351 check
if ( (dwVersion < 0x80000000 ) &&
(dwWindowsMajorVersion==3 ) &&
(dwWindowsMinorVersion==51) )
PosOffset=0;
else{
PosOffset=4;
}
// Resize frame to the dimensions of the multimedia window
GetParent()->CalcWindowRect(&MMRect);
MMRect.OffsetRect(-MMRect.left,-MMRect.top);
GetParent()->SetWindowPos(&wndTopMost, 0, 0,
MMRect.right+PosOffset,
MMRect.bottom+PosOffset,
SWP_NOACTIVATE|SWP_NOMOVE);
GetParent()->InvalidateRect(NULL,FALSE);
MgraFont(M_DEFAULT, M_FONT_DEFAULT_LARGE);
MgraColor(M_DEFAULT, M_RGB888(255, 0, 255));
MgraText(M_DEFAULT, MilImageBufferId, DispSizeX/2-200, DispSizeY/2-75, "Welcome to Robot Soccer!");
MgraFont(M_DEFAULT, M_FONT_DEFAULT);
MgraColor(M_DEFAULT, M_RGB888(0, 255, 255));
MgraText(M_DEFAULT, MilImageBufferId, DispSizeX/2-150, DispSizeY/2+15, "Load VXD.");
}
}
}
///////////////////////////////////////////////////////////////////////////////////
// MIL: Code that will be executed when a view is first attached to the document //
///////////////////////////////////////////////////////////////////////////////////
////////////////////////////////
// initialization by defaults //
////////////////////////////////
rectBoundary = CRect(0, 0, DispSizeX-1, DispSizeY-1);
bFlagIsOurHomeLeft = TRUE;
m_bFlagFindBall = TRUE;
m_bFlagFindHgoalie = TRUE;
m_bFlagFindHome1 = TRUE;
m_bFlagFindHome2 = TRUE;
m_bFlagFindHome3 = TRUE;
m_bFlagFindHome4 = TRUE;
m_bFlagFindOpponent1 = TRUE;
m_bFlagFindOpponent2 = TRUE;
m_bFlagFindOpponent3 = TRUE;
m_bFlagFindOpponent4 = TRUE;
m_bFlagFindOpponent5 = TRUE;
bFlagFindBall = TRUE;
bFlagFindHomeRobot[HOME1] = TRUE;
bFlagFindHomeRobot[HOME2] = TRUE;
// bFlagFindHomeRobot[HOME3] = TRUE;
// bFlagFindHomeRobot[HOME4] = TRUE;
bFlagFindHomeRobot[HGOALIE] = TRUE;
bFlagFindOpponent[OPP1] = TRUE;
bFlagFindOpponent[OPP2] = TRUE;
bFlagFindOpponent[OPP3] = TRUE;
// bFlagFindOpponent[OPP4] = TRUE;
// bFlagFindOpponent[OPP5] = TRUE;
bFlagBallFound = FALSE;
bFlagHomeRobotFound[HOME1] = FALSE;
bFlagHomeRobotFound[HOME2] = FALSE;
// bFlagHomeRobotFound[HOME3] = FALSE;
// bFlagHomeRobotFound[HOME4] = FALSE;
bFlagHomeRobotFound[HGOALIE] = FALSE;
bFlagOpponentFound[OPP1] = FALSE;
bFlagOpponentFound[OPP2] = FALSE;
bFlagOpponentFound[OPP3] = FALSE;
// bFlagOpponentFound[OPP4] = FALSE;
// bFlagOpponentFound[OPP5] = FALSE;
LowerBoundOfBigTeamSize = DEF_LBOUND_BIG_TEAM_SIZE;
UpperBoundOfBigTeamSize = DEF_UBOUND_BIG_TEAM_SIZE;
LowerBoundOfSmallIDSize = DEF_LBOUND_SMALL_ID_SIZE;
UpperBoundOfSmallIDSize = DEF_UBOUND_SMALL_ID_SIZE;
LowerBoundOfBallSize = DEF_LBOUND_BALL_SIZE;
UpperBoundOfBallSize = DEF_UBOUND_BALL_SIZE;
LowerBoundOfOpponentSize = DEF_LBOUND_OPPONENT_SIZE;
UpperBoundOfOpponentSize = DEF_UBOUND_OPPONENT_SIZE;
⌨️ 快捷键说明
复制代码Ctrl + C
搜索代码Ctrl + F
全屏模式F11
增大字号Ctrl + =
减小字号Ctrl + -
显示快捷键?