📄 control.c
字号:
#include"control.h"
extern short ispause;
typedef unsigned short uint16;
typedef struct
{
int SV;
int V;
int Vacc;
float Tacc;
float Tdec;
}ConParam;
ConParam AxisParam;
typedef struct
{
long x;
long y;
}PauseParam;
PauseParam Pause_t;
typedef struct
{
long pre_LPos1;
long pre_LPos2;
int pre_IplCmd;
int pre_i;
}PreParam;
PreParam pre_t;
int j;
CiplParam_t CNode[65535];
Axis3Param_t axis3node[65535];
void smp860_append_3axis_continue_inp(uint16 *AxisArray,Axis3Param_t *Node )
{
smp860_set_port(C_WR5,0x4324);
smp860_set_param(C_X,C_P,Node->PosX);
smp860_set_param(C_Y,C_P,Node->PosY);
smp860_set_param(C_Z,C_P,Node->PosZ);
smp860_send_inp_cmd(C_CMD_INP_3LINE);
}
void smp860_3axis_continue_inp(uint16 *AxisArray,Axis3Param_t *Node,int StrVel,int MaxVel,float Tacc,float Tdec,int A0)
{
uint16 pValue;
smp860_set_port(C_WR5,0x4324);
smp860_get_reg(C_X,C_WR3,&pValue);
smp860_set_reg(C_X,C_WR3,pValue|0x6);
smp860_send_inp_cmd(C_CMD_INP_DEC_INVALID);
smp860_start_tr_line3(AxisArray, Node->PosX, Node->PosY, Node->PosZ, StrVel, MaxVel, Tacc, Tdec, A0 );
}
void Test_smp860_3axis_continue_inp(uint16 *AxisArray)
{
unsigned int i;
Axis3Param_t ANode[4];
const int TotalNode=4;
const int DecNode=TotalNode-1;
int iStrVel=RPS(2);
int iMaxVel=RPS(10);
float Tacc=2.5;
float Tdec=3.0;
int iA0=0;
ANode[0].PosX=0;
ANode[0].PosY=-80000;
ANode[0].PosZ=0;
ANode[1].PosX=0;
ANode[1].PosY=80000;
ANode[1].PosZ=0;
ANode[2].PosX=0;
ANode[2].PosY=-80000;
ANode[2].PosZ=0;
ANode[3].PosX=0;
ANode[3].PosY=80000;
ANode[3].PosZ=0;
smp860_set_param(C_X,C_LP,0);
smp860_set_param(C_Y,C_LP,0);
smp860_set_param(C_Z,C_LP,0);
smp860_3axis_continue_inp(AxisArray,&ANode[0],iStrVel,iMaxVel,Tacc,Tdec,iA0);
for(i=1;i<TotalNode;i++)
{
while(smp860_get_state(C_IDRV) && !smp860_get_state(C_CNEXT));
if(i==DecNode)
smp860_send_inp_cmd(C_CMD_INP_DEC_VALID);
smp860_append_3axis_continue_inp(AxisArray,&ANode[i]);
}
}
void Test_smp860_3axis(uint16 *AxisArray,int Dist1,int Dist2,int Dist3)
{
int StrVel=RPS(5);
int MaxVel=RPS(30);
float Tacc=2.5;
// float Tdec=0.2;
int Vsacc=RPS(0);
int A0=0;
smp860_start_sr_line3(AxisArray,Dist1,Dist2,Dist3,StrVel,MaxVel,Tacc,Vsacc,A0);
}
void Test_smp860_3axis_inp()
{
int TotalSegNumbers = 10;
int SV = RPS(100);
int V = RPS(150);
unsigned int i;
uint16 AxisArray[4];
int32 XPos[10];
int32 YPos[10];
int32 ZPos[10];
long value;
XPos[0] = 0; YPos[0] = 0; ZPos[0]= 300000;
XPos[1] = 0; YPos[1] = 0; ZPos[1]= -300000;
XPos[2] = 100000; YPos[2] = 0; ZPos[2]= 300000;
XPos[3] = 0; YPos[3] = 0; ZPos[3]= -300000;
XPos[4] = 0; YPos[4] =100000; ZPos[4]= 300000;
XPos[5] = 0; YPos[5] = 0; ZPos[5]= -300000;
XPos[6] = -100000; YPos[6] = 0; ZPos[6]= 300000;
XPos[7] = 0; YPos[7] = 0; ZPos[7]= -300000;
XPos[8] = 0; YPos[8] = -100000; ZPos[8]= 0;
XPos[9] = 0; YPos[9] = 0; ZPos[9]= 0;
set_port( C_WR0, 0x8000); // 软件复位IC
Delay(1);
set_port( C_WR0, 0x0f0f);
AxisArray[0] = C_X; // 选择多轴
AxisArray[1] = C_Y;
AxisArray[2] = C_Z;
AxisArray[3] = 0;
set_reg_multi_axis(AxisArray, C_WR1,0x0000); // 默认
//set_reg_multi_axis(AxisArray, C_WR2,0x0040); // 单脉冲输出
set_reg_multi_axis(AxisArray, C_WR3,0xf000); // 屏蔽紧急停止和正负硬限位
set_port( C_WR5,0x00e4); // 设定插补轴
set_param(C_X, C_R , 4000000 ); // 范围设定 4M
set_param(C_X, C_A , 20 ); // 加速度设定
set_param(C_X, C_SV , SV ); // SV 初速度设定
set_param(C_X, C_V , V ); // 驱动速度设定
set_param(C_X, C_P , XPos[0]); // 位移设定
set_param(C_X, C_DP , 9000 ); // 减速点设定
set_param(C_X, C_LP,0); // 逻辑位置计数器设置
set_param(C_X, C_EP,0); // 逻辑位置计数器设置
set_param(C_Y, C_R , 5656854 ); // 范围设定 8M*root(2)
set_param(C_Y, C_P , YPos[0] ); // 位移设定
set_param(C_Y, C_LP,0); // 逻辑位置计数器设置
set_param(C_Y, C_EP,0); // 逻辑位置计数器设置
set_param(C_Z, C_R , 6928203 );
set_param(C_Z, C_P, ZPos[0] );
set_param(C_Z, C_LP,0);
set_param(C_Z, C_EP,0);
send_inp_cmd( C_CMD_INP_DEC_INVALID ); // 禁止插补减速
send_inp_cmd( C_CMD_INP_3LINE ); // 2轴直线插补驱动
for(i = 1 ; i < TotalSegNumbers; i++)
{
while(!get_state(C_CNEXT)); // 等待发送下一段允许信号
if(i==(TotalSegNumbers-1))
{
set_param(C_X, C_DP , 9000 ); // 减速点设定
send_inp_cmd( C_CMD_INP_DEC_VALID ); // 允许插补减速
}
set_param( C_X, C_P , XPos[i] ); // 位移设定
set_param( C_Y, C_P , YPos[i] ); // 位移设定
set_param( C_Z, C_P , ZPos[i] );
send_inp_cmd( C_CMD_INP_3LINE ); // 3轴直线插补驱动
}
while(get_state(C_IDRV));
}
//T型差补运动
void Test_smp860_2axis_continue_inp_T(uint16 *AxisArray)
{
unsigned int i;
//long value;
const int TotlaNode = j;
const int DecNode = TotlaNode-1;
const int SV = RPS(AxisParam.SV);
const int V = RPS(AxisParam.V);
const float Tacc = 2.5;
const float Tdec = 3.0;
int AO = 0;
smp860_set_param(AxisArray[0],C_LP,0);
smp860_set_param(AxisArray[1],C_LP,0);
smp860_start_tr_2axis_continue_inp(AxisArray,&CNode[0],SV,V,Tacc,Tdec,AO);
for(i = 1 ; i < TotlaNode; i++)
{
/* if(ispause==1)
{
smp860_send_drv_cmd(C_X,C_CMD_DRV_DEC_Stop);
break;
}
pre_t.pre_LPos1+=CNode[i-1].Pos1;
pre_t.pre_LPos2+=CNode[i-1].Pos2;
pre_t.pre_IplCmd=CNode[i-1].IplCmd;
pre_t.pre_i=i-1;*/
while( smp860_get_state(C_IDRV) && !smp860_get_state(C_CNEXT) ); // 等待发送下一段允许信号
if(i==DecNode)
smp860_send_inp_cmd( C_CMD_INP_DEC_VALID ); // 允许插补减速
smp860_append_2axis_continue_inp( AxisArray, &CNode[i] );
}
}
//S型差补运动
void Test_smp860_2axis_continue_inp_S(uint16 *AxisArray)
{
unsigned int i;
// long value;
const int TotlaNode = j;
const int DecNode = TotlaNode-1;
const int SV = RPS(AxisParam.SV);
const int V = RPS(AxisParam.V);
const int Vsacc = RPS(AxisParam.Vacc);
const float Tacc = 2.5;
int AO = 0;
smp860_set_param(AxisArray[0],C_LP,0);
smp860_set_param(AxisArray[1],C_LP,0);
smp860_start_sr_2axis_continue_inp(AxisArray,&CNode[0],SV,V,Tacc,Vsacc,AO);
for(i = 1 ; i < TotlaNode; i++)
{
while( smp860_get_state(C_IDRV) && !smp860_get_state(C_CNEXT) ); // 等待发送下一段允许信号
if(i==DecNode)
smp860_send_inp_cmd( C_CMD_INP_DEC_VALID ); // 允许插补减速
smp860_append_2axis_continue_inp( AxisArray, &CNode[i] );
}
}
void Pause(void)
{
long value;
int index,index_t;
index_t=1;
smp860_get_param(C_X,C_rLP,&value);
Pause_t.x=value;
smp860_get_param(C_Y,C_rLP,&value);
Pause_t.y=value;
value=Pause_t.x-pre_t.pre_LPos1;
CNode[0].Pos1=CNode[pre_t.pre_i].Pos1-value;
value=Pause_t.y-pre_t.pre_LPos2;
CNode[0].Pos2=CNode[pre_t.pre_i].Pos2-value;
CNode[0].Center1=CNode[pre_t.pre_i].Center1;
CNode[0].Center2=CNode[pre_t.pre_i].Center2;
CNode[0].IplCmd=CNode[pre_t.pre_i].IplCmd;
for(index=pre_t.pre_i+1;index<=j-pre_t.pre_i;index++)
{
CNode[index_t].Pos1=CNode[index].Pos1;
CNode[index_t].Pos2=CNode[index].Pos2;
CNode[index_t].Center1=CNode[index].Center1;
CNode[index_t].Center2=CNode[index].Center2;
CNode[index_t].IplCmd=CNode[index].IplCmd;
index_t++;
}
}
//回原点
void Home(void)
{
/* if(!smp860_set_reg(C_X,C_WR4,65535))
{
SerialPrintf("Set Xcount!");
}*/
if(!smp860_set_reg(C_X,C_WR1,128))
{
// SerialPrintf("X Set home right !\n");
}
if(!smp860_home_move(C_X,1,1000,10000,10))
{
// SerialPrintf("X Home move right !\n");
}
while(smp860_get_state(C_XDRV));
//SerialPrintf("Y Start home !\n");
if(!smp860_set_reg(C_Y,C_WR1,128))
{
// SerialPrintf("Y Set home right !\n");
}
if(!smp860_home_move(C_Y,1,1000,10000,10))
{
// SerialPrintf("Y Home move right !\n");
}
while(smp860_get_state(C_YDRV));
// SerialPrintf("Finish home !\n");
/* if(!smp860_set_reg(C_X,C_WR4,0));
{
SerialPrintf("Cleaer count !\n");
for(i=0;i<1000000;i++);
}
if(!smp860_set_reg(C_X,C_WR4,0x0))
{
SerialPrintf("Set count!\n");
}*/
}
void Init(void)
{
// uint16 xValues;
// uint16 yValues;
int i;
smp860_set_port(C_WR0,0x8000);
// smp860_set_reg(C_Y,C_WR4, 0x022);
}
//串口通信
void com(void)
{
int index;
char cmd[10];
char temp[10];
int data,i;
SerialGetString(0,cmd);
strcpy(temp,"yes");
if(0!=strcmp(temp,cmd))
return;
SerialPrintf("@%d*%d$",11,0);
SerialGetString(0,cmd);
strcpy(temp,"axis2");
if(0==strcmp(temp,cmd))
index=2;
strcpy(temp,"axis3");
if(0==strcmp(temp,cmd))
index=3;
if(2==index)
{
SerialPrintf("@%d*%d$",22,220); //set CNode len
j=SerialGetIntNum(0);
SerialPrintf("@%d*%d$",22,221); //set start speed
data=SerialGetIntNum(0);
AxisParam.SV=data;
SerialPrintf("@%d*%d$",22,222); //set dirve speed
data=SerialGetIntNum(0);
AxisParam.V=data;
SerialPrintf("@%d*%d$",22,223); //set acc
data=SerialGetIntNum(0);
AxisParam.Vacc=data;
for(i=0;i<j;i++)
{
SerialPrintf("@%d*%d$",22,224);
data=SerialGetIntNum(0);
CNode[i].Pos1=data;
SerialPrintf("@%d*%d$",22,225);
data=SerialGetIntNum(0);
CNode[i].Pos2=data;
SerialPrintf("@%d*%d$",22,226);
data=SerialGetIntNum(0);
CNode[i].Center1=data;
SerialPrintf("@%d*%d$",22,227);
data=SerialGetIntNum(0);
CNode[i].Center2=data;
SerialPrintf("@%d*%d$",22,228);
data=SerialGetIntNum(0);
switch(data)
{
case 1:
CNode[i].IplCmd=C_CMD_INP_2LINE;
break;
case 2:
CNode[i].IplCmd=C_CMD_INP_CCW;
break;
case 3:
CNode[i].IplCmd=C_CMD_INP_CW;
break;
default:
break;
}
SerialPrintf("@%d*%d$",22,229); //responed sign
}
SerialPrintf("@%d*%d$",22,230); //file successful
}
else if(3==index)
{
SerialPrintf("@%d*%d$",33,330); //set CNode len
j=SerialGetIntNum(0);
SerialPrintf("@%d*%d$",33,331); //set start speed
data=SerialGetIntNum(0);
AxisParam.SV=data;
SerialPrintf("@%d*%d$",33,332); //set dirve speed
data=SerialGetIntNum(0);
AxisParam.V=data;
SerialPrintf("@%d*%d$",22,333); //set acc
data=SerialGetIntNum(0);
AxisParam.Vacc=data;
for(i=0;i<j;i++)
{
SerialPrintf("@%d*%d$",33,334);
data=SerialGetIntNum(0);
axis3node[i].PosX=data;
SerialPrintf("@%d*%d$",33,335);
data=SerialGetIntNum(0);
axis3node[i].PosY=data;
SerialPrintf("@%d*%d$",33,336);
data=SerialGetIntNum(0);
axis3node[i].PosZ=data;
SerialPrintf("@%d*%d$",33,337); //responed sign
}
SerialPrintf("@%d*%d$",33,338); //file successful
}
else
{
SerialPrintf("error");
}
}
void SetInterrupt(void)
{
rINTMSK=~(BIT_GLOBAL|BIT_EINT4567|BIT_TIMER0);
}
void CloseInterrupt(void)
{
rINTMSK=~(BIT_GLOBAL|BIT_EINT4567);
}⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -