sm5000.cpp

一个很好的VC++程序

// SM5000 function library
#include "stdafx.h"
#include "SMT5K.h"
#include "SMT5KDoc.h"
#include "SMT5KView.h"
#include "SM5000.h"
#include "sm5000chip.h"
#include "SerialPort.h"
#include "MainFrm.h"
#include <time.h>
#include <math.h>
#include "MonitorAxis.h"
#include "MonitorIPL.h"


CSm5000Chip chip;
CSerialPort m_ComPort; 
CString m_strRXDataCOM1; //COM1接收数据
CString m_strTXDataCOM1; //COM1接收数据
BOOL m_bRxStrOkCom1;
BOOL bStart;
unsigned short m_RR0;
unsigned short m_RR1;
unsigned short m_RR2;
int IO_Counter=0;
int CurrentAxis = 0;   //轴号
int TimerOn = 0;
int SerialPortNo = 1;
int BusWidth = 16;
int ChipSel = 4;
int g_ExppEn = 0;
int g_AutoDec = 0;
int g_AO = 0;

CDialog * pRegTestDlg		=NULL;
CDialog * pConfigDlg		=NULL;
CDialog * pDrvFuncTestDlg	=NULL;
CDialog * pIPLFuncTestDlg	=NULL;
CDialog * pReadRegisterDlg	=NULL;
CView   * pMyView           =NULL;

extern CMonitorAxis *pMonitorAxis[4];
extern CMonitorIPL  *pMonitorIPL;

void sleep( clock_t wait )
{
   clock_t goal;
   goal = wait + clock();
   while( goal > clock() );
}


void WriteIO( unsigned short RegNo, unsigned short Data)
{
	CString Str1;
	Str1.Format("$W%d#%d*end",RegNo,Data);

	m_strTXDataCOM1+=Str1;
}

void ReadIO( unsigned short RegNo)
{
	CString Str1;
	Str1.Format("$R%d#*end",RegNo);
	m_strTXDataCOM1+=Str1;
}


void WriteReg( unsigned short RegNo, unsigned short Data, short AxisXEn, short AxisYEn, short AxisZEn, short AxisUEn )
{
	int mAxis;

	if(RegNo>0 && RegNo<4)
	{
		mAxis=GetmAxisValue(AxisXEn, AxisYEn, AxisZEn, AxisUEn);
		WriteIO( 0, (mAxis<<8)+15); 
	}
	WriteIO( RegNo, Data );

	int AxisArray[4];
	AxisArray[0] = AxisXEn;
	AxisArray[1] = AxisYEn;
	AxisArray[2] = AxisZEn;
	AxisArray[3] = AxisUEn;

	chip.SetReg(RegNo,Data,AxisArray);

}

void WriteReg( unsigned short RegNo, unsigned short Data, short AxisNo)
{
	int mAxis;

	if(RegNo>0 && RegNo<4)
	{
		mAxis=GetmAxisValue(AxisNo==0, AxisNo==1, AxisNo==2, AxisNo==3);
		WriteIO( 0, (mAxis<<8)+15); 
	}
	WriteIO( RegNo, Data );

	chip.SetReg(RegNo,Data,AxisNo);
}

void WriteReg( unsigned short RegNo, unsigned short Data )
{
	WriteIO( RegNo, Data );
	if(RegNo==5)
	{
		//pMonitorIPL->m_Axis4_En = GetBit(Data,13);
		pMonitorIPL->UpdateWR5(Data);

	}

	chip.SetReg(RegNo,Data);
}

void SetSLMTP( unsigned short BitLevel, short AxisNo)
{
	unsigned short WR2Old;
	unsigned short WR2New;
	
	WR2Old = chip.GetReg( 2, AxisNo );
	WR2New = WR2Old & 0xfffe | BitLevel;
	chip.SetReg( 2, WR2New, AxisNo );

	int mAxis=GetmAxisValue(AxisNo);
	WriteIO( 0, (mAxis<<8)+15); 
	WriteIO( 2, WR2New );
}

void SetSLMTM( unsigned short BitLevel, short AxisNo)
{
	unsigned short WR2Old;
	unsigned short WR2New;
	
	WR2Old = chip.GetReg( 2, AxisNo );
	WR2New = WR2Old & 0xfffd | BitLevel<<1;
	chip.SetReg( 2, WR2New, AxisNo );

	int mAxis=GetmAxisValue(AxisNo);
	WriteIO( 0, (mAxis<<8)+15); 
	WriteIO( 2, WR2New );
}

void SetManld( unsigned short BitLevel, short AxisNo)
{
	unsigned short WR3Old;
	unsigned short WR3New;
	
	WR3Old = chip.GetReg( 3, AxisNo );
	WR3New = WR3Old & 0xfffe | BitLevel;
	chip.SetReg( 3, WR3New, AxisNo );

	int mAxis=GetmAxisValue(AxisNo==0, AxisNo==1, AxisNo==2, AxisNo==3);
	WriteIO( 0, (mAxis<<8)+15); 
	WriteIO( 3, WR3New );
}

void SetDsnde( unsigned short BitLevel, short AxisNo)
{
	unsigned short WR3Old;
	unsigned short WR3New;
	
	WR3Old = chip.GetReg( 3, AxisNo );
	WR3New = WR3Old & 0xfffd | BitLevel<<1;
	chip.SetReg( 3, WR3New, AxisNo );

	int mAxis=GetmAxisValue(AxisNo==0, AxisNo==1, AxisNo==2, AxisNo==3);
	WriteIO( 0, (mAxis<<8)+15); 
	WriteIO( 3, WR3New );
}

void SetSacc( unsigned short BitLevel, short AxisNo)
{
	unsigned short WR3Old;
	unsigned short WR3New;
	
	WR3Old = chip.GetReg( 3, AxisNo );
	WR3New = WR3Old & 0xfffb | BitLevel<<2;
	chip.SetReg( 3, WR3New, AxisNo );

	int mAxis=GetmAxisValue(AxisNo==0, AxisNo==1, AxisNo==2, AxisNo==3);
	WriteIO( 0, (mAxis<<8)+15); 
	WriteIO( 3, WR3New );
}

void SetExop( unsigned short BitLevel, short AxisNo)
{
	unsigned short WR3Old;
	unsigned short WR3New;
	
	WR3Old = chip.GetReg( 3, AxisNo );
	WR3New = WR3Old & 0xffe7 | BitLevel<<3;
	chip.SetReg( 3, WR3New, AxisNo );

	int mAxis=GetmAxisValue(AxisNo==0, AxisNo==1, AxisNo==2, AxisNo==3);
	WriteIO( 0, (mAxis<<8)+15); 
	WriteIO( 3, WR3New );
}

void ReadReg (unsigned short RegNo, short AxisXEn, short AxisYEn, short AxisZEn, short AxisUEn)
{
	int mAxis;
	if( (RegNo>0 && RegNo<4) || (RegNo>10 && RegNo<14) )
	{
		mAxis=GetmAxisValue(AxisXEn, AxisYEn, AxisZEn, AxisUEn);
		WriteIO( 0, (mAxis<<8)+15); 
	}
	ReadIO(RegNo);
}

void ReadReg (unsigned short RegNo )
{
	ReadIO(RegNo);
}

void WriteParam ( short ParamID, long Data , short AxisXEn, short AxisYEn, short AxisZEn, short AxisUEn)
{
	int mAxis;
	mAxis=GetmAxisValue(AxisXEn, AxisYEn, AxisZEn, AxisUEn);
	WriteIO( 6, (unsigned short)Data );
	WriteIO( 7, (unsigned short)(Data>>16) );
	WriteIO( 0, (mAxis<<8)+ParamID); 

	//if(ParamID==2 || ParamID==5 || ParamID==6 ) // A/V/P
	//	((CSMT5KView *)pMyView)->SetDrawRange(ParamID, Data , AxisXEn, AxisYEn, AxisZEn, AxisUEn);
}

void WriteParam ( short ParamID, long Data , short AxisNo)
{
	int mAxis;
	mAxis=GetmAxisValue(AxisNo==0, AxisNo==1, AxisNo==2, AxisNo==3);
	WriteIO( 6, (unsigned short)Data );
	WriteIO( 7, (unsigned short)(Data>>16) );
	WriteIO( 0, (mAxis<<8)+ParamID); 
	//if(ParamID==2 || ParamID==5 || ParamID==6 ) // A/V/P
	//	((CSMT5KView *)pMyView)->SetDrawRange(ParamID, Data , AxisNo==0, AxisNo==1, AxisNo==2, AxisNo==3);

}

void ReadParam ( short ParamID, short AxisXEn, short AxisYEn, short AxisZEn, short AxisUEn)
{
	int mAxis;
	mAxis=GetmAxisValue(AxisXEn, AxisYEn, AxisZEn, AxisUEn);
	WriteIO( 0, (mAxis<<8)+ParamID+16); 
	ReadIO(ParamID+20);
}


void SendDriveCommand( short DrvCmd, short AxisXEn, short AxisYEn, short AxisZEn, short AxisUEn )
{
	int mAxis;
	mAxis=GetmAxisValue(AxisXEn, AxisYEn, AxisZEn, AxisUEn);
	WriteIO( 0, (mAxis<<8)+DrvCmd ); 
	
	if(DrvCmd>=0x20 && DrvCmd<=0x25 )
	{
		if(!TimerOn)
			AfxGetMainWnd()->SendMessage(WM_SM5K_TIMER_ON, 0, 0 );
	}

	else if(DrvCmd>=0x30 && DrvCmd<=0x35)
	{

		if(!TimerOn)
			AfxGetMainWnd()->SendMessage(WM_SM5K_TIMER_ON, 0, 0 );

		if(DrvCmd==0x31 || DrvCmd==0x35)
		{
			if(pMonitorIPL->m_Axis4_En)
				pMonitorIPL->m_AxisRunIpl = 4;
			else
				pMonitorIPL->m_AxisRunIpl = 3;

		}
		else
				pMonitorIPL->m_AxisRunIpl = 2;
	}
}

void SendDriveCommand( short DrvCmd, short AxisNo )
{
	int mAxis;
	mAxis=GetmAxisValue(AxisNo==0, AxisNo==1, AxisNo==2, AxisNo==3);
	WriteIO( 0, (mAxis<<8)+DrvCmd ); 

	if(DrvCmd>=0x20 && DrvCmd<=0x25 )
	{
		if(!TimerOn)
			AfxGetMainWnd()->SendMessage(WM_SM5K_TIMER_ON, 0, 0 );
	}

	else if(DrvCmd>=0x30 && DrvCmd<=0x35)
	{

		if(!TimerOn)
			AfxGetMainWnd()->SendMessage(WM_SM5K_TIMER_ON, 0, 0 );

		if(DrvCmd==0x31 || DrvCmd==0x35)
		{
			if(pMonitorIPL->m_Axis4_En)
				pMonitorIPL->m_AxisRunIpl = 4;
			else
				pMonitorIPL->m_AxisRunIpl = 3;

		}
		else
				pMonitorIPL->m_AxisRunIpl = 2;
	}
}


int  cv_move(int AxisNo,int MaxVel, int Dir)
{
	WriteParam ( R_ADDR, R_DEFAULT , AxisNo);
	WriteParam ( SV_ADDR, MaxVel , AxisNo);
	WriteParam ( V_ADDR,  MaxVel , AxisNo);
	WriteParam ( A_ADDR, 100 , AxisNo);

	if( g_ExppEn )  return 0;

	if(Dir==0)
		SendDriveCommand( 0x22, AxisNo );
	else
		SendDriveCommand( 0x23, AxisNo );

	return 0;
}

int  tv_move(int AxisNo,int StrVel, int MaxVel, float Tacc, int Dir)
{
	CString str;
	int A;
	A=(MaxVel-StrVel)/(Tacc*125);
	if(A==0) A = 20;

	//WriteReg( 3, 0, AxisNo);		// MANLD=0,DSNDE=0
		SetManld(0, AxisNo );
		SetDsnde(0, AxisNo );
		SetSacc (0, AxisNo );

	WriteParam ( R_ADDR, R_DEFAULT , AxisNo);
	WriteParam ( A_ADDR, A , AxisNo);
	WriteParam ( SV_ADDR, StrVel , AxisNo);
	WriteParam ( V_ADDR, MaxVel , AxisNo);

	if( g_ExppEn )  return 0;

	if(Dir==DIR_POS)
		SendDriveCommand( 0x22, AxisNo );
	else
		SendDriveCommand( 0x23, AxisNo );

	return 0;
}

