public short SetMovePos(short cardId, short axisId, int pos, double vel, double acc, double dec)
{
//set trap move
short rtn = mc.API_TrapMode(axisId, cardId);
if (rtn != 0)
{
return(rtn);
}
//set trap param
mc.TTrapPrm trapPrm = new mc.TTrapPrm();
trapPrm.acc = acc;
trapPrm.dec = dec;
trapPrm.smoothTime = 0;
rtn = mc.API_SetTrapPrm(axisId, ref trapPrm, cardId);
if (rtn != 0)
{
return(rtn);
}
//set pos
rtn = mc.API_SetTrapPos(axisId, pos, cardId);
if (rtn != 0)
{
return(rtn);
}
//set vel
rtn = mc.API_SetVel(axisId, vel, cardId);
if (rtn != 0)
{
return(rtn);
}
return(0);
}
public static void GoSafetyPosition(short axi, short card)
{
AxiCondition.AxiSituation axiSituation = new AxiCondition.AxiSituation();
axiSituation = AxiCondition.readCondition(axi, card);
if (axiSituation.negativeLimit == true)
{
mc.TTrapPrm p_trap = new mc.TTrapPrm();
p_trap.acc = 0.25;
p_trap.dec = 0.25;
p_trap.smoothTime = 25;
TrapParameter setTrapParameter = new TrapParameter();
setTrapParameter.position = 5000;
setTrapParameter.sleep = 20;
Trap(true, p_trap, setTrapParameter, axi);
}
else if (axiSituation.positveLimit == true)
{
mc.TTrapPrm p_trap = new mc.TTrapPrm();
p_trap.acc = 0.25;
p_trap.dec = 0.25;
p_trap.smoothTime = 25;
TrapParameter setTrapParameter = new TrapParameter();
setTrapParameter.position = -5000;
setTrapParameter.sleep = 20;
Trap(true, p_trap, setTrapParameter, axi);
}
}
public static void GoHome(short axi, short card)
{
mc.TJogPrm jopPra = new mc.TJogPrm();
jopPra.acc = 0.25;
jopPra.dec = 0.25;
double value = 20;
AxiCondition.AxiSituation axiSituation = new AxiCondition.AxiSituation();
JogMotin(0, 1, jopPra, -value);
do
{
axiSituation = AxiCondition.JustReadCondition(axi, card);
}while (axiSituation.negativeLimit == false);
mc.TTrapPrm p_trap = new mc.TTrapPrm();
p_trap.acc = 0.25;
p_trap.dec = 0.25;
p_trap.smoothTime = 25;
p_trap.velStart = 1;
TrapParameter setTrapParameter = new TrapParameter();
setTrapParameter.position = 10000;
setTrapParameter.sleep = 20;
Trap(true, p_trap, setTrapParameter, axi);
}
public int AxisRelMove(int index, int axis, int step, int vel)
{
var ret = mc.GT_ClrSts((short)Id, (short)axis, 1);
if (ret != 0)
{
return(-1);
}
var encPos = 0d;
GetCmdPos(index, (short)axis, ref encPos);
//0.3s for acc or dec
//to do test
var trapPrm = new mc.TTrapPrm
{
acc = vel / 0.3 / 1000000,
dec = vel / 0.3 / 1000000,
smoothTime = 10
};
ret = mc.GT_PrfTrap((short)Id, (short)axis);
if (ret != 0)
{
return(-1);
}
ret = mc.GT_SetTrapPrm((short)Id, (short)axis, ref trapPrm);
if (ret != 0)
{
return(-1);
}
ret = mc.GT_SetVel((short)Id, (short)axis, (double)vel / 1000);
if (ret != 0)
{
return(-1);
}
ret = mc.GT_SetPos((short)Id, (short)axis, Convert.ToInt32(step + encPos));
if (ret != 0)
{
return(-1);
}
ret = mc.GT_Update((short)Id, 0x1 << (axis - 1));
if (ret != 0)
{
return(-1);
}
return(0);
}
public int RelMove(int id, int axis, int step, int vel)
{
ClearAxisStatus(id, axis);
int encPos;
var ret = GetCommandPos((short)id, (short)axis, out encPos);
if (ret != 0)
{
return(-1);
}
ret = mc.GT_PrfTrap((short)id, (short)axis);
if (ret != 0)
{
return(-1);
}
var trapPrm = new mc.TTrapPrm
{
acc = Acc[axis - 1] / 1000000, //mm/s^2
dec = Dec[axis - 1] / 1000000, //mm/s^2
smoothTime = 10
};
ret = mc.GT_SetTrapPrm((short)id, (short)axis, ref trapPrm);
if (ret != 0)
{
return(-1);
}
ret = mc.GT_SetVel((short)id, (short)axis, (double)vel / 1000);
if (ret != 0)
{
return(-1);
}
ret = mc.GT_SetPos((short)id, (short)axis, Convert.ToInt32(step + encPos));
if (ret != 0)
{
return(-1);
}
ret = mc.GT_Update((short)id, 1 << (axis - 1));
if (ret != 0)
{
return(-1);
}
return(0);
}
/// <summary>
/// 设置Trap运动参数
/// </summary>
/// <param name="nCardNum"></param>
/// <param name="nAxis">轴号</param>
/// <param name="acc">加速度 单位“脉冲/毫秒2”</param>
/// <param name="dec">减速度 单位“脉冲/毫秒2”</param>
/// <param name="smoothTime">平滑系数，取值范围[0,1]</param>
/// <returns></returns>
public override short SetTrapPrm(short nCardNum, short nAxis, double acc, double dec, short smoothTime = 5)
{
short nRtn = 0;
//设置Jog运动参数
var mytprfvel = new mc.TTrapPrm();
mytprfvel.acc = acc; //加速度，单位“脉冲/毫秒2”
mytprfvel.dec = dec; //减速度，单位“脉冲/毫秒2”
mytprfvel.smoothTime = smoothTime; //平滑时间，取值范围[0,1]
nRtn |= mc.GT_SetTrapPrm(nCardNum, nAxis, ref mytprfvel);
return(nRtn);
}
public int AbsMove(int id, int axis, int pos, int vel)
{
ClearAxisStatus(id, axis);
var ret = mc.GT_PrfTrap((short)id, (short)axis);
if (ret != 0)
{
return(-1);
}
var trapPrm = new mc.TTrapPrm
{
acc = Acc[axis - 1] / 1000000, //mm/s^2
dec = Dec[axis - 1] / 1000000, //mm/s^2
smoothTime = 10
};
ret = mc.GT_SetTrapPrm((short)id, (short)axis, ref trapPrm);
if (ret != 0)
{
return(-1);
}
ret = mc.GT_SetVel((short)id, (short)axis, (double)vel / 1000);
if (ret != 0)
{
return(-1);
}
ret = mc.GT_SetPos((short)id, (short)axis, pos);
if (ret != 0)
{
return(-1);
}
ret = mc.GT_Update((short)id, 1 << (axis - 1));
if (ret != 0)
{
return(-1);
}
return(0);
}
public short SetMovePos(short cardId, short axisId, int pos, double vel, double acc, double dec)
{
//set prfPos zero
//short rtn = 0;
//rtn = mc.GT_SetPrfPos(cardId, axisId, 0);
//if (rtn != 0) return rtn;
//set trap move
short rtn = mc.GT_PrfTrap(cardId, axisId);
if (rtn != 0)
{
return(rtn);
}
//set trap param
mc.TTrapPrm trapPrm = new mc.TTrapPrm();
trapPrm.acc = acc;
trapPrm.dec = dec;
trapPrm.smoothTime = 0;
rtn = mc.GT_SetTrapPrm(cardId, axisId, ref trapPrm);
if (rtn != 0)
{
return(rtn);
}
//set pos
rtn = mc.GT_SetPos(cardId, axisId, pos);
if (rtn != 0)
{
return(rtn);
}
//set vel
rtn = mc.GT_SetVel(cardId, axisId, vel);
if (rtn != 0)
{
return(rtn);
}
return(0);
}
//trap运动
public static void Trap(bool positive, mc.TTrapPrm p_trap, TrapParameter setTrapParameter, short axi)
{
int AxiStatus;
uint pClock;
short sRtn = mc.GT_GetSts(0, axi, out AxiStatus, 1, out pClock);
if ((AxiStatus & 0x2) == 0x2 && (AxiStatus & 0x20) == 0x20)
{
Config.commandhandler("伺服报警或处于正限位", sRtn);
}
else
{
mc.TTrapPrm trap = new mc.TTrapPrm();
//清除各轴的报警和限位
sRtn = mc.GT_ClrSts(0, axi, 1);
if (sRtn != 0)
{
Config.commandhandler("清除出错", sRtn);
}
// 伺服使能
sRtn = mc.GT_AxisOn(0, axi);
if (sRtn != 0)
{
Config.commandhandler("使能出错", sRtn);
}
// 位置清零
sRtn = mc.GT_ZeroPos(0, axi, 1);
if (sRtn != 0)
{
Config.commandhandler("位置清零出错", sRtn);
}
// AXIS轴规划位置清零
sRtn = mc.GT_SetPrfPos(0, axi, 0);
if (sRtn != 0)
{
Config.commandhandler("规划出错", sRtn);
}
// 将AXIS轴设为点位模式
sRtn = mc.GT_PrfTrap(0, axi);
if (sRtn != 0)
{
Config.commandhandler("设置模式出错", sRtn);
}
// 读取点位运动参数
sRtn = mc.GT_GetTrapPrm(0, axi, out trap);
if (sRtn != 0)
{
Config.commandhandler("读取参数出错", sRtn);
}
trap.acc = p_trap.acc;
trap.dec = p_trap.dec;
trap.smoothTime = p_trap.smoothTime;
// 设置点位运动参数
sRtn = mc.GT_SetTrapPrm(0, axi, ref trap);
if (sRtn != 0)
{
Config.commandhandler("设置参数出错", sRtn);
}
// 设置AXIS轴的目标位置
int posation = Convert.ToInt32(setTrapParameter.position);
sRtn = mc.GT_SetPos(0, axi, posation);
if (sRtn != 0)
{
Config.commandhandler("设置位置出错", sRtn);
}
// 设置AXIS轴的目标速度
double 速度 = Convert.ToDouble(setTrapParameter.sleep);
sRtn = mc.GT_SetVel(0, axi, 速度);
if (sRtn != 0)
{
Config.commandhandler("设置速度出错", sRtn);
}
// 启动AXIS轴的运动
sRtn = mc.GT_Update(0, 2 ^ axi);
if (sRtn != 0)
{
Config.commandhandler("启动运动出错", sRtn);
}
}
}
private void button1_Click(object sender, EventArgs e)
{
mc.TTrapPrm trap = new mc.TTrapPrm();
trap.acc = 0.25;
trap.dec = 0.125;
trap.smoothTime = 25;
MotionPunction.TrapParameter tp = new MotionPunction.TrapParameter();
tp.position = Convert.ToInt32(numericUpDown2.Value);
tp.sleep = Convert.ToInt32(numericUpDown1.Value);
MotionPunction.Trap(true, trap, tp, 1);
//int AxiStatus;
//uint pClock;
//short sRtn1 = mc.GT_GetSts(0,1,out AxiStatus,1,out pClock);
//if ((AxiStatus&0x2)==0x2&&(AxiStatus&0x20)==0x20)
//{
// MessageBox.Show("伺服报警或处于正限位");
//}
//else
//{
// short sRtn;
// mc.TTrapPrm trap;
// //清除各轴的报警和限位
// sRtn = mc.GT_ClrSts(0,1, 8);
// if (sRtn != 0)
// {
// MessageBox.Show("清除出错");
// }
// // 伺服使能
// sRtn = mc.GT_AxisOn(0,1);
// if (sRtn != 0)
// {
// MessageBox.Show("使能出错");
// }
// // 位置清零
// sRtn = mc.GT_ZeroPos(0,1,1);
// if (sRtn != 0)
// {
// MessageBox.Show("位置清出错");
// }
// // AXIS轴规划位置清零
// sRtn = mc.GT_SetPrfPos(0,1, 0);
// if (sRtn != 0)
// {
// MessageBox.Show("规划出错");
// }
// // 将AXIS轴设为点位模式
// sRtn = mc.GT_PrfTrap(0,1);
// if (sRtn != 0)
// {
// MessageBox.Show("模式出错");
// }
// // 读取点位运动参数
// sRtn = mc.GT_GetTrapPrm(0,1, out trap);
// if (sRtn != 0)
// {
// MessageBox.Show("读取参数出错");
// }
// trap.acc = 0.25;
// trap.dec = 0.125;
// trap.smoothTime = 25;
// // 设置点位运动参数
// sRtn = mc.GT_SetTrapPrm(0,1, ref trap);
// if (sRtn != 0)
// {
// MessageBox.Show("设置参数出错");
// }
// // 设置AXIS轴的目标位置
// int posation = Convert.ToInt32(numericUpDown2.Value);
// sRtn =mc.GT_SetPos(0,1, posation);
// if (sRtn != 0)
// {
// MessageBox.Show("位置出错");
// }
// // 设置AXIS轴的目标速度
// double 速度 = Convert.ToDouble(numericUpDown1.Value);
// sRtn = mc.GT_SetVel(0,1, 速度);
// if (sRtn != 0)
// {
// MessageBox.Show("速度出错");
// }
// // 启动AXIS轴的运动
// sRtn = mc.GT_Update(0,1);
// if (sRtn != 0)
// {
// MessageBox.Show("运动出错");
// }
//}
}
