📄 liveviewdlg.cpp
字号:
for(i=0; i<NUM_BUFFERS; ++i)
{
m_Bcam.GrabImageAsync(m_ptrBitmaps[i], AoiSize.cx * AoiSize.cy, (void*)i, USE_ONESHOT);
}
//启动定时器，刷新窗体
SetTimer(ID_TIMER_DISPLAY, TIMER_SPACE_NUM, NULL);
//开始计算显示帧率和采集帧率
m_DisplayWatch.Start();
m_AcquisitionWatch.Start();
// 判断是否进行连续采集
if(!USE_ONESHOT)
m_Bcam.ContinuousShot = true;
}
CATCH_MSGBOX( "CLiveViewDlg::OnIdentifytest" )
}
void CLiveViewDlg::OnOK()
{
// TODO: Add extra validation here
if(m_LiveGrabbing)
{
MessageBox("请先按停止键！","error");
return;
}
CDialog::OnOK();
}
void CLiveViewDlg::IdentifyTest()
{
if(m_object==-1)
{
MessageBox("请选择一个我方对象！","警告",MB_ICONWARNING);
return;
}
RGBTRIPLE*pRGB=(RGBTRIPLE*)m_pDispBitmap;
CDC*pDC1=m_display.GetDC();
CDC*pDC2=m_display.GetDC();
CPen IdentiPen1(PS_SOLID,2,RGB(0,255,0));
CPen IdentiPen2(PS_SOLID,1,RGB(255,0,0));
CPen* pOldPen1=pDC1->SelectObject(&IdentiPen1);
CPen* pOldPen2=pDC2->SelectObject(&IdentiPen2);
destDC.FillSolidRect(0,0,640,480,RGB(0,0,0));
pDC1->StretchBlt(0,0,640,480,&destDC,0,0,640,480,SRCCOPY);
while(IdentifySearchLUT(m_object,pRGB, &m_LUT[m_object][0],50,300,0,0,640,480,MASKCOLOR))
{
pDC1->MoveTo(m_xLeft,m_yTop);
pDC1->LineTo(m_xLeft,m_yBottom);
pDC1->LineTo(m_xRight,m_yBottom);
pDC1->LineTo(m_xRight,m_yTop);
pDC1->LineTo(m_xLeft,m_yTop);
// pOldPen=pDC->SelectObject(&IdentiPen2);
pDC2->MoveTo(m_xLeft,m_Target[m_object].y);
pDC2->LineTo(m_xRight,m_Target[m_object].y);
pDC2->MoveTo(m_Target[m_object].x,m_yTop);
pDC2->LineTo(m_Target[m_object].x,m_yBottom);
}
pDC1->SelectObject(pOldPen1);
pDC2->SelectObject(pOldPen2);
ReleaseDC(pDC1);
ReleaseDC(pDC2);
}
void CLiveViewDlg::OnInitial()
{
// TODO: Add your control notification handler code here
UpdateData();
CDC* pDC = m_display.GetDC();
pDC->SetBkMode(TRANSPARENT);
pDC->SetTextColor(RGB(0,255,255));
pDC->StretchBlt(0,0,640,480,&groundDC,0,0,640,480,SRCCOPY);
for(int i=0; i<11; i++)
{
robotInfor[i].x = 0.0;
robotInfor[i].y = 0.0;
robotInfor[i].theta = 0.0;
robotBk[i].x = 0.0;
robotBk[i].y = 0.0;
robotBk[i].theta = 0.0;
}
///决策初始化///
m_DecisionMakingx.Initialize();
DEGame dmDEG;
dmDEG.DEGameMode = 2;
dmDEG.DEGameGround = m_area;
dmDEG.DEStartMode = m_kick;
dmDEG.DEStartState = m_attack;
// dmDEG.DEDQDirect = m_dqdirect;
// dmDEG.DEDQSMDirect = m_dqsmd;
m_DecisionMakingx.InitDEG(dmDEG);
//发送指令1
m_RobotComm.SendCommand(0, (int)0, (int)0);
m_RobotComm.SendCommand(1, (int)0, (int)0);
m_RobotComm.SendCommand(2, (int)0, (int)0);
m_RobotComm.SendIt(0);
m_RobotComm.SendIt(0);
if(!InitOK)
{
InitOK=TRUE;
}
m_colortest=false;
CLiveViewDlg::OnGrabsingle();
RGBTRIPLE *pSingle=(RGBTRIPLE *)m_pDispSingle;
m_pIdentify=pSingle;
Identify();
DrawRobot(pDC);
DrawBall(pDC);
ReleaseDC(pDC);
m_colortest=true;
}
void CLiveViewDlg::Identify()
{
for(int i=0; i<11; i++)
{
robotBk[i] = robotInfor[i];
}
IdentifyRobot();
IdentifyBall();
}
void CLiveViewDlg::IdentifyRobot()
{
int i,j;
int tl,tr,tu,td;
int ObjectCount;
int UnIdentifyCount;
double temptheta,OrientAngle;
CPoint ReferPoint[4];
BOOL blackID[4];
int NotBlack;
int RobotID;
BOOL RobotFound[3];
ObjectCount=0;
UnIdentifyCount=0;
for(i=0;i<3;i++)
RobotFound[i]=0;
tl = ground.LeftTop.x-35;
if(tl<0) tl = 0;
tu = ground.LeftTop.y-35;
if(tu<0) tu = 0;
tr = ground.RightBottom.x+35;
if(tr>639) tr=639;
td = ground.RightBottom.y+35;
if(td>479) td = 479;
while(SearchLUT(TEAM,m_pIdentify,&m_LUT[TEAM][0],50,300,tl,tu,tr,td,MASKCOLOR))
{
TeamTarget[ObjectCount].x=m_Target[TEAM].x;
TeamTarget[ObjectCount].y=m_Target[TEAM].y;
NormalTheta[ObjectCount]=m_theta;
if((TeamTarget[ObjectCount].x<0)||(TeamTarget[ObjectCount].x>639)||(TeamTarget[ObjectCount].y<0)||(TeamTarget[ObjectCount].y>479))
continue;
ObjectCount++;
if(ObjectCount==3)
break;
}
for (i=0;i<ObjectCount;i++)
{
temptheta=Pi/2-atan(0.75)-NormalTheta[i];
ReferPoint[0].x=TeamTarget[i].x+m_length*cos(temptheta);//参考点1 //8 or 8.5
ReferPoint[0].y=TeamTarget[i].y+m_length*sin(temptheta);
ReferPoint[1].x=TeamTarget[i].x+m_length*cos(temptheta+2*atan(0.75));//参考点2
ReferPoint[1].y=TeamTarget[i].y+m_length*sin(temptheta+2*atan(0.75));
ReferPoint[2].x=TeamTarget[i].x+m_length*cos(temptheta+Pi);//参考点3
ReferPoint[2].y=TeamTarget[i].y+m_length*sin(temptheta+Pi);
ReferPoint[3].x=TeamTarget[i].x+m_length*cos(temptheta+2*atan(0.75)+Pi);//参考点4
ReferPoint[3].y=TeamTarget[i].y+m_length*sin(temptheta+2*atan(0.75)+Pi);
for(j=0;j<4;j++)
blackID[j]=0;
for(j=0;j<4;j++)
blackID[j]=FindBlackID(ReferPoint[j].x,ReferPoint[j].y,j);
if(blackID[0]&&blackID[1])
{
OrientAngle=NormalTheta[i]+Pi;
RobotID=FindRobotID(ReferPoint[2],ReferPoint[3]);
}
else if(blackID[2]&&blackID[3])
{
OrientAngle=NormalTheta[i];
RobotID=FindRobotID(ReferPoint[0],ReferPoint[1]);
}
else //非正常情况处理
{
if(!m_CorrectPatch) continue;
NotBlack=0;
for(j=0;j<4;j++)
if(!blackID[j]) NotBlack++;
if(NotBlack==2)
{
if(!blackID[0]&&!blackID[2])
{
if(BlackSum[0]>BlackSum[2])//0 1 black
{
OrientAngle=NormalTheta[i]+Pi;
RobotID=FindRobotID(ReferPoint[2],ReferPoint[3]);
}
else if(BlackSum[2]>BlackSum[0])//2 3 black
{
OrientAngle=NormalTheta[i];
RobotID=FindRobotID(ReferPoint[0],ReferPoint[1]);
}
}
else if(!blackID[0]&&!blackID[3])
{
if(BlackSum[0]>BlackSum[3])//0 1 black
{
OrientAngle=NormalTheta[i]+Pi;
RobotID=FindRobotID(ReferPoint[2],ReferPoint[3]);
}
else if(BlackSum[3]>BlackSum[0])//2 3 black
{
OrientAngle=NormalTheta[i];
RobotID=FindRobotID(ReferPoint[0],ReferPoint[1]);
}
}
else if(!blackID[1]&&!blackID[2])
{
if(BlackSum[1]>BlackSum[2])// 0 1 black
{
OrientAngle=NormalTheta[i]+Pi;
RobotID=FindRobotID(ReferPoint[2],ReferPoint[3]);
}
else if(BlackSum[2]>BlackSum[1])// 2 3 black
{
OrientAngle=NormalTheta[i];
RobotID=FindRobotID(ReferPoint[0],ReferPoint[1]);
}
}
else if(!blackID[1]&&!blackID[3])
{
if(BlackSum[1]>BlackSum[3]) // 0 1 black
{
OrientAngle=NormalTheta[i]+Pi;
RobotID=FindRobotID(ReferPoint[2],ReferPoint[3]);
}
else if(BlackSum[3]>BlackSum[1])// 2 3 black
{
OrientAngle=NormalTheta[i];
RobotID=FindRobotID(ReferPoint[0],ReferPoint[1]);
}
}
}
else if(NotBlack==3)
{
if(!blackID[0]&&!blackID[1])
{
if(!blackID[2]&&BlackSum[0]>BlackSum[2]&&BlackSum[1]>BlackSum[2])// guess 0 1 black
{
OrientAngle=NormalTheta[i]+Pi;
RobotID=FindRobotID(ReferPoint[2],ReferPoint[3]);
}
else if(!blackID[2]&&(BlackSum[2]>BlackSum[0]||BlackSum[2]>BlackSum[1]))// guess 2 3 black
{
OrientAngle=NormalTheta[i];
RobotID=FindRobotID(ReferPoint[0],ReferPoint[1]);
}
if(!blackID[3]&&BlackSum[0]>BlackSum[3]&&BlackSum[1]>BlackSum[3])// guess 0 1 black
{
OrientAngle=NormalTheta[i]+Pi;
RobotID=FindRobotID(ReferPoint[2],ReferPoint[3]);
}
else if(!blackID[3]&&(BlackSum[3]>BlackSum[0]||BlackSum[3]>BlackSum[1])) //guess 2 3 black
{
OrientAngle=NormalTheta[i];
RobotID=FindRobotID(ReferPoint[0],ReferPoint[1]);
}
}
else if(!blackID[2]&&!blackID[3])
{
if(!blackID[0]&&BlackSum[2]>BlackSum[0]&&BlackSum[3]>BlackSum[0]) //guess 2 3 black
{
OrientAngle=NormalTheta[i];
RobotID=FindRobotID(ReferPoint[0],ReferPoint[1]);
}
else if(!blackID[0]&&(BlackSum[0]>BlackSum[2]||BlackSum[0]>BlackSum[3])) // guess 0 1 black
{
OrientAngle=NormalTheta[i]+Pi;
RobotID=FindRobotID(ReferPoint[2],ReferPoint[3]);
}
if(!blackID[1]&&BlackSum[2]>BlackSum[1]&&BlackSum[3]>BlackSum[1])// guess 2 3 black
{
OrientAngle=NormalTheta[i];
RobotID=FindRobotID(ReferPoint[0],ReferPoint[1]);
}
else if(!blackID[1]&&(BlackSum[1]>BlackSum[2]||BlackSum[1]>BlackSum[3]))// guess 0 1 black
{
OrientAngle=NormalTheta[i]+Pi;
RobotID=FindRobotID(ReferPoint[2],ReferPoint[3]);
}
}
}
else //high ambiguity,give up
continue;
}
if(RobotID>=0&&RobotID<=2)
{
robotInfor[RobotID].theta=OrientAngle*180/Pi;
robotInfor[RobotID].x=ground.groundInfo[TeamTarget[i].x][TeamTarget[i].y].x;
robotInfor[RobotID].y=ground.groundInfo[TeamTarget[i].x][TeamTarget[i].y].y;
}
}
}
bool CLiveViewDlg::FindBlackID(int m, int n,int Num)
{
bool IDCode;
int ii,jj;
int sumblack;
sumblack=0;
for(jj=n-1;jj<=n+1;jj++)
for(ii=m-1;ii<=m+1;ii++)
{
if(jj<0||jj>479||ii<0||ii>639) continue;
if((!m_LUT[1][screenBuffer(ii,jj,m_pIdentify)&0x7fff])&&(screenBuffer(ii,jj,m_pIdentify)!=12684))
{
sumblack++;
if(sumblack>=5) break;
}
}
if(sumblack>=5) IDCode=1;
else
{
IDCode=0;
BlackSum[Num]=sumblack;
}
return IDCode;
}
int CLiveViewDlg:: FindRobotID(CPoint RP1,CPoint RP2)
{
int roboID,RPID1,RPID2;
int ii,jj;
int sum1,sum0;
sum1=sum0=0;
for(jj=RP1.y-2;jj<=RP1.y+2;jj++)
for(ii=RP1.x-2;ii<=RP1.x+2;ii++)
{
if(jj<0||jj>479||ii<0||ii>639) continue;
if(m_LUT[1][screenBuffer(ii,jj,m_pIdentify)&0x7fff])
sum1++;
else if(screenBuffer(ii,jj,m_pIdentify)!=12684)
sum0++;
}
RPID1=JudgeColor(sum1,sum0);
sum1=sum0=0;
for(jj=RP2.y-2;jj<=RP2.y+2;jj++)
for(ii=RP2.x-2;ii<=RP2.x+2;ii++)
{
if(jj<0||jj>479||ii<0||ii>639) continue;
if(m_LUT[1][screenBuffer(ii,jj,m_pIdentify)&0x7fff])
sum1++;
else if(screenBuffer(ii,jj,m_pIdentify)!=12684)
sum0++;
}
RPID2=JudgeColor(sum1,sum0);
if(RPID1==0&&RPID2==1) roboID=0;
else if(RPID1==1&&RPID2==0) roboID=1;
else if(RPID1==1&&RPID2==1) roboID=2;
else roboID=-1;
return roboID;
}
int CLiveViewDlg::JudgeColor(int a,int b)
{
int colorID;
if(a>=10&&b>=10) return -1;
if(a>=10) colorID=1;
else if(b>=10) colorID=0;
else colorID=-1;
/* if(b<=a&&c<=a&&d<=a) colorID=1; //Mem1
else if(a<=b&&c<=b&&d<=b) colorID=2; //Mem2
else if(a<=c&&b<=c&&d<=c) colorID=3; //Mem3
else if(a<=d&&b<=d&&c<=d) colorID=0; //black*/
return colorID;
}
void CLiveViewDlg::IdentifyBall()
{
int tl,tu,tr,td;
tl = ground.LeftTop.x-35;
if(tl<0) tl = 0;
tu = ground.LeftTop.y-35;
if(tu<0) tu = 0;
tr = ground.RightBottom.x+35;
if(tr>639) tr=639;
td = ground.RightBottom.y+35;
if(td>479) td = 479;
if(SearchLUT(BALL,m_pIdentify, &m_LUT[BALL][0],20,350,tl,tu,tr,td,0))
{
robotInfor[10].x=ground.groundInfo[m_Target[BALL].x][m_Target[BALL].y].x;
robotInfor[10].y=ground.groundInfo[m_Target[BALL].x][m_Target[BALL].y].y;
}
}
void CLiveViewDlg::DrawRobot(CDC *pDC)
{
int x,y,theta,i;
CPen* pOldPen=pDC->SelectObject(&OurRobotPen);
///将机器人的坐标转换成屏幕坐标///
for(i=0;i<3;i++)
{
if((robotBk[i].x!=0)&&(robotBk[i].y!=0))
{
x=(int)(robotBk[i].x*3.5)+50;
y=(int)(robotBk[i].y*3.5)+12;
///擦除旧机器人///
pDC->StretchBlt(x-15,y-15,31,31,&groundDC,x-15,y-15,31,31,SRCCOPY);
}
}
CString str;
for( i=0;i<3;i++)
{
if((robotInfor[i].x!=0)&&(robotInfor[i].y!=0))
{
x=(int)(robotInfor[i].x*3.5)+50;
y=(int)(robotInfor[i].y*3.5)+12⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -