标签：int double LTSMC conti 实验 定点 移动 smc CardNum
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
本站声明： 1. iCode9 技术分享网（下文简称本站）提供的所有内容，仅供技术学习、探讨和分享； 2. 关于本站的所有留言、评论、转载及引用，纯属内容发起人的个人观点，与本站观点和立场无关； 3. 关于本站的所有言论和文字，纯属内容发起人的个人观点，与本站观点和立场无关； 4. 本站文章均是网友提供，不完全保证技术分享内容的完整性、准确性、时效性、风险性和版权归属；如您发现该文章侵犯了您的权益，可联系我们第一时间进行删除； 5. 本站为非盈利性的个人网站，所有内容不会用来进行牟利，也不会利用任何形式的广告来间接获益，纯粹是为了广大技术爱好者提供技术内容和技术思想的分享性交流网站。