其他分享
首页 > 其他分享> > 定点移动与实验

定点移动与实验

作者:互联网

  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