int  sv_move(int AxisNo,int StrVel, int MaxVel, float Tacc, int Vsacc ,int Dir)
{
	CString str;
	int A;
	int K;
	float Tsacc;
	float MaxA;
	float Jerk;
	int NewMaxVel;
	int NewVsacc;

	NewMaxVel = MaxVel;
	NewVsacc = Vsacc==0? (StrVel+MaxVel)/2 : Vsacc;

	MaxA = (NewMaxVel + 2*NewVsacc - 3*StrVel)/Tacc;
	Tsacc = 2*(NewVsacc - StrVel)/MaxA ;
	Jerk = MaxA/Tsacc;
	K = 62500000/Jerk;
	A=MaxA/125;

		SetManld(0, AxisNo );
		SetDsnde(0, AxisNo );
		SetSacc (1, AxisNo );

	WriteParam ( R_ADDR, R_DEFAULT , AxisNo);
	WriteParam ( K_ADDR, K , AxisNo);
	WriteParam ( A_ADDR, A , AxisNo);
	WriteParam ( SV_ADDR, StrVel , AxisNo);
	WriteParam ( V_ADDR,  MaxVel , AxisNo);

	if( g_ExppEn )  return 0;

	if(Dir==DIR_POS)
		SendDriveCommand( 0x22, AxisNo==0, AxisNo==1, AxisNo==2, AxisNo==3 );
	else
		SendDriveCommand( 0x23, AxisNo==0, AxisNo==1, AxisNo==2, AxisNo==3 );

	return 0;
}

int  start_ca_move(int AxisNo, int Pos , int MaxVel)
{
	int Dist;
	int LP;

	LP = pMonitorAxis[0]->GetLP(AxisNo);	// should Read LP from chip in real time,here for temprary
	Dist = Pos - LP;

	start_cr_move(AxisNo,Dist,MaxVel);

	return 0;
}

int  start_cr_move(int AxisNo, int Dist, int MaxVel)
{
	WriteParam (R_ADDR, R_DEFAULT , AxisNo);
	WriteParam (SV_ADDR, MaxVel , AxisNo);
	WriteParam (V_ADDR,  MaxVel , AxisNo);
	WriteParam (P_ADDR, abs(Dist) , AxisNo);

	if( g_ExppEn ) 
	{
		return 0;
	}

	if(Dist>=0)
	{
		SendDriveCommand(0x20, AxisNo );
	}
	else
	{
		SendDriveCommand(0x21, AxisNo );
	}

	return 0;
}

int  start_ta_move(int AxisNo, int Pos , int StrVel, int MaxVel, float Tacc, float Tdec)
{
	int Dist;
	int LP;

	LP = pMonitorAxis[0]->GetLP(AxisNo);	// should Read LP from chip in real time,here for temprary
	Dist = Pos - LP;

	start_tr_move(AxisNo, Dist, StrVel, MaxVel, Tacc, Tdec );

	return 0;
}

int  start_tr_move(int AxisNo, int Dist, int StrVel, int MaxVel, float Tacc, float Tdec)

{
	CString str;
	int A,D;
	int DP,P;
	double AP,DP0;

	A=(MaxVel-StrVel)/(Tacc*125)+1;
	D=(MaxVel-StrVel)/(Tdec*125)+1;

	P = abs(Dist);
	AP = (StrVel+ MaxVel)*Tacc/2;
	DP0 = (StrVel+ MaxVel)*Tdec/2;
	if(AP+DP0>P)
		DP0 = P*DP0/(AP+DP0);

	DP = P - DP0 - g_AO;


	WriteParam ( R_ADDR, R_DEFAULT , AxisNo);
	WriteParam ( A_ADDR, A , AxisNo);
	WriteParam ( D_ADDR, D , AxisNo);
	WriteParam ( SV_ADDR, StrVel , AxisNo);
	WriteParam ( V_ADDR,  MaxVel , AxisNo);
	WriteParam ( P_ADDR, abs(Dist) , AxisNo);
	WriteParam ( DP_ADDR, DP , AxisNo);

	//WriteReg( 3, 3, AxisNo);		// MANLD=1,DSNDE=1
	SetManld(!g_AutoDec, AxisNo );
	SetDsnde(!g_AutoDec, AxisNo );
	SetSacc (0, AxisNo );

	if( g_ExppEn )  return 0;

	if(Dist>=0)
		SendDriveCommand( 0x20, AxisNo );
	else