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)
};
