stewartwin_dlg.cpp

bulkloop application source code based on mfc

	// TODO: Add your control notification handler code here
	CircleA.DoModal();	
	//UpdateData(true);	// 输入数据的更新
	CircleTrace();
}
void CCStewartWinAppDlg::OnTriAgl() 
{
	TriAglA.DoModal();	
	//UpdateData(true);	// 输入数据的更新
	TriAglTrace();	
}
void CCStewartWinAppDlg::OnPetal() 
{
	Petal_oo.DoModal();	
	//UpdateData(true);	// 输入数据的更新
	PetalTrace();	
	
}

void CCStewartWinAppDlg::OnSwing() 
{
	Swing_oo.DoModal();	
	//UpdateData(true);	// 输入数据的更新
	SwingTrace();	
}

int CCStewartWinAppDlg::OnRun() 
{
	// TODO: Add your control notification handler code here
	//MeiObjects::startPointList( int AxisNum ,double points[], long pointCount,
	//							 double timeSlice, double initialPosition[])
	double point[12];
	int pointI ,pointJ;
	double initialPosition[6];
	CString ss;
	pointI = 0;
	pointJ = 0;
	usTime NusTime;
	long returnValue;

	if( !( meiObjects->CheckAmpEnable() ) )
	{
		MessageBox("请先使能伺服放大器！！！");
		return(0);
	}
	//
	meiObjects->MapAxisToMotion(1);

		GoHomeFlage = 0;

	/* Start Recorder and wait for Event */
    
	for( pointI=0 ;pointI<1 ;pointI++)
	{
		for( pointJ=0 ;pointJ < MOTOR_NUMBER ;pointJ++)
		{
			point[pointJ] = -( Legd[pointJ][pointI] - Legd[pointJ][0] ) * 8192 / 5 ;
		}	
	}
	
	meiObjects->startPointList( meiObjects->MpiMotion( 6 ) ,AXIS_NUMBER ,point, (long)1,TIME,initialPosition);

	//LARGE_INTEGER PriviousTime;
	
	for( pointI = 1; pointI < Nt; pointI++)
	{	
		for( pointJ=0 ;pointJ < MOTOR_NUMBER ;pointJ++)
		{
			point[pointJ] = -( Legd[pointJ][pointI] - Legd[pointJ][0] ) * 8192 / 5 ;
			if( point[pointJ] < 0 && Direct[pointJ] == -1 )
			{
				Direct[pointJ] = 1;//伸长
				BuCangFlage[pointJ] = 1;
			}
			if( point[pointJ] > 0 && Direct[pointJ] == 1)
			{
				Direct[pointJ] = -1;//伸长
				BuCangFlage[pointJ] = -1;
			}
			if( BuCangFlage[pointJ] > 0 )
			{
				point[pointJ] = point[pointJ] - BeiXi[pointJ]/50;
				BuCangFlage[pointJ]++;
			}
			if( BuCangFlage[pointJ] < -0 )
			{
				point[pointJ] = point[pointJ] + BeiXi[pointJ]/50;
				BuCangFlage[pointJ]--;
			}
			if( BuCangFlage[pointJ] >= 50+1 || BuCangFlage[pointJ] >= -50-1 )
			{
				BuCangFlage[pointJ] = 0;
			}
		}

		if( pointI < Nt-1 )
			meiObjects->ModifyPointList( meiObjects->MpiMotion( 6 ) , AXIS_NUMBER ,point, (long)1,TIME,initialPosition,0);
		else
			meiObjects->ModifyPointList( meiObjects->MpiMotion( 6 ) , AXIS_NUMBER ,point, (long)1,TIME,initialPosition,1);
		m_CmdLeg1.Format("%f", Legd[0][pointI] );
		m_CmdLeg2.Format("%f", Legd[1][pointI] );
		m_CmdLeg3.Format("%f", Legd[2][pointI] );
		m_CmdLeg4.Format("%f", Legd[3][pointI] );
		m_CmdLeg5.Format("%f", Legd[4][pointI] );
		m_CmdLeg6.Format("%f", Legd[5][pointI] );
		UpdateData(false);
		//NusTime.WaitNTus( 9400 , PriviousTime);//( 9900);
		while( meiObjects->GetAxisFrameBufferInfo(0) < 100 )
		{
			;
		}
	}
/*
	for( int index = 0; index < AXIS_NUMBER; index++)//index < numberOfAxes
	{
		returnValue =
		mpiMotionAxisRemove( meiObjects->MpiMotion( 6 ) , meiObjects->MpiAxis( index ) );
		CheckMessage(returnValue);
	}
	meiPlatformSleep(200);
	returnValue =
		mpiMotionAction(meiObjects->MpiMotion( 6 ) , static_cast<MPIAction>(MEIActionMAP));
	CheckMessage(returnValue);
*/
	return(1);
}
void CCStewartWinAppDlg::OnExit() 
{
	// TODO: Add your control notification handler code here
	KillTimer(102);
	//fclose (RecodeFile);
	CDialog::OnOK();
}

void CCStewartWinAppDlg::OnRecoder() 
{
	// TODO: Add your control notification handler code here
	MPIRecorderRecord   records[RECORD_COUNT];
	long                axisNumbers[1] = {0};

	/* Configure recorder object */
    
	/* Start recorder, get data, and stop recorder */
   // meiObjects->StartRecordData( );
    /* Print the records */
    //printRecords(RECORD_COUNT, records, 1, axisNumbers);
}
void CCStewartWinAppDlg::ReadIO(void)
{
	unsigned long DedicatedIOState1[ MOTOR_NUMBER ] = {0 ,0 ,0 ,0 ,0 ,0 };
	unsigned long DedicatedIOState2[ MOTOR_NUMBER ] = {0 ,0 ,0 ,0 ,0 ,0 };
	bool Limit_WH_POS[ MOTOR_NUMBER ] = {false ,false ,false ,false ,false ,false };
	bool Limit_WH_NEG[ MOTOR_NUMBER ] = {false ,false ,false ,false ,false ,false };
	usTime NusTime_oo2;
	int i ;
	for( i = 0 ; i< 100 ; i ++ )
	{
		meiObjects->ReadDedicatedIO( DedicatedIOState1 );
		NusTime_oo2.MySleep( 100 );//1ms
		meiObjects->ReadDedicatedIO( DedicatedIOState2 );

		DedicatedIOState1[i] = DedicatedIOState1[i] & 0x00000003;
		DedicatedIOState2[i] = DedicatedIOState2[i] & 0x00000003;
		if( DedicatedIOState1[i] == DedicatedIOState2[i] )
			break;
	}
	if( i == 100 )
	{
		MessageBox("读IO错误");
		OnOK();
	}
	for( i = 0 ; i< MOTOR_NUMBER ; i ++ )
	{	
		if( DedicatedIOState2[i] != 3 )
		{ 
			if( DedicatedIOState2[i] == 0)	
			{	
				Limit_WH_POS[i] = true;
				Limit_WH_NEG[i] = true;
			}
			else
			{
				if( DedicatedIOState2[i] == 2)	
				{	
					Limit_WH_POS[i] = true;
					Limit_WH_NEG[i] = false;
				}
				else if( DedicatedIOState2[i] == 1)	
				{	
					Limit_WH_NEG[i] = true;
					Limit_WH_POS[i] = false;
				}
			}
		}
		else//==3
		{
			Limit_WH_NEG[i] = false;
			Limit_WH_POS[i] = false;
		}
		switch( i )
		{
			case 0 :
				DriverStatusOO.m_Limit_WH_POS1 = Limit_WH_POS[0];
				DriverStatusOO.m_Limit_WH_NEG1 = Limit_WH_NEG[0];
				break;
			case 1 :
				DriverStatusOO.m_Limit_WH_POS2 = Limit_WH_POS[1];
				DriverStatusOO.m_Limit_WH_NEG2 = Limit_WH_NEG[1];
				break;
			case 2:
				DriverStatusOO.m_Limit_WH_POS3 = Limit_WH_POS[2];
				DriverStatusOO.m_Limit_WH_NEG3 = Limit_WH_NEG[2];
				break;
			case 3:
				DriverStatusOO.m_Limit_WH_POS4 = Limit_WH_POS[3];
				DriverStatusOO.m_Limit_WH_NEG4 = Limit_WH_NEG[3];
				break;
			case 4:
				DriverStatusOO.m_Limit_WH_POS5 = Limit_WH_POS[4];
				DriverStatusOO.m_Limit_WH_NEG5 = Limit_WH_NEG[4];
				break;
			case 5:
				DriverStatusOO.m_Limit_WH_POS6 = Limit_WH_POS[5];
				DriverStatusOO.m_Limit_WH_NEG6 = Limit_WH_NEG[5];
				break;
			default :
				;
		}// end switch( i )
	}// end for		
}

void CCStewartWinAppDlg::OnReadIO() 
{
	// TODO: Add your control notification handler code here
	ReadIO( );
	DisplyMotorStatus();
	DisplyMotionStatus();
	UpdateData(false);
	DriverStatusOO.DoModal();	
}

int CCStewartWinAppDlg::OnAPI() 
{
	// TODO: Add your control notification handler code here
	CString cs;
	double APIBaseWeiZi[6] = {0,0,0,0,0,0};
	double APITatgetBlatWeiZi[6] = {0,0,-630,0,0,0};
	double APIleglong[6];
	double APIsphagl[6];
	double MPILegLong[6];
	double MPIVelocity[6];
	double point[6];
	int i ,j,k,m;
	double initialPosition[6];
	FILE * fp1;
	const double Legd0[6] = { 643.02,641.89,640.196,642.788,643.589,642.676 };
	//const double Legd0[6] = { 643.830 ,637.969 ,638.069 ,637.152 ,643.242  ,645.843};   // 下平台复零位-630 时各支腿的长度: 2007-09-12
	//const double Legd0[6] = { 253.273983,253.422240 ,252.512536 ,252.402998 ,252.049935  ,248.624231}; 
	k = 0;
	j = 0;
	m = 0;
	if( !( meiObjects->CheckAmpEnable() ) )
	{
		MessageBox("请先使能伺服放大器！！！");
		return(0);
	}
	meiObjects->MapAxisToMotion(1);
	fp1 =  fopen("APIBaseWeiZi.dat", "w");
	while(m < 1)
	{
		/*
		APIBaseWeiZi[0]=* ( double * ) ( APIdata ) ;
		APIBaseWeiZi[1]=* ( double * ) ( APIdata+8  ) ;
		APIBaseWeiZi[2]=* ( double * ) ( APIdata+16  ) ;
		APIBaseWeiZi[3]=* ( double * ) ( APIdata+24 ) ;
		APIBaseWeiZi[4]=* ( double * ) ( APIdata+32  ) ;
		APIBaseWeiZi[5]=* ( double * ) ( APIdata+40  ) ;
		*/
		APIBaseWeiZi[2] = 5*( 1 - cos( 0.0314 * k ) );//2Hz 0.1256//0.0314
		//APIBaseWeiZi[2]=* ( double * ) ( APIdata ) ;
		if( k == 628 )
		{
			k = 0;
			m++;
		}
		else
		{
				k++;
		}
/*		cs.Format("%f",APIBaseWeiZi[0]) ;
		m_ActPosX = cs;
		cs.Format("%f",APIBaseWeiZi[1]) ;
		m_ActPosY = cs;
		cs.Format("%f",APIBaseWeiZi[2]) ;
		m_ActPosZ = cs;
		cs.Format("%f",APIBaseWeiZi[3]) ;
		m_ActRoll = cs;
		cs.Format("%f",APIBaseWeiZi[4]) ;
		m_ActYaw = cs;
		cs.Format("%f",APIBaseWeiZi[5]) ;
		m_ActPitch = cs;
		UpdateData(false);
*/

		cmpdinvkin(APIBaseWeiZi, APITatgetBlatWeiZi,  APIleglong, APIsphagl);
		meiObjects->ObtainPositonVelocity( MPILegLong , MPIVelocity);
		
		for(i = 0;i <AXIS_NUMBER; i++)
		{
			point[i] = (APIleglong[i] * 8192 / 5  - (MPILegLong[i] + Legd0[i]* 8192 / 5));		
			/*	if( j < 6 )
				{
					j++;
					if( point[i] < 0 )
						Direct[i] = 1;//伸长
					else
						Direct[i] = -1;//
				}
				if( point[i] < 0 && Direct[i] == -1 )
				{
					Direct[i] = 1;//伸长
					BuCangFlage[i] = 1;
				}
				if( point[i] > 0 && Direct[i] == 1)
				{
					Direct[i] = -1;//
					BuCangFlage[i] = -1;
				}
				if( BuCangFlage[i] > 0 )
				{
					point[i] = point[i] - BeiXi[i]/5;
					BuCangFlage[i]++;
				}
				if( BuCangFlage[i] < 0 )
				{
					point[i] = point[i] + BeiXi[i]/5;
					BuCangFlage[i]--;
				}
				if( BuCangFlage[i] > 5 || BuCangFlage[i] < -5 )
				{
					BuCangFlage[i] = 0;
				}
			*/
		}
/*		
		if( j == 0 )
		{
			meiObjects->startPointList( meiObjects->MpiMotion( 6 ) ,AXIS_NUMBER ,point, (long)1,TIME,initialPosition);
			j++;
		}
		else
			meiObjects->ModifyPointList( meiObjects->MpiMotion( 6 ) , AXIS_NUMBER ,point, (long)1,TIME,initialPosition,0);	
*/
		if( meiObjects->IsMotionDone( meiObjects->MpiMotion( 6 ) ) )
    	{
    		meiObjects->startPointList( meiObjects->MpiMotion( 6 ) ,AXIS_NUMBER ,point, (long)1,TIME,initialPosition);
		}
    	else
		{
			meiObjects->ModifyPointList( meiObjects->MpiMotion( 6 ) , AXIS_NUMBER ,point, (long)1,TIME,initialPosition,0);
    	}

		while( meiObjects->GetAxisFrameBufferInfo(0) < 127 )
		{
			if( meiObjects->IsMotionDone( meiObjects->MpiMotion( 6 ) ) )
    		{
    			break;
			}
			//Sleep(1);//
		}
		//for (i = 0; i < 6; i ++ )
		//	fprintf(fp1,"%f    , %f     , %f     , %f\n",APIBaseWeiZi[i],APIleglong[i]* 8192 / 5,MPILegLong[i] + Legd0[i]* 8192 / 5,point[i] );
		//Sleep(1);
	}
	fclose (fp1); 
	return(1);
}

void CCStewartWinAppDlg::OnAmpEnable() 
{

	if( m_AmpEnable == FALSE )
	{
		m_AmpEnable = TRUE;
		//MessageBox("true");
	}
	else 
	{
		m_AmpEnable = FALSE;
		//MessageBox("FALSE");
	}
	meiObjects->MotionAmpOnOff( m_AmpEnable , TRUE ,0);
}