📄 sm5000.cpp
字号:
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_sr_arc2( int *AxisArray, int OffsetCx, int OffsetCy, int OffsetEx, int OffsetEy, int Dir, 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;
int P;
int NewMaxVel;
int NewVsacc;
int NewTdec;
NewTdec = Tacc;
DP = (StrVel+ MaxVel)*NewTdec/2;
P = ArcLength(OffsetCx,OffsetCy,OffsetEx,OffsetEy,Dir);
if(DP>P/2)
{
DP = P/2;
NewMaxVel = DP*2/NewTdec - StrVel;
NewVsacc = (StrVel+NewMaxVel)/2;
}
else
{
NewMaxVel = MaxVel;
NewVsacc = Vsacc==0? (StrVel+MaxVel)/2 : Vsacc;
}
DP = P - DP - g_AO;
MaxA = (NewMaxVel + 2*NewVsacc - 3*StrVel)/Tacc;
Tsacc = 2*(NewVsacc - StrVel)/MaxA ;
Jerk = MaxA/Tsacc;
K = 62500000/Jerk;
A=MaxA/125;
if(StrVel<MaxVel)
{
SetManld(1, 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, OffsetEx , AxisArray[0]);
WriteParam ( P_ADDR, OffsetEy , AxisArray[1]);
WriteParam ( C_ADDR, OffsetCx , AxisArray[0]);
WriteParam ( C_ADDR, OffsetCy , AxisArray[1]);
if(Dir==1)
SendDriveCommand( 0x33, 0, 0, 0, 0 );
else
SendDriveCommand( 0x32, 0, 0, 0, 0 );
return 0;
}
int ArcLength(int Cx,int Cy,int Ex,int Ey,int Dir)
{
int S0x,S0y,E0x,E0y,ZoneS,ZoneE,R,DeltaZone,Soffset,Eoffset,Length,result;
S0x = -Cx;
S0y = -Cy;
E0x = Ex - Cx;
E0y = Ey - Cy;
R =sqrt((double)Cx*(double)Cx+(double)Cy*(double)Cy);
ZoneS = Zone(S0x,S0y);
ZoneE = Zone(E0x,E0y);
DeltaZone = ZoneE-ZoneS;
if(DeltaZone<0) DeltaZone+=8;
Soffset = ArcOffset(S0x,S0y,R,ZoneS);
Eoffset = ArcOffset(E0x,E0y,R,ZoneE);
Length = DeltaZone*R*sqrt(2)/2-Soffset+Eoffset;
if(Length<=0) Length += R*4*sqrt(2);
if(Dir==ARC_TYPE_CCW)
result = Length;
else
result = R*4*sqrt(2) - Length;
if(result==0)
result = R*4*sqrt(2);
return result;
}
int ArcOffset(int X, int Y, int R, int Zone)
{
int xtemp = abs(X);
int ytemp = abs(Y);
int rtemp = R*sqrt(2)/2;
int result;
switch(Zone)
{
case 0 : result = ytemp ;break;
case 1 : result = rtemp -xtemp ;break;
case 2 : result = xtemp ;break;
case 3 : result = rtemp -ytemp ;break;
case 4 : result = ytemp ;break;
case 5 : result = rtemp -xtemp ;break;
case 6 : result = xtemp ;break;
case 7 : result = rtemp -ytemp ;break;
default: result = 0;
}
return result;
}
int Zone(int x, int y)
{
int absx = abs(x);
int absy = abs(y);
if (x >0 && y>=0 && absx> absy) return 0;
else if(x >0 && y> 0 && absx<=absy) return 1;
else if(x<=0 && y> 0 && absx< absy) return 2;
else if(x< 0 && y> 0 && absx>=absy) return 3;
else if(x< 0 && y<=0 && absx> absy) return 4;
else if(x< 0 && y< 0 && absx<=absy) return 5;
else if(x>=0 && y< 0 && absx< absy) return 6;
else if(x> 0 && y< 0 && absx>=absy) return 7;
else return 0;
}
int start_c1r_bit2(int *AxisArray,m_BP1Array *BP1Array,m_BP2Array *BP2Array,int MaxVel)
{
int i = 0;
m_BP1Array *head1 = NULL;
m_BP2Array *head2 = NULL;
WriteParam ( SV_ADDR, MaxVel , AxisArray[0]);
WriteParam ( V_ADDR, MaxVel , AxisArray[0]);
WriteParam ( R_ADDR, R_DEFAULT , AxisArray[0]);
WriteParam ( R_ADDR, R_DEFAULT_ROOT2 , AxisArray[1]);
WriteReg ( 0, 0x39 ); // BP_DATA_CLR
WriteReg ( 0, 0x36 ); // BP_REG_WEN
head1 = BP1Array;
head2 = BP2Array;
for (i=0;head1!=NULL && head2!=NULL && i<31;i++)
{
WriteBpReg ( 2, head1->m_number1 ); // BP1P
WriteBpReg ( 3, head1->m_number2 ); // BP1M
WriteBpReg ( 4, head2->m_number1 ); // BP2P
WriteBpReg ( 5, head2->m_number2 ); // BP2M
WriteReg ( 0, 0x38 ); // BP_DATA_STACK
head1 = head1->next;
head2 = head2->next;
}
m_ComPort.WriteToPort(LPCTSTR(m_strTXDataCOM1));
sleep(550);
m_strTXDataCOM1.Empty();
if (head1==NULL || head2==NULL)
{
return 0;
}
else
{
while (head1!=NULL && head2!=NULL)
{
WriteBpReg ( 2, head1->m_number1 ); // BP1P
WriteBpReg ( 3, head1->m_number2 ); // BP1M
WriteBpReg ( 4, head2->m_number1 ); // BP2P
WriteBpReg ( 5, head2->m_number2 ); // BP2M
WriteReg ( 0, 0x38 ); // BP_DATA_STACK
SendDriveCommand(0x34, 0, 0, 0, 0 );
i++;
if (i%31 == 0)
{
m_ComPort.WriteToPort(LPCTSTR(m_strTXDataCOM1));
Sleep(500);
m_strTXDataCOM1.Empty();
}
head1 = head1->next;
head2 = head2->next;
}
m_ComPort.WriteToPort(LPCTSTR(m_strTXDataCOM1));
Sleep(50);
m_strTXDataCOM1.Empty();
WriteReg(0, 0x37 ); // BP_REG_WEN_DIS
}
return 0;
}
int start_c1r_bit4(int *AxisArray,m_BP1Array *BP1Array,m_BP2Array *BP2Array,
m_BP3Array *BP3Array,m_BP4Array *BP4Array,int MaxVel)
{
int i = 0;
m_BP1Array *head1 = NULL;
m_BP2Array *head2 = NULL;
m_BP3Array *head3 = NULL;
m_BP4Array *head4 = NULL;
WriteParam ( SV_ADDR, MaxVel , 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]);
WriteReg ( 0, 0x39 ); // BP_DATA_CLR
WriteReg ( 0, 0x36 ); // BP_REG_WEN
head1 = BP1Array;
head2 = BP2Array;
head3 = BP3Array;
head4 = BP4Array;
for (i=0;head1!=NULL && head2!=NULL && head3!=NULL && head4!=NULL && i<20;i++)
{
WriteBpReg( 2, head1->m_number1); // BP1P
WriteBpReg( 3, head1->m_number2); // BP1M
WriteBpReg( 4, head2->m_number1); // BP2P
WriteBpReg( 5, head2->m_number2); // BP2M
WriteBpReg( 6, head3->m_number1); // BP2P
WriteBpReg( 7, head3->m_number2); // BP2M
WriteBpReg( 1, head4->m_number1); // BP2P
WriteBpReg( 1, head4->m_number2); // BP2M
WriteReg(0, 0x38 ); // BP_DATA_STACK
}
SendDriveCommand(0x35, 0, 0, 0, 0 );
m_ComPort.WriteToPort(LPCTSTR(m_strTXDataCOM1));
Sleep(380);
m_strTXDataCOM1.Empty();
if (head1==NULL || head2==NULL)
{
return 0;
}
else
{
while (head1!=NULL && head2!=NULL && head3!=NULL && head4!=NULL)
{
WriteBpReg ( 2, head1->m_number1 ); // BP1P
WriteBpReg ( 3, head1->m_number2 ); // BP1M
WriteBpReg ( 4, head2->m_number1 ); // BP2P
WriteBpReg ( 5, head2->m_number2 ); // BP2M
WriteBpReg ( 6, head3->m_number1 ); // BP2P
WriteBpReg ( 7, head3->m_number2 ); // BP2M
WriteBpReg ( 1, head4->m_number1 ); // BP2P
WriteBpReg ( 1, head4->m_number2 ); // BP2M
WriteReg ( 0, 0x38 ); // BP_DATA_STACK
SendDriveCommand(0x35, 0, 0, 0, 0 );
i++;
if (i%20 == 0)
{
m_ComPort.WriteToPort(LPCTSTR(m_strTXDataCOM1));
Sleep(320);
m_strTXDataCOM1.Empty();
}
head1 = head1->next;
head2 = head2->next;
head3 = head3->next;
head4 = head4->next;
}
m_ComPort.WriteToPort(LPCTSTR(m_strTXDataCOM1));
Sleep(100);
m_strTXDataCOM1.Empty();
WriteReg(0, 0x37 ); // BP_REG_WEN_DIS
}
m_ComPort.WriteToPort(LPCTSTR(m_strTXDataCOM1));
Sleep(10);
return 0;
}
int start_c1r_bit3(int *AxisArray,m_BP1Array *BP1Array,m_BP2Array *BP2Array,
m_BP3Array *BP3Array,int MaxVel)
{
int i = 0;
m_BP1Array *head1 = NULL;
m_BP2Array *head2 = NULL;
m_BP3Array *head3 = NULL;
WriteParam ( SV_ADDR, MaxVel , 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]);
WriteReg (0, 0x39 ); // BP_DATA_CLR
WriteReg (0, 0x36 ); // BP_REG_WEN
head1 = BP1Array;
head2 = BP2Array;
head3 = BP3Array;
for (i=0;head1!=NULL && head2!=NULL && head3!=NULL&& i<20;i++)
{
WriteBpReg ( 2, head1->m_number1 ); // BP1P
WriteBpReg ( 3, head1->m_number2 ); // BP1M
WriteBpReg ( 4, head2->m_number1 ); // BP2P
WriteBpReg ( 5, head2->m_number2 ); // BP2M
WriteBpReg ( 6, head3->m_number1 ); // BP2P
WriteBpReg ( 7, head3->m_number2 ); // BP2M
WriteReg(0, 0x38 ); // BP_DATA_STACK
}
SendDriveCommand(0x35, 0, 0, 0, 0 );
m_ComPort.WriteToPort(LPCTSTR(m_strTXDataCOM1));
Sleep(380);
m_strTXDataCOM1.Empty();
if (head1==NULL || head2==NULL)
{
return 0;
}
else
{
while (head1!=NULL && head2!=NULL && head3!=NULL)
{
WriteBpReg ( 2, head1->m_number1 ); // BP1P
WriteBpReg ( 3, head1->m_number2 ); // BP1M
WriteBpReg ( 4, head2->m_number1 ); // BP2P
WriteBpReg ( 5, head2->m_number2 ); // BP2M
WriteBpReg ( 6, head3->m_number1 ); // BP2P
WriteBpReg ( 7, head3->m_number2 ); // BP2M
WriteReg ( 0, 0x38 ); // BP_DATA_STACK
SendDriveCommand(0x35, 0, 0, 0, 0 );
i++;
if (i%20 == 0)
{
m_ComPort.WriteToPort(LPCTSTR(m_strTXDataCOM1));
Sleep(320);
m_strTXDataCOM1.Empty();
}
head1 = head1->next;
head2 = head2->next;
head3 = head3->next;
}
m_ComPort.WriteToPort(LPCTSTR(m_strTXDataCOM1));
Sleep(100);
m_strTXDataCOM1.Empty();
WriteReg(0, 0x37 ); // BP_REG_WEN_DIS
}
m_ComPort.WriteToPort(LPCTSTR(m_strTXDataCOM1));
Sleep(10);
return 0;
}
int start_cr_bit2( int *AxisArray, int *BP1PArray, int *BP1MArray, int *BP2PArray,
int *BP2MArray, int MaxVel )
{
int i;
WriteParam (SV_ADDR, MaxVel , AxisArray[0]);
WriteParam (V_ADDR, MaxVel , AxisArray[0]);
WriteParam (R_ADDR, R_DEFAULT , AxisArray[0]);
WriteParam (R_ADDR, R_DEFAULT_ROOT2 , AxisArray[1]);
WriteReg(0, 0x39 ); // BP_DATA_CLR
WriteReg(0, 0x36 ); // BP_REG_WEN
for(i=0;i<16;i++)
{
WriteBpReg ( 2, BP1PArray[i] ); // BP1P
WriteBpReg ( 3, BP1MArray[i] ); // BP1M
WriteBpReg ( 4, BP2PArray[i] ); // BP2P
WriteBpReg ( 5, BP2MArray[i] ); // BP2M
WriteReg ( 0, 0x38 ); // BP_DATA_STACK
}
WriteReg ( 0, 0x37 ); // BP_REG_WEN_DIS
SendDriveCommand( 0x34, 0, 0, 0, 0 );
return 0;
}
int start_cr_bit3( int *AxisArray, int *BP1PArray, int *BP1MArray, int *BP2PArray, int *BP2MArray, int *BP3PArray, int *BP3MArray, int MaxVel )
{
int i;
WriteParam ( SV_ADDR, MaxVel , 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]);
WriteReg ( 0, 0x39 ); // BP_DATA_CLR
WriteReg ( 0, 0x36 ); // BP_REG_WEN
for(i=0;i<16;i++)
{
WriteBpReg ( 2, BP1PArray[i] ); // BP1P
WriteBpReg ( 3, BP1MArray[i] ); // BP1M
WriteBpReg ( 4, BP2PArray[i] ); // BP2P
WriteBpReg ( 5, BP2MArray[i] ); // BP2M
WriteBpReg ( 6, BP3PArray[i] ); // BP3P
WriteBpReg ( 7, BP3MArray[i] ); // BP3M
WriteReg ( 0, 0x38 ); // BP_DATA_STACK
}
WriteReg ( 0, 0x37 ); // BP_REG_WEN_DIS
SendDriveCommand( 0x35, 0, 0, 0, 0 );
return 0;
}
int start_cr_bit4( int *AxisArray, int *BP1PArray, int *BP1MArray, int *BP2PArray, int *BP2MArray, int *BP3PArray, int *BP3MArray, int *BP4PArray, int *BP4MArray, int MaxVel )
{
int i;
WriteParam ( SV_ADDR, MaxVel , 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]);
WriteReg ( 0, 0x39 ); // BP_DATA_CLR
WriteReg ( 0, 0x36 ); // BP_REG_WEN
for(i=0;i<16;i++)
{
WriteBpReg ( 2, BP1PArray[i] ); // BP1P
WriteBpReg ( 3, BP1MArray[i] ); // BP1M
WriteBpReg ( 4, BP2PArray[i] ); // BP2P
WriteBpReg ( 5, BP2MArray[i] ); // BP2M
WriteBpReg ( 6, BP3PArray[i] ); // BP3P
WriteBpReg ( 7, BP3MArray[i] ); // BP3M
WriteBpReg ( 1, BP4PArray[i] ); // BP4P
WriteBpReg ( 1, BP4MArray[i] ); // BP4M
WriteReg ( 0, 0x38 ); // BP_DATA_STACK
}
WriteReg ( 0, 0x37 ); // BP_REG_WEN_DIS
SendDriveCommand( 0x35, 0, 0, 0, 0 );
return 0;
}
void sd_stop(int AxisNo)
{
SendDriveCommand( 0x26, AxisNo );
}
void emg_stop(int AxisNo)
{
SendDriveCommand( 0x27, AxisNo );
}
void go_home(int AxisNo)
{
WriteParam ( LP_ADDR, 0 , AxisNo);
WriteParam ( EP_ADDR, 0 , AxisNo);
}
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -