📄 sm5000.cpp
字号:
SendDriveCommand( 0x21, AxisNo );
return 0;
}
int start_sa_move(int AxisNo, int Pos , int StrVel, int MaxVel, float Tacc, float Tdec, int Vsacc, int Vsdec )
{
int Dist;
int LP;
LP = pMonitorAxis[0]->GetLP(AxisNo); // should Read LP from chip in real time,here for temprary
Dist = Pos - LP;
start_sr_move(AxisNo, Dist, StrVel, MaxVel, Tacc, Tdec, Vsacc, Vsdec );
return 0;
}
int start_sr_move(int AxisNo, int Dist, int StrVel, int MaxVel, float Tacc, float Tdec, int Vsacc, int Vsdec)
{
CString str;
int A;
int K;
float Tsacc;
float MaxA;
float Jerk;
int DP,DP0;
int P;
int NewMaxVel;
int NewVsacc;
int NewTdec;
NewTdec = Tacc;
DP0 = (StrVel+ MaxVel)*NewTdec/2;
P = abs(Dist);
if(DP0>P/2)
{
DP0 = P/2;
NewMaxVel = DP0*2/NewTdec - StrVel;
NewVsacc = (StrVel+NewMaxVel)/2;
}
else
{
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;
DP = P - DP0 - g_AO;
//WriteReg( 3, 5, AxisNo); // MANLD=1,DSNDE=0,SACC=1
SetManld(!g_AutoDec, 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, NewMaxVel , AxisNo);
WriteParam ( P_ADDR, abs(Dist) , AxisNo);
WriteParam ( DP_ADDR, DP , AxisNo);
if( g_ExppEn ) return 0;
if(Dist>=0)
SendDriveCommand( 0x20, AxisNo );
else
SendDriveCommand( 0x21, AxisNo );
return 0;
}
int inp_line2(int *AxisArray, int DistX, int DistY)
{
WriteParam ( P_ADDR, DistX , AxisArray[0]);
WriteParam ( P_ADDR, DistY , AxisArray[1]);
SendDriveCommand( 0x30, 0, 0, 0, 0 );
return 0;
}
int inp_line3(int *AxisArray, int DistX, int DistY, int DistZ)
{
WriteParam ( P_ADDR, DistX , AxisArray[0]);
WriteParam ( P_ADDR, DistY , AxisArray[1]);
WriteParam ( P_ADDR, DistZ , AxisArray[2]);
SendDriveCommand( 0x31, 0, 0, 0, 0 );
return 0;
}
int inp_line4(int *AxisArray, int DistX, int DistY, int DistZ, int DistU)
{
WriteParam ( P_ADDR, DistX , AxisArray[0]);
WriteParam ( P_ADDR, DistY , AxisArray[1]);
WriteParam ( P_ADDR, DistZ , AxisArray[2]);
WriteParam ( P_ADDR, DistU , AxisArray[3]);
SendDriveCommand( 0x31, 0, 0, 0, 0 );
return 0;
}
int inp_arc2( int *AxisArray, int OffsetCx, int OffsetCy, int OffsetEx, int OffsetEy, int Dir )
{
WriteParam ( P_ADDR, OffsetEx , AxisArray[0]);
WriteParam ( P_ADDR, OffsetEy , AxisArray[1]);
WriteParam ( C_ADDR, OffsetCx , AxisArray[0]);
WriteParam ( C_ADDR, OffsetCy , AxisArray[1]);
if(Dir==ARC_TYPE_CCW)
SendDriveCommand( 0x33, 0, 0, 0, 0 );
else
SendDriveCommand( 0x32, 0, 0, 0, 0 );
return 0;
}
int start_tr_line2(int *AxisArray, int DistX, int DistY, int StrVel, int MaxVel, float Tacc, float Tdec )
{
CString str;
int A,D;
int DP,P;
double AP,DPn;
A=(MaxVel-StrVel)/(Tacc*125)+1;
D=(MaxVel-StrVel)/(Tdec*125)+1;
P = max(abs(DistX),abs(DistY));
AP = (StrVel+ MaxVel)*Tacc/2;
DPn = (StrVel+ MaxVel)*Tdec/2;
if(AP+DPn>P)
{
DPn = P*DPn/(AP+DPn);
}
DP = P - DPn - g_AO;
WriteParam (A_ADDR, A, AxisArray[0]);
WriteParam (D_ADDR, D, AxisArray[0]);
//WriteReg ( 3, 3 , AxisArray[0]); // MANLD=1,DSNDE=1
SetManld(!g_AutoDec, AxisArray[0] );
SetDsnde(!g_AutoDec, AxisArray[0] );
SetSacc (0, AxisArray[0] );
WriteParam ( DP_ADDR, DP, AxisArray[0]);
WriteParam ( SV_ADDR, StrVel, AxisArray[0]);
WriteParam ( V_ADDR, MaxVel, AxisArray[0]);
WriteParam ( R_ADDR, R_DEFAULT, AxisArray[0]);
WriteParam ( R_ADDR, R_DEFAULT_ROOT2 , AxisArray[1]);
WriteParam ( P_ADDR, DistX, AxisArray[0]);
WriteParam ( P_ADDR, DistY, AxisArray[1]);
SendDriveCommand( 0x30, 0, 0, 0, 0 );
return 0;
}
int start_sr_line2(int *AxisArray, int DistX, int DistY, int StrVel, int MaxVel, float Tacc, float Tdec, int Vsacc, int Vsdec )
{
CString str;
int A;
int K;
float Tsacc;
float MaxA;
float Jerk;
int DP,DP0;
int P;
int NewMaxVel;
int NewVsacc;
int NewTdec;
NewTdec = Tacc;
DP0 = (StrVel+ MaxVel)*NewTdec/2;
P = max(abs(DistX),abs(DistY));
if(DP0>P/2)
{
DP0 = P/2;
NewMaxVel = DP0*2/NewTdec - StrVel;
NewVsacc = (StrVel+NewMaxVel)/2;
}
else
{
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;
DP = P - DP0 - g_AO;
//WriteReg ( 3, 5 , AxisArray[0]); // MANLD=1,DSNDE=0,SACC=1
SetManld(!g_AutoDec, AxisArray[0] );
SetDsnde(0, AxisArray[0] );
SetSacc (1, AxisArray[0] );
WriteParam ( K_ADDR, K , AxisArray[0]);
WriteParam ( A_ADDR, A , AxisArray[0]);
WriteParam ( DP_ADDR, DP , AxisArray[0]);
WriteParam ( SV_ADDR, StrVel , AxisArray[0]);
WriteParam ( V_ADDR, NewMaxVel , AxisArray[0]);
WriteParam ( R_ADDR, R_DEFAULT , AxisArray[0]);
WriteParam ( R_ADDR, R_DEFAULT_ROOT2 , AxisArray[1]);
WriteParam ( P_ADDR, DistX , AxisArray[0]);
WriteParam ( P_ADDR, DistY , AxisArray[1]);
SendDriveCommand( 0x30, 0, 0, 0, 0 );
return 0;
}
int start_tr_line3(int *AxisArray, int DistX, int DistY, int DistZ, int StrVel, int MaxVel, float Tacc, float Tdec )
{
CString str;
int A,D;
int DP,P;
double AP,DPn;
A=(MaxVel-StrVel)/(Tacc*125)+1;
D=(MaxVel-StrVel)/(Tdec*125)+1;
P = max(max(abs(DistX),abs(DistY)),abs(DistZ));
AP = (StrVel+ MaxVel)*Tacc/2;
DPn = (StrVel+ MaxVel)*Tdec/2;
if(AP+DPn>P)
DPn = P*DPn/(AP+DPn);
DP = P - DPn - g_AO;
WriteParam ( A_ADDR, A , AxisArray[0]);
WriteParam ( D_ADDR, D , AxisArray[0]);
//WriteReg ( 3, 3 , AxisArray[0]); // MANLD=1,DSNDE=1
SetManld(!g_AutoDec, AxisArray[0] );
SetDsnde(!g_AutoDec, AxisArray[0] );
SetSacc (0, AxisArray[0] );
WriteParam ( DP_ADDR, DP , AxisArray[0]);
WriteParam ( SV_ADDR, StrVel , AxisArray[0]);
WriteParam ( V_ADDR, MaxVel , AxisArray[0]);
WriteParam ( R_ADDR, R_DEFAULT , AxisArray[0]);
WriteParam ( R_ADDR, R_DEFAULT_ROOT2 , AxisArray[1]);
WriteParam ( R_ADDR, R_DEFAULT_ROOT3 , AxisArray[2]);
WriteParam ( P_ADDR, DistX , AxisArray[0]);
WriteParam ( P_ADDR, DistY , AxisArray[1]);
WriteParam ( P_ADDR, DistZ , AxisArray[2]);
SendDriveCommand( 0x31, 0, 0, 0, 0 );
return 0;
}
int start_sr_line3(int *AxisArray, int DistX, int DistY, int DistZ, int StrVel, int MaxVel, float Tacc, float Tdec, int Vsacc, int Vsdec )
{
CString str;
int A;
int K;
float Tsacc;
float MaxA;
float Jerk;
int DP,DP0;
int P;
int NewMaxVel;
int NewVsacc;
int NewTdec;
NewTdec = Tacc;
DP0 = (StrVel+ MaxVel)*NewTdec/2;
P = max(max(abs(DistX),abs(DistY)),abs(DistZ));
if(DP0>P/2)
{
DP0 = P/2;
NewMaxVel = DP0*2/NewTdec - StrVel;
NewVsacc = (StrVel+NewMaxVel)/2;
}
else
{
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;
DP = P - DP0 - g_AO;
//WriteReg ( 3, 5 , AxisArray[0]); // MANLD=1,DSNDE=0,SACC=1
SetManld(!g_AutoDec, AxisArray[0] );
SetDsnde(0, AxisArray[0] );
SetSacc (1, AxisArray[0] );
WriteParam ( K_ADDR, K , AxisArray[0]);
WriteParam ( A_ADDR, A , AxisArray[0]);
WriteParam ( DP_ADDR, DP , AxisArray[0]);
WriteParam ( SV_ADDR, StrVel , AxisArray[0]);
WriteParam ( V_ADDR, NewMaxVel , AxisArray[0]);
WriteParam ( R_ADDR, R_DEFAULT , AxisArray[0]);
WriteParam ( R_ADDR, R_DEFAULT_ROOT2 , AxisArray[1]);
WriteParam ( R_ADDR, R_DEFAULT_ROOT3 , AxisArray[2]);
WriteParam ( P_ADDR, DistX , AxisArray[0]);
WriteParam ( P_ADDR, DistY , AxisArray[1]);
WriteParam ( P_ADDR, DistZ , AxisArray[2]);
SendDriveCommand( 0x31, 0, 0, 0, 0 );
return 0;
}
int start_tr_line4(int *AxisArray, int DistX, int DistY, int DistZ, int DistU, int StrVel, int MaxVel, float Tacc, float Tdec )
{
CString str;
int A,D;
int DP,P;
double AP,DPn;
A=(MaxVel-StrVel)/(Tacc*125)+1;
D=(MaxVel-StrVel)/(Tdec*125)+1;
P = max(max(abs(DistX),abs(DistY)),max(abs(DistZ),abs(DistU)));
AP = (StrVel+ MaxVel)*Tacc/2;
DPn = (StrVel+ MaxVel)*Tdec/2;
if(AP+DPn>P)
DPn = P*DPn/(AP+DPn);
DP = P - DPn - g_AO;
WriteParam ( A_ADDR, A , AxisArray[0]);
WriteParam ( D_ADDR, D , AxisArray[0]);
//WriteReg ( 3, 3 , AxisArray[0]); // MANLD=1,DSNDE=1
SetManld(!g_AutoDec, AxisArray[0] );
SetDsnde(!g_AutoDec, AxisArray[0] );
SetSacc (0, AxisArray[0] );
WriteParam ( DP_ADDR, DP , AxisArray[0]);
WriteParam ( SV_ADDR, StrVel , AxisArray[0]);
WriteParam ( V_ADDR, MaxVel , AxisArray[0]);
WriteParam ( R_ADDR, R_DEFAULT , AxisArray[0]);
WriteParam ( R_ADDR, R_DEFAULT_ROOT2 , AxisArray[1]);
WriteParam ( R_ADDR, R_DEFAULT_ROOT3 , AxisArray[2]);
WriteParam ( P_ADDR, DistX , AxisArray[0]);
WriteParam ( P_ADDR, DistY , AxisArray[1]);
WriteParam ( P_ADDR, DistZ , AxisArray[2]);
WriteParam ( P_ADDR, DistU , AxisArray[3]);
SendDriveCommand( 0x31, 0, 0, 0, 0 );
return 0;
}
int start_sr_line4(int *AxisArray, int DistX, int DistY, int DistZ, int DistU, int StrVel, int MaxVel, float Tacc, float Tdec, int Vsacc, int Vsdec )
{
CString str;
int A;
int K;
float Tsacc;
float MaxA;
float Jerk;
int DP,DP0;
int P;
int NewMaxVel;
int NewVsacc;
int NewTdec;
NewTdec = Tacc;
DP0 = (StrVel+ MaxVel)*NewTdec/2;
P = max(max(abs(DistX),abs(DistY)),max(abs(DistZ),abs(DistU)));
if(DP0>P/2)
{
DP0 = P/2;
NewMaxVel = DP0*2/NewTdec - StrVel;
NewVsacc = (StrVel+NewMaxVel)/2;
}
else
{
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;
DP = P - DP0 - g_AO;
//WriteReg ( 3, 5 , AxisArray[0]); // MANLD=1,DSNDE=0,SACC=1
SetManld(!g_AutoDec, AxisArray[0] );
SetDsnde(0, AxisArray[0] );
SetSacc (1, AxisArray[0] );
WriteParam ( K_ADDR, K , AxisArray[0]);
WriteParam ( A_ADDR, A , AxisArray[0]);
WriteParam ( DP_ADDR, DP , AxisArray[0]);
WriteParam ( SV_ADDR, StrVel , AxisArray[0]);
WriteParam ( V_ADDR, NewMaxVel , AxisArray[0]);
WriteParam ( R_ADDR, R_DEFAULT , AxisArray[0]);
WriteParam ( R_ADDR, R_DEFAULT_ROOT2 , AxisArray[1]);
WriteParam ( R_ADDR, R_DEFAULT_ROOT3 , AxisArray[2]);
WriteParam ( P_ADDR, DistX , AxisArray[0]);
WriteParam ( P_ADDR, DistY , AxisArray[1]);
WriteParam ( P_ADDR, DistZ , AxisArray[2]);
WriteParam ( P_ADDR, DistU , AxisArray[3]);
SendDriveCommand( 0x31, 0, 0, 0, 0 );
return 0;
}
int start_tr_arc2( int *AxisArray, int OffsetCx, int OffsetCy, int OffsetEx, int OffsetEy, int Dir, int StrVel, int MaxVel, float Tacc, float Tdec )
{
CString str;
int A,D;
int DP,P;
double AP,DPn;
A=(MaxVel-StrVel)/(Tacc*125)+1;
D=(MaxVel-StrVel)/(Tdec*125)+1;
P = ArcLength(OffsetCx,OffsetCy,OffsetEx,OffsetEy,Dir);
AP = (StrVel+ MaxVel)*Tacc/2;
DPn = (StrVel+ MaxVel)*Tdec/2;
if(AP+DPn>P)
DPn = P*DPn/(AP+DPn);
DP = P - DPn - g_AO;
if(StrVel<MaxVel)
{
WriteParam ( A_ADDR, A , AxisArray[0]);
WriteParam ( D_ADDR, D , AxisArray[0]);
SetManld(1, AxisArray[0] );
SetDsnde(1, AxisArray[0] );
SetSacc (0, AxisArray[0] );
WriteParam ( DP_ADDR, DP , AxisArray[0]);
}
WriteParam ( SV_ADDR, StrVel , AxisArray[0]);
WriteParam ( V_ADDR, MaxVel , AxisArray[0]);
WriteParam ( R_ADDR, R_DEFAULT , AxisArray[0]);
WriteParam ( R_ADDR, R_DEFAULT_ROOT2 , AxisArray[1]);
WriteParam ( P_ADDR, OffsetEx , AxisArray[0]);
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -