showimagedlg.cpp

使用大恒采集卡的图像显示

	}
	CDialog::OnLButtonDown(nFlags, point);
}

void CShowImageDlg::OnSetA() 
{
	// TODO: Add your control notification handler code here
	UpdateData();
	m_point_x1=m_point_x;
	m_point_y1=m_point_y;
	m_color_R1=m_color_R;
	m_color_G1=m_color_G;
	m_color_B1=m_color_B;
	m_point_x=-1;
	OnPP();
}

void CShowImageDlg::OnInitialization()
{
	POINT center;
	int f;
	WORLDPOINT A,B,C;
	CPoint a,b,c;
	UpdateData();
	A.x=m_a_x;
	A.y=m_a_y;
	A.z=m_a_z;

	B.x=m_b_x;
	B.y=m_b_y;
	B.z=m_b_z;

	C.x=m_c_x;
	C.y=m_c_y;
	C.z=m_c_z;

	a.x=m_point_x1;
	a.y=m_point_y1;
	b.x=m_point_x2;
	b.y=m_point_y2;
	c.x=m_point_x;
	c.y=m_point_y;

	center.x=(int)m_center_x;
	center.y=(int)m_center_y;
	f=(int)(0.945*m_radius);
	CLocalization* lp=new CLocalization();
		m_p=lp->PositionPrecess(A,B,C,a,b,c,center,f);
	m_vehicle_x=m_p.o.x;
	m_vehicle_y=m_p.o.y;
	m_vehicle_z=m_p.o.z;
	m_Turn = m_p.ix;
	m_Slope = m_p.iz;
	m_Inclination = m_p.iy;
	if(m_bVehicle)
		TakeRobot(m_vehicle_x,m_vehicle_y,m_Turn);
	UpdateData(FALSE);
	delete lp;
}

void CShowImageDlg::OnSetA2() 
{
	// TODO: Add your control notification handler code here
	UpdateData();
	m_point_x2=m_point_x;
	m_point_y2=m_point_y;
	m_color_R2=m_color_R;
	m_color_G2=m_color_G;
	m_color_B2=m_color_B;
	m_point_x=-1;
	OnPP();
}

void CShowImageDlg::OnSetA3() 
{
	// TODO: Add your control notification handler code here
	UpdateData();
	m_point_x3=m_point_x;
	m_point_y3=m_point_y;
	m_bCase=FALSE;
	m_bLocalization=TRUE;
	OnPP();
}

void CShowImageDlg::EdgeS(int n, int x, int y)
{
	Eaa[n][0]=x;
	Eaa[n][1]=y;
}

void CShowImageDlg::DEdgeS(double a[][2], int n, float *x0, float *y0, float *R)
{
	int i,j,k,mm;
	mm=1;
	double a1,b1,c1,a2,b2,c2;
	double ab12;
	double xx0,yy0;
	double csx,csy,csr;
	csx=csy=csr=0;
	for(i=0;i<n-2;i++)
		for(j=i+1;j<n-1;j++)
			for(k=j+1;k<n;k++)
			{
				a1=2*(a[j][0]-a[i][0]);
				b1=2*(a[j][1]-a[i][1]);
				c1=a[j][0]*a[j][0]-a[i][0]*a[i][0]+a[j][1]*a[j][1]-a[i][1]*a[i][1];
				a2=2*(a[k][0]-a[j][0]);
				b2=2*(a[k][1]-a[j][1]);
				c2=a[k][0]*a[k][0]-a[j][0]*a[j][0]+a[k][1]*a[k][1]-a[j][1]*a[j][1];
				ab12=a1*b2-a2*b1;
				if(ab12==0) ab12=1;
				xx0=(c1*b2-c2*b1)/ab12;
				yy0=(a1*c2-a2*c1)/ab12;
				csx+=xx0;
				csy+=yy0;
				csr+=sqrt((xx0-a[i][0])*(xx0-a[i][0])+(yy0-a[i][1])*(yy0-a[i][1]));

			}

	for(i=n;i>n-3;i--)
		mm*=i;
	*x0=(float)(6*csx/mm-11);
	*y0=(float)(6*csy/mm-11);
	*R=(float)(6*csr/mm);
}

void CShowImageDlg::OnTake() 
{
	// TODO: Add your control notification handler code here
	if(m_bCali)
	{
		if(m_n>100)
			MessageBox("取的点太多");
		else if(m_n>2)
			DEdgeS(Eaa,m_n,&m_center_x,&m_center_y,&m_radius);
		m_n=0;
		m_bCali=FALSE;
		m_bCase=TRUE;
		UpdateData(FALSE);
	}
	else return;
}

void CShowImageDlg::TakeRobot(int Robot_x, int Robot_y, int angle)
{	
	int x,s,a1,b1;
	x=Robot_y-100;
	s=(int)(atan2(Robot_y,x)*180.0/PIE);
	
	if(angle<-90) b1=3;
	else if(angle<0) b1=4;
	else if(angle<90) b1=1;
	else b1=2;
	CPID pid;
	pid.pid_tune();
	switch(b1)
		{
		case 1:
			{
				a1=90-angle;					
				if(x>0)
				{
					Drive_Rotate_Position(-a1,20);
					Drive_Velocity((int)pid.pid_calc(-x/3),30);
				}
				else Drive_Velocity((int)pid.pid_calc(-a1-x/3),30);
				break;
			}
		case 2:
			{
				a1=angle-90;
				if(x>0)
					Drive_Velocity((int)pid.pid_calc(a1-x/3),30);
				else
				{
					Drive_Rotate_Position(a1,20);
					Drive_Velocity((int)pid.pid_calc(-x/3),30);
				}
				break;
			}
		case 3:
			{
				a1=-angle-90;
				if(x<0)
				{
					Drive_Rotate_Position(-a1,20);
					Drive_Velocity((int)pid.pid_calc(x/3),30);
				}
				else 
					Drive_Velocity((int)pid.pid_calc(-a1+x/3),30);
				break;
			}
		case 4:
			{
				a1=90+angle;
				if(x<0)
					Drive_Velocity((int)pid.pid_calc(a1+x/3),30);
				else 
				{
					Drive_Rotate_Position(a1,20);
					Drive_Velocity((int)pid.pid_calc(x/3),30);
				}
				break;
			}
		}
}

void CShowImageDlg::OnStartrobot() 
{
	// TODO: Add your control notification handler code here
	m_bVehicle=TRUE;
}

void CShowImageDlg::OnShoprobot() 
{
	// TODO: Add your control notification handler code here
	Drive_Velocity(0,0);
	m_bVehicle=FALSE;
}

void CShowImageDlg::Correction(BYTE *Src, BYTE *dst, WORD *dat)
{
	ASSERT(Src);
	ASSERT(dst);
	ASSERT(dat);

	WORD XX,YY;
	WORD dx,dy,qp;
	long indxy;
	long	stride;
	int i,j;
	unsigned int x2y0,x0y2,x2y2,x2y4,x4y2,x0y0,x4y0,x0y4,x4y4,t,x2y0g,x0y2g,x2y2g,x2y4g,x4y2g,x0y0g,x4y0g,x0y4g,x4y4g,tg,x2y0b,x0y2b,x2y2b,x2y4b,x4y2b,x0y0b,x4y0b,x0y4b,x4y4b,tb;
	long index=0;
	WORD* lp=dat;
	stride=768*3;
	LPBYTE lpbuf=Src;
	for(i=0;i<576;i++)
	{
		LPBYTE ddd=dst+index;

		for(j=0;j<768;j++)
		{
			XX=*lp++;
			YY=*lp++;
			dx=XX>>4;
			qp=XX&15;
			dy=YY;
			indxy=dy*stride+dx*3;

			if(dx<768&&dx>0&&dy>0&&dy<576)
			{
				x0y0=*(lpbuf+indxy);
				x4y0=*(lpbuf+indxy+3);
				x0y4=*(lpbuf+indxy+stride);
				x4y4=*(lpbuf+indxy+stride+3);
				x2y0=(x0y0 + x4y0)>>1;
				x0y2=(x0y0+x0y4)>>1;
				x2y4=(x0y4+x4y4)>>1;
				x4y2=(x4y0+x4y4)>>1;
				x2y2=(x2y0+x2y4)>>1;

				x0y0g=*(lpbuf+indxy+1);
				x4y0g=*(lpbuf+indxy+4);
				x0y4g=*(lpbuf+indxy+stride+1);
				x4y4g=*(lpbuf+indxy+stride+4);
				x2y0g=(x0y0g + x4y0g)>>1;
				x0y2g=(x0y0g+x0y4g)>>1;
				x2y4g=(x0y4g+x4y4g)>>1;
				x4y2g=(x4y0g+x4y4g)>>1;
				x2y2g=(x2y0g+x2y4g)>>1;

				x0y0b=*(lpbuf+indxy+2);
				x4y0b=*(lpbuf+indxy+5);
				x0y4b=*(lpbuf+indxy+stride+2);
				x4y4b=*(lpbuf+indxy+stride+5);
				x2y0b=(x0y0b + x4y0b)>>1;
				x0y2b=(x0y0b+x0y4b)>>1;
				x2y4b=(x0y4b+x4y4b)>>1;
				x4y2b=(x4y0b+x4y4b)>>1;
				x2y2b=(x2y0b+x2y4b)>>1;

				switch(qp)
				{
				case 0:
					{
						t=x0y0;
						tg=x0y0g;
						tb=x0y0b;
						break;
					}
				case 1:
					{
						t=(x0y0+x2y0)>>1;
						tg=(x0y0g+x2y0g)>>1;
						tb=(x0y0b+x2y0b)>>1;
						break;
					}
				case 2:
					{
						t=x2y0;
						tg=x2y0g;
						tb=x2y0b;
						break;
					}
				case 3:
					{
						t=(x2y0+x4y0)>>1;
						tg=(x2y0g+x4y0g)>>1;
						tb=(x2y0b+x4y0b)>>1;
						break;
					}
				case 4:
					{
						t=(x0y0+x0y2)>>1;
						tg=(x0y0g+x0y2g)>>1;
						tb=(x0y0b+x0y2b)>>1;
						break;
					}
				case 5:
					{
						t=(x0y0+x2y0+x0y2+x2y2)>>2;
						tg=(x0y0g+x2y0g+x0y2g+x2y2g)>>2;
						tb=(x0y0b+x2y0b+x0y2b+x2y2b)>>2;
						break;
					}
				case 6:
					{
						t=(x2y0+x2y2)>>1;
						tg=(x2y0g+x2y2g)>>1;
						tb=(x2y0b+x2y2b)>>1;
						break;
					}
				case 7:
					{
						t=(x4y0+x2y2+x4y2+x2y0)>>2;
						tg=(x4y0g+x2y2g+x4y2g+x2y0g)>>2;
						tb=(x4y0b+x2y2b+x4y2b+x2y0b)>>2;
						break;
					}
				case 8:
					{
						t=x0y2;
						tg=x0y2g;
						tb=x0y2b;
						break;
					}
				case 9:
					{
						t=(x0y2+x2y2)>>1;
						tg=(x0y2g+x2y2g)>>1;
						tb=(x0y2b+x2y2b)>>1;
						break;
					}
				case 10:
					{
						t=x2y2;
						tg=x2y2g;
						tb=x2y2b;
						break;
					}
				case 11:
					{
						t=(x2y2+x4y2)>>1;
						tg=(x2y2g+x4y2g)>>1;
						tb=(x2y2b+x4y2b)>>1;
						break;
					}
				case 12:
					{
						t=(x0y2+x0y4)>>1;
						tg=(x0y2g+x0y4g)>>1;
						tb=(x0y2b+x0y4b)>>1;
						break;
					}
				case 13:
					{
						t=(x0y2+x0y4+x2y2+x2y4)>>2;
						tg=(x0y2g+x0y4g+x2y2g+x2y4g)>>2;
						tb=(x0y2b+x0y4b+x2y2b+x2y4b)>>2;
						break;
					}
				case 14:
					{
						t=(x2y2+x2y4)>>1;
						tg=(x2y2g+x2y4g)>>1;
						tb=(x2y2b+x2y4b)>>1;
						break;
					}
				case 15:
					{
						t=(x2y2+x2y4+x4y2+x4y4)>>2;
						tg=(x2y2g+x2y4g+x4y2g+x4y4g)>>2;
						tb=(x2y2b+x2y4b+x4y2b+x4y4b)>>2;
						break;
					}
				}

			}
			else {t=tg=tb=0;}
			if(t<0) t=0;
			if(t>255) t=255;
			*(ddd+3*j)=t;

			if(tg<0) tg=0;
			if(tg>255) tg=255;
			*(ddd+3*j+1)=tg;

			if(tb<0) tb=0;
			if(tb>255) tb=255;
			*(ddd+3*j+2)=tb;

		}
		index+=stride;
	}

}

void CShowImageDlg::OnCheck1() 
{
	// TODO: Add your control notification handler code here
	UpdateData();
	m_color_R=m_color_G=m_color_B=0;
	m_color_R1=m_color_G1=m_color_B1=0;
	m_color_R1=m_color_G1=m_color_B1=0;

	m_point_x1=m_point_y1=m_point_x2=m_point_y2=m_point_x3=m_point_y3=-1;
	m_point_x=m_point_y=-1;
	m_p.ix=m_p.iy=m_p.iz=0;
	m_p.o.x=m_p.o.y=m_p.o.z=0;

	m_bCase=FALSE;
	m_bCali=TRUE;
	m_bLocalization=FALSE;
	m_bVehicle=FALSE;
	OnPP();
}