寸动功能
作者：互联网
//x+
private void button5_Click(object sender, EventArgs e)
{
if (wether_link == 0)
{
MessageBox.Show("请先打开设备");
return;
}
if (SF == 0)
{
MessageBox.Show("请先打开伺服");
return;
}
if (machine_origin == 0)
{
MessageBox.Show("请先确定原点");
return;
}
double[] Delta = new double[3];
double[] PKM_Delta = new double[3];
double[] PKM_Pstar_motor = new double[3];
double[] PKM_Pstar_motor1 = new double[3];
double[] theta = new double[3];
for (int i = 0; i < axis_all.Length; i = i + 1)
{
LTSMC.smc_get_encoder_unit(CardNum, axis_all[i], ref PKM_Pstar_motor1[i]);//当前编码器位置
PKM_Pstar_motor[i] = PKM_Pstar_motor1[i] / (fp / 360);//主动臂的转角（驱动器设置控制电机正反转的方向）顺时针方向为正
}
Forward.foward_calculate(PKM_Pstar_motor[0], PKM_Pstar_motor[1], PKM_Pstar_motor[2]);
PKM_Delta[0] = Forward.instantaneous_Gx;
PKM_Delta[1] = Forward.instantaneous_Gy;
PKM_Delta[2] = Forward.instantaneous_Gz; //当前位置下动平台相对于静坐标系的坐标
//让机器知道此时位置，沿着此时位置向X轴方向运动
Delta[0] = PKM_Delta[0] + decimal.ToDouble(movement_distance.Value);
Delta[1] = PKM_Delta[1];
Delta[2] = PKM_Delta[2];
Inverse.inverse_position(Delta[0], Delta[1], Delta[2]);//定点移动的距离坐标，通过逆解计算出编码器的位置，再插补到该点上。
theta[0] = Inverse.theta1;
theta[1] = Inverse.theta2;
theta[2] = Inverse.theta3;
for (int i = 0; i < axis_all.Length; i = i + 1)
{
LTSMC.smc_set_alm_mode(CardNum, axis_all[i], 0, 0, 0);//设置报警使能，关闭报警
LTSMC.smc_write_sevon_pin(CardNum, axis_all[i], 0);//打开伺服使能
}
double speed = gear * decimal.ToDouble(run_speed.Value) * (fp / (2 * Math.PI)); //单位：unit/s
LTSMC.smc_set_vector_profile_unit(CardNum, 0, start, speed, acc, dec, stop);//设置插补运动速度参数
LTSMC.smc_set_vector_s_profile(CardNum, 0, 0, acc / 2);//设置s曲线平滑时间
LTSMC.smc_line_unit(CardNum, 0, 3, axis_all, new double[] { theta[0], theta[1], theta[2] }, 1);//运动模式，0： 相对坐标模式， 1： 绝对坐标模式
}
//x-
private void button6_Click(object sender, EventArgs e)
{
if (wether_link == 0)
{
MessageBox.Show("请先打开设备");
return;
}
if (SF == 0)
{
MessageBox.Show("请先打开伺服");
return;
}
if (machine_origin == 0)
{
MessageBox.Show("请先确定原点");
return;
}
double[] Delta = new double[3];
double[] PKM_Delta = new double[3];
double[] PKM_Pstar_motor = new double[3];
double[] PKM_Pstar_motor1 = new double[3];
double[] theta = new double[3];
for (int i = 0; i < axis_all.Length; i = i + 1)
{
LTSMC.smc_get_encoder_unit(CardNum, axis_all[i], ref PKM_Pstar_motor1[i]);//当前编码器位置
PKM_Pstar_motor[i] = PKM_Pstar_motor1[i] / (fp / 360);//主动臂的转角（驱动器设置控制电机正反转的方向）顺时针方向为正
}
Forward.foward_calculate(PKM_Pstar_motor[0], PKM_Pstar_motor[1], PKM_Pstar_motor[2]);
PKM_Delta[0] = Forward.instantaneous_Gx;
PKM_Delta[1] = Forward.instantaneous_Gy;
PKM_Delta[2] = Forward.instantaneous_Gz; //当前位置下动平台相对于静坐标系的坐标
//让机器知道此时位置，沿着此时位置向X轴方向运动
Delta[0] = PKM_Delta[0] - decimal.ToDouble(movement_distance.Value);
Delta[1] = PKM_Delta[1];
Delta[2] = PKM_Delta[2];
Inverse.inverse_position(Delta[0], Delta[1], Delta[2]);//定点移动的距离坐标，通过逆解计算出编码器的位置，再插补到该点上。
theta[0] = Inverse.theta1;
theta[1] = Inverse.theta2;
theta[2] = Inverse.theta3;
double speed = gear * decimal.ToDouble(run_speed.Value) * (fp / (2 * Math.PI)); //单位：unit/s
LTSMC.smc_set_vector_profile_unit(CardNum, 0, start, speed, acc, dec, stop);//设置插补运动速度参数
LTSMC.smc_set_vector_s_profile(CardNum, 0, 0, acc / 2);//设置s曲线平滑时间
LTSMC.smc_line_unit(CardNum, 0, 3, axis_all, new double[] { Inverse.theta1, Inverse.theta2, Inverse.theta3 }, 1);//运动模式，0： 相对坐标模式， 1： 绝对坐标模式
}
//y+
private void button8_Click(object sender, EventArgs e)
{
if (wether_link == 0)
{
MessageBox.Show("请先打开设备");
return;
}
if (SF == 0)
{
MessageBox.Show("请先打开伺服");
return;
}
if (machine_origin == 0)
{
MessageBox.Show("请先确定原点");
return;
}
double[] Delta = new double[3];
double[] PKM_Delta = new double[3];
double[] PKM_Pstar_motor = new double[3];
double[] PKM_Pstar_motor1 = new double[3];
double[] theta = new double[3];
for (int i = 0; i < axis_all.Length; i = i + 1)
{
LTSMC.smc_get_encoder_unit(CardNum, axis_all[i], ref PKM_Pstar_motor1[i]);//当前编码器位置
PKM_Pstar_motor[i] = PKM_Pstar_motor1[i] / (fp / 360);//主动臂的转角（驱动器设置控制电机正反转的方向）顺时针方向为正
}
Forward.foward_calculate(PKM_Pstar_motor[0], PKM_Pstar_motor[1], PKM_Pstar_motor[2]);
PKM_Delta[0] = Forward.instantaneous_Gx;
PKM_Delta[1] = Forward.instantaneous_Gy;
PKM_Delta[2] = Forward.instantaneous_Gz; //当前位置下动平台相对于静坐标系的坐标
//让机器知道此时位置，沿着此时位置向X轴方向运动
Delta[0] = PKM_Delta[0];
Delta[1] = PKM_Delta[1] + decimal.ToDouble(movement_distance.Value);
Delta[2] = PKM_Delta[2];
Inverse.inverse_position(Delta[0], Delta[1], Delta[2]);//定点移动的距离坐标，通过逆解计算出编码器的位置，再插补到该点上。
theta[0] = Inverse.theta1;
theta[1] = Inverse.theta2;
theta[2] = Inverse.theta3;
for (int i = 0; i < axis_all.Length; i = i + 1)
{
LTSMC.smc_set_alm_mode(CardNum, axis_all[i], 0, 0, 0);//设置报警使能，关闭报警
LTSMC.smc_write_sevon_pin(CardNum, axis_all[i], 0);//打开伺服使能
}
double speed = gear * decimal.ToDouble(run_speed.Value) * (fp / (2 * Math.PI)); //单位：unit/s
LTSMC.smc_set_vector_profile_unit(CardNum, 0, start, speed, acc, dec, stop);//设置插补运动速度参数
LTSMC.smc_set_vector_s_profile(CardNum, 0, 0, acc / 2);//设置s曲线平滑时间
LTSMC.smc_line_unit(CardNum, 0, 3, axis_all, new double[] { theta[0], theta[1], theta[2] }, 1);//运动模式，0： 相对坐标模式， 1： 绝对坐标模式
}
//y-
private void button7_Click(object sender, EventArgs e)
{
if (wether_link == 0)
{
MessageBox.Show("请先打开设备");
return;
}
if (SF == 0)
{
MessageBox.Show("请先打开伺服");
return;
}
if (machine_origin == 0)
{
MessageBox.Show("请先确定原点");
return;
}
double[] Delta = new double[3];
double[] PKM_Delta = new double[3];
double[] PKM_Pstar_motor = new double[3];
double[] PKM_Pstar_motor1 = new double[3];
double[] theta = new double[3];
for (int i = 0; i < axis_all.Length; i = i + 1)
{
LTSMC.smc_get_encoder_unit(CardNum, axis_all[i], ref PKM_Pstar_motor1[i]);//当前编码器位置
PKM_Pstar_motor[i] = PKM_Pstar_motor1[i] / (fp / 360);//主动臂的转角（驱动器设置控制电机正反转的方向）顺时针方向为正
}
Forward.foward_calculate(PKM_Pstar_motor[0], PKM_Pstar_motor[1], PKM_Pstar_motor[2]);
PKM_Delta[0] = Forward.instantaneous_Gx;
PKM_Delta[1] = Forward.instantaneous_Gy;
PKM_Delta[2] = Forward.instantaneous_Gz; //当前位置下动平台相对于静坐标系的坐标
//让机器知道此时位置，沿着此时位置向X轴方向运动
Delta[0] = PKM_Delta[0];
Delta[1] = PKM_Delta[1] - decimal.ToDouble(movement_distance.Value);
Delta[2] = PKM_Delta[2];
Inverse.inverse_position(Delta[0], Delta[1], Delta[2]);//定点移动的距离坐标，通过逆解计算出编码器的位置，再插补到该点上。
theta[0] = Inverse.theta1;
theta[1] = Inverse.theta2;
theta[2] = Inverse.theta3;
for (int i = 0; i < axis_all.Length; i = i + 1)
{
LTSMC.smc_set_alm_mode(CardNum, axis_all[i], 0, 0, 0);//设置报警使能，关闭报警
LTSMC.smc_write_sevon_pin(CardNum, axis_all[i], 0);//打开伺服使能
}
double speed = gear * decimal.ToDouble(run_speed.Value) * (fp / (2 * Math.PI)); //单位：unit/s
LTSMC.smc_set_vector_profile_unit(CardNum, 0, start, speed, acc, dec, stop);//设置插补运动速度参数
LTSMC.smc_set_vector_s_profile(CardNum, 0, 0, acc / 2);//设置s曲线平滑时间
LTSMC.smc_line_unit(CardNum, 0, 3, axis_all, new double[] { theta[0], theta[1], theta[2] }, 1);//运动模式，0： 相对坐标模式， 1： 绝对坐标模式
}
//z+
private void button10_Click(object sender, EventArgs e)
{
if (wether_link == 0)
{
MessageBox.Show("请先打开设备");
return;
}
if (SF == 0)
{
MessageBox.Show("请先打开伺服");
return;
}
if (machine_origin == 0)
{
MessageBox.Show("请先确定原点");
return;
}
double[] Delta = new double[3];
double[] PKM_Delta = new double[3];
double[] PKM_Pstar_motor = new double[3];
double[] PKM_Pstar_motor1 = new double[3];
double[] theta = new double[3];
for (int i = 0; i < axis_all.Length; i = i + 1)
{
LTSMC.smc_get_encoder_unit(CardNum, axis_all[i], ref PKM_Pstar_motor1[i]);//当前编码器位置
PKM_Pstar_motor[i] = PKM_Pstar_motor1[i] / (fp / 360);//主动臂的转角（驱动器设置控制电机正反转的方向）顺时针方向为正
}
Forward.foward_calculate(PKM_Pstar_motor[0], PKM_Pstar_motor[1], PKM_Pstar_motor[2]);
PKM_Delta[0] = Forward.instantaneous_Gx;
PKM_Delta[1] = Forward.instantaneous_Gy;
PKM_Delta[2] = Forward.instantaneous_Gz; //当前位置下动平台相对于静坐标系的坐标
//让机器知道此时位置，沿着此时位置向X轴方向运动
Delta[0] = PKM_Delta[0];
Delta[1] = PKM_Delta[1];
Delta[2] = PKM_Delta[2] - decimal.ToDouble(movement_distance.Value);
Inverse.inverse_position(Delta[0], Delta[1], Delta[2]);//定点移动的距离坐标，通过逆解计算出编码器的位置，再插补到该点上。
theta[0] = Inverse.theta1;
theta[1] = Inverse.theta2;
theta[2] = Inverse.theta3;
for (int i = 0; i < axis_all.Length; i = i + 1)
{
LTSMC.smc_set_alm_mode(CardNum, axis_all[i], 0, 0, 0);//设置报警使能，关闭报警
LTSMC.smc_write_sevon_pin(CardNum, axis_all[i], 0);//打开伺服使能
}
double speed = gear * decimal.ToDouble(run_speed.Value) * (fp / (2 * Math.PI)); //单位：unit/s
LTSMC.smc_set_vector_profile_unit(CardNum, 0, start, speed, acc, dec, stop);//设置插补运动速度参数
LTSMC.smc_set_vector_s_profile(CardNum, 0, 0, acc / 2);//设置s曲线平滑时间
LTSMC.smc_line_unit(CardNum, 0, 3, axis_all, new double[] { theta[0], theta[1], theta[2] }, 1);//运动模式，0： 相对坐标模式， 1： 绝对坐标模式
}
//z-
private void button9_Click(object sender, EventArgs e)
{
if (wether_link == 0)
{
MessageBox.Show("请先打开设备");
return;
}
if (SF == 0)
{
MessageBox.Show("请先打开伺服");
return;
}
if (machine_origin == 0)
{
MessageBox.Show("请先确定原点");
return;
}
double[] Delta = new double[3];
double[] PKM_Delta = new double[3];
double[] PKM_Pstar_motor = new double[3];
double[] PKM_Pstar_motor1 = new double[3];
double[] theta = new double[3];
for (int i = 0; i < axis_all.Length; i = i + 1)
{
LTSMC.smc_get_encoder_unit(CardNum, axis_all[i], ref PKM_Pstar_motor1[i]);//当前编码器位置
PKM_Pstar_motor[i] = PKM_Pstar_motor1[i] / (fp / 360);//主动臂的转角（驱动器设置控制电机正反转的方向）顺时针方向为正
}
Forward.foward_calculate(PKM_Pstar_motor[0], PKM_Pstar_motor[1], PKM_Pstar_motor[2]);
PKM_Delta[0] = Forward.instantaneous_Gx;
PKM_Delta[1] = Forward.instantaneous_Gy;
PKM_Delta[2] = Forward.instantaneous_Gz; //当前位置下动平台相对于静坐标系的坐标
//让机器知道此时位置，沿着此时位置向X轴方向运动
Delta[0] = PKM_Delta[0];
Delta[1] = PKM_Delta[1];
Delta[2] = PKM_Delta[2] + decimal.ToDouble(movement_distance.Value);
Inverse.inverse_position(Delta[0], Delta[1], Delta[2]);//定点移动的距离坐标，通过逆解计算出编码器的位置，再插补到该点上。
theta[0] = Inverse.theta1;
theta[1] = Inverse.theta2;
theta[2] = Inverse.theta3;
for (int i = 0; i < axis_all.Length; i = i + 1)
{
LTSMC.smc_set_alm_mode(CardNum, axis_all[i], 0, 0, 0);//设置报警使能，关闭报警
LTSMC.smc_write_sevon_pin(CardNum, axis_all[i], 0);//打开伺服使能
}
double speed = gear * decimal.ToDouble(run_speed.Value) * (fp / (2 * Math.PI)); //单位：unit/s
LTSMC.smc_set_vector_profile_unit(CardNum, 0, start, speed, acc, dec, stop);//设置插补运动速度参数
LTSMC.smc_set_vector_s_profile(CardNum, 0, 0, acc / 2);//设置s曲线平滑时间
LTSMC.smc_line_unit(CardNum, 0, 3, axis_all, new double[] { theta[0], theta[1], theta[2] }, 1);//运动模式，0： 相对坐标模式， 1： 绝对坐标模式
}
//主动臂A+
private void A_p_MouseDown(object sender, MouseEventArgs e)
{
if (wether_link == 0)
{
MessageBox.Show("请先打开设备");
return;
}
if (SF == 0)
{
MessageBox.Show("请先打开设备");
return;
}
ushort axis = 1;
double speed = gear * decimal.ToDouble(run_speed.Value) * (fp / (2 * Math.PI)); //单位：unit/s
LTSMC.smc_set_equiv(CardNum, axis, 1);//设置脉冲当量
LTSMC.smc_set_alm_mode(CardNum, axis, 0, 0, 0);//设置报警使能，关闭报警
LTSMC.smc_set_s_profile(CardNum, axis, 0, 0.01);//设置S段时间（0-0.05s)
LTSMC.smc_set_profile_unit(CardNum, axis, start, speed, acc, dec, stop);//设置起始速度、运行速度、加速时间、减速时间、停止速度
LTSMC.smc_set_dec_stop_time(CardNum, axis, dec);//设置减速停止时间
LTSMC.smc_vmove(CardNum, axis, 1);//连续运动，1：正向，0：反向
}
private void A_p_MouseUp(object sender, MouseEventArgs e)
{
ushort axis = 1;
LTSMC.smc_stop(CardNum, axis, 0);//减速停止运动
}
//主动臂A-
private void A_m_MouseDown(object sender, MouseEventArgs e)
{
if (wether_link == 0)
{
MessageBox.Show("请先打开设备");
return;
}
if (SF == 0)
{
MessageBox.Show("请先打开设备");
return;
}
ushort axis = 1;
double speed = gear * decimal.ToDouble(run_speed.Value) * (fp / (2 * Math.PI)); //单位：unit/s
LTSMC.smc_set_equiv(CardNum, axis, 1);//设置脉冲当量
LTSMC.smc_set_alm_mode(CardNum, axis, 0, 0, 0);//设置报警使能，关闭报警
LTSMC.smc_set_s_profile(CardNum, axis, 0, 0.01);//设置S段时间（0-0.05s)
LTSMC.smc_set_profile_unit(CardNum, axis, start, speed, acc, dec, stop);//设置起始速度、运行速度、加速时间、减速时间、停止速度
LTSMC.smc_set_dec_stop_time(CardNum, axis, dec);//设置减速停止时间
LTSMC.smc_vmove(CardNum, axis, 0);//连续运动，1：正向，0：反向
}
private void A_m_MouseUp(object sender, MouseEventArgs e)
{
ushort axis = 1;
LTSMC.smc_stop(CardNum, axis, 0);//减速停止运动
}
//主动臂B+
private void B_p_MouseDown(object sender, MouseEventArgs e)
{
{
if (wether_link == 0)
{
MessageBox.Show("请先打开设备");
return;
}
if (SF == 0)
{
MessageBox.Show("请先打开设备");
return;
}
ushort axis = 2;
double speed = gear * decimal.ToDouble(run_speed.Value) * (fp / (2 * Math.PI)); //单位：unit/s
LTSMC.smc_set_equiv(CardNum, axis, 1);//设置脉冲当量
LTSMC.smc_set_alm_mode(CardNum, axis, 0, 0, 0);//设置报警使能，关闭报警
LTSMC.smc_set_s_profile(CardNum, axis, 0, 0.01);//设置S段时间（0-0.05s)
LTSMC.smc_set_profile_unit(CardNum, axis, start, speed, acc, dec, stop);//设置起始速度、运行速度、加速时间、减速时间、停止速度
LTSMC.smc_set_dec_stop_time(CardNum, axis, dec);//设置减速停止时间
LTSMC.smc_vmove(CardNum, axis, 1);//连续运动，1：正向，0：反向
}
}
private void B_p_MouseUp(object sender, MouseEventArgs e)
{
ushort axis = 2;
LTSMC.smc_stop(CardNum, axis, 0);//减速停止运动
}
//主动臂B-
private void B_m_MouseDown(object sender, MouseEventArgs e)
{
if (wether_link == 0)
{
MessageBox.Show("请先打开设备");
return;
}
if (SF == 0)
{
MessageBox.Show("请先打开设备");
return;
}
ushort axis = 2;
double speed = gear * decimal.ToDouble(run_speed.Value) * (fp / (2 * Math.PI)); //单位：unit/s
LTSMC.smc_set_equiv(CardNum, axis, 1);//设置脉冲当量
LTSMC.smc_set_alm_mode(CardNum, axis, 0, 0, 0);//设置报警使能，关闭报警
LTSMC.smc_set_s_profile(CardNum, axis, 0, 0.01);//设置S段时间（0-0.05s)
LTSMC.smc_set_profile_unit(CardNum, axis, start, speed, acc, dec, stop);//设置起始速度、运行速度、加速时间、减速时间、停止速度
LTSMC.smc_set_dec_stop_time(CardNum, axis, dec);//设置减速停止时间
LTSMC.smc_vmove(CardNum, axis, 0);//连续运动，1：正向，0：反向
}
private void B_m_MouseUp(object sender, MouseEventArgs e)
{
ushort axis = 2;
LTSMC.smc_stop(CardNum, axis, 0);//减速停止运动
}
//主动臂C+
private void C_p_MouseDown(object sender, MouseEventArgs e)
{
if (wether_link == 0)
{
MessageBox.Show("请先打开设备");
return;
}
if (SF == 0)
{
MessageBox.Show("请先打开设备");
return;
}
ushort axis = 3;
double speed = gear * decimal.ToDouble(run_speed.Value) * (fp / (2 * Math.PI)); //单位：unit/s
LTSMC.smc_set_equiv(CardNum, axis, 1);//设置脉冲当量
LTSMC.smc_set_alm_mode(CardNum, axis, 0, 0, 0);//设置报警使能，关闭报警
LTSMC.smc_set_s_profile(CardNum, axis, 0, 0.01);//设置S段时间（0-0.05s)
LTSMC.smc_set_profile_unit(CardNum, axis, start, speed, acc, dec, stop);//设置起始速度、运行速度、停止速度、加速时间、减速时间
LTSMC.smc_set_dec_stop_time(CardNum, axis, dec);//设置减速停止时间
LTSMC.smc_vmove(CardNum, axis, 1);//连续运动，1：正向，0：反向
}
private void C_p_MouseUp(object sender, MouseEventArgs e)
{
ushort axis = 3;
LTSMC.smc_stop(CardNum, axis, 0);//减速停止运动
}
//主动臂C-
private void C_m_MouseDown(object sender, MouseEventArgs e)
{
if (wether_link == 0)
{
MessageBox.Show("请先打开设备");
return;
}
if (SF == 0)
{
MessageBox.Show("请先打开设备");
return;
}
ushort axis = 3;
double speed = gear * decimal.ToDouble(run_speed.Value) * (fp / (2 * Math.PI)); //单位：unit/s
LTSMC.smc_set_equiv(CardNum, axis, 1);//设置脉冲当量
LTSMC.smc_set_alm_mode(CardNum, axis, 0, 0, 0);//设置报警使能，关闭报警
LTSMC.smc_set_s_profile(CardNum, axis, 0, 0.01);//设置S段时间（0-0.05s)
LTSMC.smc_set_profile_unit(CardNum, axis, start, speed, acc, dec, stop);//设置起始速度、运行速度、停止速度、加速时间、减速时间
LTSMC.smc_set_dec_stop_time(CardNum, axis, dec);//设置减速停止时间
LTSMC.smc_vmove(CardNum, axis, 0);//连续运动，1：正向，0：反向
}
private void C_m_MouseUp(object sender, MouseEventArgs e)
{
ushort axis = 3;
LTSMC.smc_stop(CardNum, axis, 0);//减速停止运动
}
////急停
private void stop_now_Click(object sender, EventArgs e)
{
LTSMC.smc_emg_stop(CardNum);
}
int machine_origin = 0;
private void confirm_zero_Click(object sender, EventArgs e)
{
if (wether_link == 0)
{
MessageBox.Show("请先打开设备");
return;
}
else
{
ushort[] axis = { 1, 2, 3 };
for (int i = 0; i < axis.Length; i = i + 1)
{
LTSMC.smc_set_encoder_unit(CardNum, axis[i], 0);
LTSMC.smc_set_position_unit(CardNum, axis[i], 0);
}
machine_origin = 1;
MessageBox.Show("原点已校准！");
}
}
标签：寸动,LTSMC,功能,CardNum,Delta,axis,smc,PKM 来源： https://www.cnblogs.com/charles48789982/p/14348654.html