定点移动与实验
作者：互联网
int wq = 0;
private void button18_Click(object sender, EventArgs e)
{
OpenFileDialog openDlg = new OpenFileDialog();
openDlg.Title = "打开数据文件";
openDlg.Filter = "数据文件 (*.txt)|*.txt|All|*.*";
bool result = false;
string FilePath;
string[] TempData;
string[] TempArry;
Row = 0;
Col = 0;
if (openDlg.ShowDialog() == DialogResult.OK)
{
FilePath = openDlg.FileName;
TempData = System.IO.File.ReadAllLines(FilePath, System.Text.Encoding.Default);
Row = TempData.GetLength(0);
TempArry = TempData[1].Split(',');
Col = TempArry.GetLength(0);
Date = new double[Row, Col];
for (int i = 0; i < Row; i++)
{
string[] Arry = TempData[i].Split(',');
for (int j = 0; j < Col; j++)
{
Date[i, j] = Convert.ToDouble(Arry[j]);
}
}
result = true;
}
if (result)
{
double[] X = new double[Row]; double[] Y = new double[Row]; double[] Z = new double[Row];
for (int j = 0; j < Row; j++)
{
X[j] = Date[j, 0]; Y[j] = Date[j, 1]; Z[j] = Date[j, 2];
}
Inverse.inverse_run(X, Y, Z);
for (int k = 0; k < Row; k = k + 1)
{
xx1[k, 0] = Math.Round(Inverse.B1[k, 0]);//每个电机的编码器位置
xx1[k, 1] = Math.Round(Inverse.B1[k, 1]);
xx1[k, 2] = Math.Round(Inverse.B1[k, 2]);
}
MessageBox.Show("路径读取完成！");
}
else
{
MessageBox.Show("路径读取错误！");
return;
}
wq = 1;
}
private void button20_Click(object sender, EventArgs e)
{
if (wether_link == 0)
{
MessageBox.Show("请先打开设备");
return;
}
if (SF == 0)
{
MessageBox.Show("请先打开伺服");
return;
}
for (int k = 0; k < axis_all.Length; k = k + 1)
{
LTSMC.smc_set_alm_mode(CardNum, axis_all[k], 0, 0, 0);//设置报警使能，关闭报警
LTSMC.smc_write_sevon_pin(CardNum, axis_all[k], 0);//打开伺服使能
}
int xh = 0;//轨迹点整除余数
int kk = 0;
double speed = decimal.ToDouble(run_speed.Value) * (fp / (2 * Math.PI)); //单位：unit/s
ThreadPool.QueueUserWorkItem(
delegate
{
LTSMC.smc_conti_set_lookahead_mode(CardNum, 0, 0, 0, 0, 0);
LTSMC.smc_conti_open_list(CardNum, 0, 3, axis_all);
LTSMC.smc_set_vector_profile_unit(CardNum, 0, speed, speed, acc / 2, dec / 2, speed);//设置插补运动速度参数
LTSMC.smc_set_vector_s_profile(CardNum, 0, 0, acc / 2);//设置s曲线平滑时间
LTSMC.smc_conti_change_speed_ratio(CardNum, 0, 0.5);
LTSMC.smc_conti_line_unit(CardNum, 0, 3, axis_all, new double[] { xx1[0, 0], xx1[0, 1], xx1[0, 2] }, 1, 0);//运动模式，0： 相对坐标模式， 1： 绝对坐标模式
LTSMC.smc_conti_delay(CardNum, 0, 3, 0);
for (int j = 20; j < xx1.GetLength(0); j = j + 20)
{
for (int i = 0 + 20 * kk; i < xh + j; i = i + 1)
{
LTSMC.smc_conti_line_unit(CardNum, 0, 3, axis_all, new double[] { xx1[i, 0], xx1[i, 1], xx1[i, 2] }, 1, 0);//运动模式，0： 相对坐标模式， 1： 绝对坐标模式
}
LTSMC.smc_conti_delay(CardNum, 0, 2, 0);
kk = kk + 1;
}
LTSMC.smc_conti_start_list(CardNum, 0);//启动插补
LTSMC.smc_conti_close_list(CardNum, 0);//关闭插补缓冲区
}
);
}
private void Designated_moving_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;
}
if (Designated_x.Text.Trim() == "" || Designated_y.Text.Trim() == "" || Designated_z.Text.Trim() == "")
{
MessageBox.Show("未定义移动点");
return;
}
double[] theta = new double[3];
double X1 = Convert.ToDouble(Designated_x.Text);
double Y1 = Convert.ToDouble(Designated_y.Text);
double Z1 = Convert.ToDouble(Designated_z.Text);
Inverse.inverse_position(X1, Y1, Z1);//定点移动的距离坐标，通过逆解计算出编码器的位置，再插补到该点上。
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 * Convert.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： 绝对坐标模式
Designated_x.Text = "";
Designated_y.Text = "";
}
private void button18_Click(object sender, EventArgs e)
{
OpenFileDialog openDlg = new OpenFileDialog();
openDlg.Title = "打开数据文件";
openDlg.Filter = "数据文件 (*.txt)|*.txt|All|*.*";
bool result = false;
string FilePath;
string[] TempData;
string[] TempArry;
Row = 0;
Col = 0;
if (openDlg.ShowDialog() == DialogResult.OK)
{
FilePath = openDlg.FileName;
TempData = System.IO.File.ReadAllLines(FilePath, System.Text.Encoding.Default);
Row = TempData.GetLength(0);
TempArry = TempData[1].Split(',');
Col = TempArry.GetLength(0);
Date = new double[Row, Col];
for (int i = 0; i < Row; i++)
{
string[] Arry = TempData[i].Split(',');
for (int j = 0; j < Col; j++)
{
Date[i, j] = Convert.ToDouble(Arry[j]);
}
}
result = true;
}
if (result)
{
double[] X = new double[Row]; double[] Y = new double[Row]; double[] Z = new double[Row];
for (int j = 0; j < Row; j++)
{
X[j] = Date[j, 0]; Y[j] = Date[j, 1]; Z[j] = Date[j, 2];
}
Inverse.inverse_run(X, Y, Z);
for (int k = 0; k < Row; k = k + 1)
{
xx1[k, 0] = Math.Round(Inverse.B1[k, 0]);//每个电机的编码器位置
xx1[k, 1] = Math.Round(Inverse.B1[k, 1]);
xx1[k, 2] = Math.Round(Inverse.B1[k, 2]);
}
MessageBox.Show("路径读取完成！");
}
else
{
MessageBox.Show("路径读取错误！");
return;
}
wq = 1;
}
private void one_opint_Click(object sender, EventArgs e)
{
if (wether_link == 0)
{
MessageBox.Show("请先打开设备");
return;
}
if (SF == 0)
{
MessageBox.Show("请先打开伺服");
return;
}
for (int k = 0; k < axis_all.Length; k = k + 1)
{
LTSMC.smc_set_alm_mode(CardNum, axis_all[k], 0, 0, 0);//设置报警使能，关闭报警
LTSMC.smc_write_sevon_pin(CardNum, axis_all[k], 0);//打开伺服使能
}
ThreadPool.QueueUserWorkItem(
delegate
{
LTSMC.smc_conti_set_lookahead_mode(CardNum, 0, 1, 600, 0, 0);
LTSMC.smc_conti_open_list(CardNum, 0, 3, axis_all);
for (int i = 0; i < (N + 1); i = i + 1)
{
LTSMC.smc_set_vector_profile_unit(CardNum, 0, v1[i]*2, v1[i] * 2, ac, dc, v1[i] * 2);//设置插补运动速度参数
LTSMC.smc_conti_line_unit(CardNum, 0, 3, axis_all, new double[] { xx1[i, 0], xx1[i, 1], xx1[i, 2] }, 1, 0);//运动模式，0： 相对坐标模式， 1： 绝对坐标模式
}
LTSMC.smc_conti_delay(CardNum, 0, 1, 0);
for (int i = 1; i < (N + 1); i = i + 1)
{
LTSMC.smc_set_vector_profile_unit(CardNum, 0, v2[i] * 2, v2[i] * 2, ac, dc, v2[i] * 2);//设置插补运动速度参数
LTSMC.smc_conti_line_unit(CardNum, 0, 3, axis_all, new double[] { xx2[i, 0], xx2[i, 1], xx2[i, 2] }, 1, 0);//运动模式，0： 相对坐标模式， 1： 绝对坐标模式
}
LTSMC.smc_conti_delay(CardNum, 0, 1, 0);
for (int i = 1; i < (N + 1); i = i + 1)
{
LTSMC.smc_set_vector_profile_unit(CardNum, 0, v1[i], v1[i], ac, dc, v1[i]);//设置插补运动速度参数
LTSMC.smc_conti_line_unit(CardNum, 0, 3, axis_all, new double[] { xx1[i, 0], xx1[i, 1], xx1[i, 2] }, 1, 0);//运动模式，0： 相对坐标模式， 1： 绝对坐标模式
}
LTSMC.smc_conti_delay(CardNum, 0, 1, 0);
for (int i = 1; i < (N + 1); i = i + 1)
{
LTSMC.smc_set_vector_profile_unit(CardNum, 0, v2[i], v2[i], ac, dc, v2[i]);//设置插补运动速度参数
LTSMC.smc_conti_line_unit(CardNum, 0, 3, axis_all, new double[] { xx2[i, 0], xx2[i, 1], xx2[i, 2] }, 1, 0);//运动模式，0： 相对坐标模式， 1： 绝对坐标模式
}
LTSMC.smc_conti_delay(CardNum, 0, 1, 0);
for (int i = 1; i < (N + 1); i = i + 1)
{
LTSMC.smc_set_vector_profile_unit(CardNum, 0, v1[i], v1[i], ac, dc, v1[i]);//设置插补运动速度参数
LTSMC.smc_conti_line_unit(CardNum, 0, 3, axis_all, new double[] { xx1[i, 0], xx1[i, 1], xx1[i, 2] }, 1, 0);//运动模式，0： 相对坐标模式， 1： 绝对坐标模式
}
LTSMC.smc_conti_delay(CardNum, 0, 1, 0);
for (int i = 1; i < (N + 1); i = i + 1)
{
LTSMC.smc_set_vector_profile_unit(CardNum, 0, v2[i], v2[i], ac, dc, v2[i]);//设置插补运动速度参数
LTSMC.smc_conti_line_unit(CardNum, 0, 3, axis_all, new double[] { xx2[i, 0], xx2[i, 1], xx2[i, 2] }, 1, 0);//运动模式，0： 相对坐标模式， 1： 绝对坐标模式
}
LTSMC.smc_conti_start_list(CardNum, 0);//启动插补
LTSMC.smc_conti_close_list(CardNum, 0);//关闭插补缓冲区
}
);
}
//执行PTP运动，涉及轨迹规划
private void gripping_Click(object sender, EventArgs e)
{
Point1[0] = teach_point[0, 0];
Point1[1] = teach_point[0, 1];
Point1[2] = teach_point[0, 2];
Point2[0] = teach_point[1, 0];
Point2[1] = teach_point[1, 1];
Point2[2] = teach_point[1, 2];
Point3[0] = teach_point[2, 0];
Point3[1] = teach_point[2, 1];
Point3[2] = teach_point[2, 2];
for (int i = 0; i < 3; i++)
{
Point1[i] = Point1[i] / (fp / 360);//1点所对应的转角
Point2[i] = Point2[i] / (fp / 360);
Point3[i] = Point3[i] / (fp / 360);
}
double[] P1 = new double[3];
double[] P2 = new double[3];
double[] P3 = new double[3];
Forward.foward_calculate(Point1[0], Point1[1], Point1[2]);
P1[0] = Forward.instantaneous_Gx;
P1[1] = Forward.instantaneous_Gy;
P1[2] = Forward.instantaneous_Gz;
Forward.foward_calculate(Point2[0], Point2[1], Point2[2]);
P2[0] = Forward.instantaneous_Gx;
P2[1] = Forward.instantaneous_Gy;
P2[2] = Forward.instantaneous_Gz;
Forward.foward_calculate(Point3[0], Point3[1], Point3[2]);
P3[0] = Forward.instantaneous_Gx;
P3[1] = Forward.instantaneous_Gy;
P3[2] = Forward.instantaneous_Gz;
if (wether_link == 0)
{
MessageBox.Show("请先打开设备");
return;
}
if (SF == 0)
{
MessageBox.Show("请先打开伺服");
return;
}
else
{
for (int k = 0; k < axis_all.Length; k = k + 1)
{
LTSMC.smc_set_alm_mode(CardNum, axis_all[k], 0, 0, 0);//设置报警使能，关闭报警
LTSMC.smc_write_sevon_pin(CardNum, axis_all[k], 0);//打开伺服使能
}
Pick_Down(P1, P2, P3);
}
}
标签：int,double,LTSMC,conti,实验,定点,移动,smc,CardNum 来源： https://www.cnblogs.com/charles48789982/p/14348718.html