public void Add(TsPointS point, int[] buffer) { //if (_PointList[_PointList.Count - 1].C == point.C) return; _BufferList.AddRange(buffer); _PointList.Add(point); //角度 //Console.WriteLine("Angle=" + point.C); }
//將方向指向(0, 0), 計算與Y軸的夾角 public static double ComputeDefaultAngle(TsPointS start, TsPointS center, double radio) { if (radio <= 0) { radio = ComputeDistance(start, center); } double default_angle = (center.X - start.X) / radio; if (default_angle > 1) { default_angle = 1.0; } else if (default_angle < -1) { default_angle = -1.0; } return(Arc2Angle(Math.Asin(default_angle))); }
private void 机器人坐标ToolStripMenuItem_Click(object sender, EventArgs e) { if (this.dataGridView1.SelectedRows.Count == 1) { Task.Run(() => { var robot = new TsRemoteS(); robot.SetIPaddr(0, this.IpAddress, this.Port); robot.Connect(1); TsPointS p = robot.GetPsnFbkWorld(); DataGridViewRow dr = this.dataGridView1.SelectedRows.OfType <DataGridViewRow>().Single(); this.CalData.Rows[dr.Index]["x"] = p.X; this.CalData.Rows[dr.Index]["y"] = p.Y; this.CalData.Rows[dr.Index]["z"] = p.Z; Task.Run(() => { robot.Disconnect(); robot.Dispose(); }); }); } }
public void DirectDo(string command, TsPointS stopPoint, bool isWait) { _Robot.DirectDo(command); if (isWait == false || stopPoint == null) { return; } Thread.Sleep(100); TsPointS point_bak = null; int MoveStopCnt = 0; double bakAngle = 999; while ((ConnectStatus & CONNECTTYPE.SERVOON) > 0) { TsPointS point = _Robot.GetPsnFbkWork(); if ((Math.Abs(point.X - stopPoint.X) < 0.01) && (Math.Abs(point.Y - stopPoint.Y) < 0.01) && (Math.Abs(point.Z - stopPoint.Z) < 0.01) /*&& * (Math.Abs(point.C - stopPoint.C) < 0.01) && * (Math.Abs(point.T - stopPoint.T) < 0.01)*/ ) { break; } //計算角度,中心為(500, 0) double angle = (500 - point.X) / 200.0; if (angle > 1) { angle = 1.0; } else if (angle < -1) { angle = -1.0; } angle = DPoint.Arc2Angle(Math.Asin(angle)) * 2; //超過一半 if ((point.Y - 0) < 0) { angle = 180 + (180 - angle); } if (bakAngle == 999) { bakAngle = angle; } if (maxangle < angle) { maxangle = angle; } //Console.WriteLine(DateTime.Now.TimeOfDay + ": " + point.X.ToString("000.000") + ", " + point.Y.ToString("000.000") + ", " + point.Z.ToString("000.000") + ", " + point.C.ToString("000.000") + ", " + angle.ToString("000.000") + ", " + (angle - bakAngle).ToString("000.000")); bakAngle = angle; if (point_bak == null) { point_bak = point; continue; } //幾乎沒有動作,則表示機械手停止 if ((Math.Abs(point.X - point_bak.X) < 0.0001) && (Math.Abs(point.Y - point_bak.Y) < 0.0001) && (Math.Abs(point.Z - point_bak.Z) < 0.0001) /*&& * (Math.Abs(point.C - point_bak.C) < 0.0001) && * (Math.Abs(point.T - point_bak.T) < 0.0001)*/) { MoveStopCnt++; if (MoveStopCnt > 10) { Console.WriteLine("機器手強制停止"); break; } } //else MoveStopCnt = 0; point_bak = point; //Thread.Sleep(50); } Thread.Sleep(1000);//等待穩定 }
public void DirectDo(string command, TsPointS stopPoint) { DirectDo(command, stopPoint, true); }
public static double ComputeDefaultAngle(TsPointS PA, TsPointS PB) { return(ComputeDefaultAngle(PA, PB, -1)); }
public static double ComputeDistance(TsPointS PA, TsPointS PB) { return(Math.Sqrt(Math.Pow(PA.X - PB.X, 2) + Math.Pow(PA.Y - PB.Y, 2))); }
public void reBuild_MoveLser(double range_max, double range_min) { if (_LJV7Frame.buffer == null || _LJV7Frame.buffer_compute == null || _ShowPoints == null) { return; } Console.WriteLine("build: r=" + _Radio + ", angle=" + StartPoint.C + ", center=(" + center.X + ", " + center.Y + ")"); double Y_Conversion = Distance_Conversion(Y_Distance); double diff = _LJV7Frame.Max_Buffer_Compute - _LJV7Frame.Min_Buffer_Compute; if (range_max > 0 && range_min > 0) { diff = range_max - range_min; } else { range_max = _LJV7Frame.Max_Buffer_Compute; range_min = _LJV7Frame.Min_Buffer_Compute; } TsPointS startp = StartPoint; TsPointS diffPoint = new TsPointS(); for (int i = 0, k = 0; i < _LJV7Frame.size_per_frame; i++, k++) { TsPointS point = _PointList[i / _EACHSIZE]; diffPoint.X = startp.X + (point.X - startp.X) * k / _EACHSIZE; diffPoint.Y = startp.Y + (point.Y - startp.Y) * k / _EACHSIZE; diffPoint.C = DPoint.Angle2Arc(startp.C + ((point.C - startp.C) / _EACHSIZE) * k); double arc = DPoint.Angle2Arc(startp.C + ((point.C - startp.C) / _EACHSIZE) * k); for (int j = 0; j < _LJV7Frame.LJVcount; j++) { int pos = Convert.ToInt32(i * _LJV7Frame.LJVcount + j); _ShowPoints[pos].C = diffPoint.C; if (_LJV7Frame.buffer_compute[pos] == -999 || _LJV7Frame.buffer_compute[pos] < range_min || _LJV7Frame.buffer_compute[pos] > range_max) { _ShowPoints[pos].X = _ShowPoints[pos].Y = _ShowPoints[pos].Z = -999; continue; } double Color_H = (_LJV7Frame.buffer_compute[pos] - range_min) / diff; Color_H = Color_H * (GLDrawObject.MaxColorH - GLDrawObject.MinColorH) + GLDrawObject.MinColorH; GLDrawObject.HsvToRgb(Color_H, 1.0, 1.0, out _ShowPoints[pos].r, out _ShowPoints[pos].g, out _ShowPoints[pos].b); //對應到新座標: 拉平座標(實際接收資料) //_ShowPoints[pos].X = (Convert.ToDouble(i)) / Convert.ToDouble(100); //_ShowPoints[pos].Z = _Radio - _LJV7Frame.buffer_compute[pos];+ /* * //原本程式 * _ShowPoints[pos].X = (_Radio - _LJV7Frame.buffer_compute[pos]) * Math.Cos(diffPoint.C); * _ShowPoints[pos].Z = (_Radio - _LJV7Frame.buffer_compute[pos]) * Math.Sin(diffPoint.C); */ //對應到新座標: 對應回現實座標 /* * _ShowPoints[pos].X = diffPoint.X - _LJV7Frame.buffer_compute[pos] * Math.Cos(diffPoint.C) - center.X; * _ShowPoints[pos].Z = diffPoint.Y - _LJV7Frame.buffer_compute[pos] * Math.Sin(diffPoint.C) - center.Y; * _ShowPoints[pos].Y = ((_LJV7Frame.LJVcount / 2.0 - Convert.ToDouble(j)) / Convert.ToDouble(70)) + 2; */ _ShowPoints[pos].X = (Normalization(_LJV7Frame.buffer_compute[pos])) * Math.Cos(arc); _ShowPoints[pos].Z = (Normalization(_LJV7Frame.buffer_compute[pos])) * Math.Sin(arc); _ShowPoints[pos].Y = ((_LJV7Frame.LJVcount / Y_Conversion - Convert.ToDouble(j)) / Convert.ToDouble(100)); /*_ShowPoints[pos].X = (Normalization(_LJV7Frame.buffer_compute[pos])) * Math.Cos(diffPoint.C); * _ShowPoints[pos].Z = (Normalization(_LJV7Frame.buffer_compute[pos])) * Math.Sin(diffPoint.C); * _ShowPoints[pos].Y = ((_LJV7Frame.LJVcount / Y_Conversion - Convert.ToDouble(j)) / Convert.ToDouble(100)) + 2;*/ //_ShowPoints[pos].Y = ((_LJV7Frame.LJVcount / 2.0 - Convert.ToDouble(j)) / Convert.ToDouble(70)) + 2; } if (k == 99) { k = -1; startp = point; } } }
public void reBuild_MoveObject(double range_max, double range_min) { if (_LJV7Frame.buffer == null || _LJV7Frame.buffer_compute == null || _ShowPoints == null) { return; } Console.WriteLine("build: r=" + _Radio + ", angle=" + StartPoint.C + ", center=(" + center.X + ", " + center.Y + ")"); double Y_Conversion = Distance_Conversion(Y_Distance); double diff = _LJV7Frame.Max_Buffer_Compute - _LJV7Frame.Min_Buffer_Compute; if (range_max > 0 && range_min > 0) { diff = range_max - range_min; } else { range_max = _LJV7Frame.Max_Buffer_Compute; range_min = _LJV7Frame.Min_Buffer_Compute; } double min_distance = 9999; TsPointS startp = StartPoint; for (int i = 0, k = 0; i < _LJV7Frame.size_per_frame; i++, k++) { TsPointS point = _PointList[i / _EACHSIZE]; //Console.WriteLine("Angle=" + (startp.C + diffAngle * k)); double arc = DPoint.Angle2Arc(startp.C + ((point.C - startp.C) / _EACHSIZE) * k); /*if (arc > 0) * { * arc = arc; * ; * }*/ for (int j = 0; j < _LJV7Frame.LJVcount; j++) { int pos = Convert.ToInt32(i * _LJV7Frame.LJVcount + j); _ShowPoints[pos].C = arc; if (_LJV7Frame.buffer_compute[pos] == -999 || _LJV7Frame.buffer_compute[pos] < range_min || _LJV7Frame.buffer_compute[pos] > range_max) { _ShowPoints[pos].X = _ShowPoints[pos].Y = _ShowPoints[pos].Z = -999; continue; } double Color_H = (_LJV7Frame.buffer_compute[pos] - range_min) / diff; Color_H = Color_H * (GLDrawObject.MaxColorH - GLDrawObject.MinColorH) + GLDrawObject.MinColorH; GLDrawObject.HsvToRgb(Color_H, 1.0, 1.0, out _ShowPoints[pos].r, out _ShowPoints[pos].g, out _ShowPoints[pos].b); //對應到新座標: 拉平座標(實際接收資料) //_ShowPoints[pos].X = (Convert.ToDouble(i)) / Convert.ToDouble(100); //_ShowPoints[pos].Z = _Radio - fr_LJV7Frameame.buffer_compute[pos]; if (_LJV7Frame.buffer_compute[pos] < min_distance) { min_distance = _LJV7Frame.buffer_compute[pos]; } //對應到新座標: 對應回現實座標 //* //_ShowPoints[pos].X = _Radio * Math.Cos(arc); //_ShowPoints[pos].X *= (_Radio - _LJV7Frame.buffer_compute[pos]) / _Radio; //_ShowPoints[pos].Z = _Radio * Math.Sin(arc); //_ShowPoints[pos].Z *= (_Radio - _LJV7Frame.buffer_compute[pos]) / _Radio; /* * //原本程式 * _ShowPoints[pos].X = (_Radio - _LJV7Frame.buffer_compute[pos]) * Math.Cos(diffPoint.C); * _ShowPoints[pos].Z = (_Radio - _LJV7Frame.buffer_compute[pos]) * Math.Sin(diffPoint.C); * _ShowPoints[pos].Y = ((_LJV7Frame.LJVcount / 2.0 - Convert.ToDouble(j)) / Convert.ToDouble(70)) + 2; */ _ShowPoints[pos].X = (Normalization(_LJV7Frame.buffer_compute[pos])) * Math.Cos(arc); _ShowPoints[pos].Z = (Normalization(_LJV7Frame.buffer_compute[pos])) * Math.Sin(arc); _ShowPoints[pos].Y = ((_LJV7Frame.LJVcount / Y_Conversion - Convert.ToDouble(j)) / Convert.ToDouble(100)); //_ShowPoints[pos].Y = ((_LJV7Frame.LJVcount / 2.0 - Convert.ToDouble(j)) / Convert.ToDouble(70)) + 2; /*_ShowPoints[pos].X = (i/15); * _ShowPoints[pos].Z = (double)(j-288)/25; * _ShowPoints[pos].Y = (_LJV7Frame.buffer_compute[pos] - _LJV7Frame.Min_Buffer_Compute)*8;*/ } if (k == 99) { k = -1; startp = point; } } //Console.WriteLine("Max Distance = " + min_distance); }
/// <summary> /// 两点法标定工件坐标系 /// </summary> /// <param name="origin">工件坐标系原点</param> /// <param name="direction">工件坐标系中X正方向上任意一点</param> /// <returns>工件坐标系</returns> public static TsTransS CalcTransWork(TsPointS origin, TsPointS direction) => new TsTransS { X = origin.X, Y = origin.Y, Z = origin.Z, C = 180.0 / Math.PI * Math.Atan2(direction.Y - origin.Y, direction.X - origin.X) };