private void picBox_route_Paint(object sender, PaintEventArgs e)
{
Graphics objGraphic = e.Graphics;
Pen drawLine = new Pen(Color.Green, 1);
Pen CurrentLine = new Pen(Color.DarkOrange, 1);
SolidBrush drawPoint = new SolidBrush(Color.DodgerBlue);
SolidBrush drawCurrent = new SolidBrush(Color.Red);
for (RoutePoint pointScan = PointHead.Next; pointScan != null; pointScan = pointScan.Next)
{
Point pointNormal = Normalization(pointScan.X + pointScan.shiftX, -pointScan.Y - pointScan.shiftY);
pointScan.NormalizationX = pointNormal.X;
pointScan.NormalizationY = pointNormal.Y;
pointScan.LinePointUpdata(pointScan.NormalizationX, pointScan.NormalizationY, pointScan.RX + pointScan.shiftA);
objGraphic.FillEllipse(drawPoint, pointScan.NormalizationX - 5, pointScan.NormalizationY - 5, 10, 10);
objGraphic.DrawLine(drawLine, pointScan.NormalizationX, pointScan.NormalizationY, pointScan.LinePoint.X, pointScan.LinePoint.Y);
objGraphic.DrawString(pointScan.ID, new Font("標楷體", 12), Brushes.Green, pointScan.NormalizationX, pointScan.NormalizationY - 15);
}
if (PointCurrent != null)
{
objGraphic.FillEllipse(drawCurrent, PointCurrent.NormalizationX - 5, PointCurrent.NormalizationY - 5, 10, 10);
objGraphic.DrawLine(CurrentLine, PointCurrent.NormalizationX, PointCurrent.NormalizationY, PointCurrent.LinePoint.X, PointCurrent.LinePoint.Y);
objGraphic.DrawString(PointCurrent.ID, new Font("標楷體", 12), Brushes.Red, PointCurrent.NormalizationX, PointCurrent.NormalizationY - 15);
}
}
private void btn_prevPoint_Click(object sender, EventArgs e)
{
if (isPicBoxClick)
{
if (PointCurrent.Prev == PointHead)
{
MessageBox.Show("已到第一個點", "注意");
}
else if (PointCurrent != null)
{
PointCurrent = PointCurrent.Prev;
lbl_pointID.Text = "路徑點編號: " + PointCurrent.ID;
textBox_UI_Updata();
picBox_route.Refresh();
}
}
}
private void picBox_route_MouseDown(object sender, MouseEventArgs e)
{
PointCurrent = null;
float dis_min = 10;
for (RoutePoint pointLoad = PointHead.Next; pointLoad != null; pointLoad = pointLoad.Next)
{
float distance = (float)Math.Sqrt((pointLoad.NormalizationX - e.X) * (pointLoad.NormalizationX - e.X) + (pointLoad.NormalizationY - e.Y) * (pointLoad.NormalizationY - e.Y));
if (distance < dis_min)
{
dis_min = distance;
PointCurrent = pointLoad;
break;
}
}
if (PointCurrent != null)
{
lbl_pointID.Text = "路徑點編號: " + PointCurrent.ID;
textBox_UI_Updata();
}
picBox_route.Refresh();
}
private void readData()
{
buffer = new byte[2048];
RoutePoint pointScan = null;
while (connectFlag)
{
if (moveJ)
{
//cmd = "moveJ([0,0,0,0,0,0],v = 10,delayStatus = 0,delayTime = 0,RO_Status = 00000000000000000000000000000000)";
cmd = "moveJ([" + tBx_J1.Text + "," + tBx_J2.Text + "," + tBx_J3.Text + ","
+ tBx_J4.Text + "," + tBx_J5.Text + "," + tBx_J6.Text + "],v = "
+ tBx_speed.Text + ",delayStatus = 0,delayTime = 0,RO_Status = 00000000000000000000000000000000)";
moveJ = false;
}
else if (moveL)
{
//cmd = "moveL([0,0,0,0,0,0],v = 10,delayStatus = 0,delayTime = 0,RO_Status = 00000000000000000000000000000000)";
cmd = "moveL([" + tBx_X.Text + "," + tBx_Y.Text + "," + tBx_Z.Text + ","
+ tBx_RX.Text + "," + tBx_RY.Text + "," + tBx_RZ.Text + "],v = "
+ tBx_speed.Text + ",delayStatus = 0,delayTime = 0,RO_Status = 00000000000000000000000000000000)";
moveL = false;
}
else if (playRoute)
{
if (fristFlag)
{
routePoint = PointHead.Next;
fristFlag = false;
}
tmp++;
if (routePoint != null)
{
if (tmp % 2 == 1)
{
cmd = "moveL([" + routePoint.X.ToString() + "," + routePoint.Y.ToString() + "," + routePoint.Z.ToString() + ","
+ routePoint.RX.ToString() + "," + routePoint.RY.ToString() + "," + routePoint.RZ.ToString() + "],v = 50"
+ ",delayStatus = 0,delayTime = 0,RO_Status = 00000000000000000000000000000000)";
routePoint = routePoint.Next;
}
else if (tmp % 2 == 0)
{
cmd = "moveL([" + routePoint.Prev.X.ToString() + "," + routePoint.Prev.Y.ToString() + "," + (routePoint.Prev.Z + Math.Abs(routePoint.Prev.Z - routePoint.Z) + 20).ToString() + ","
+ routePoint.Prev.RX.ToString() + "," + routePoint.Prev.RY.ToString() + "," + routePoint.Prev.RZ.ToString() + "],v = 50"
+ ",delayStatus = 0,delayTime = 0,RO_Status = 00000000000000000000000000000000)";
}
}
else
{
playRoute = false;
fristFlag = true;
}
}
else if (!(moveJ && moveL))
{
cmd = "Got Info()";
}
data = Encoding.UTF8.GetBytes(cmd);
socket.Send(data);
int length = socket.Receive(buffer);
string msg = Encoding.ASCII.GetString(buffer, 0, length);
RobotInfo(msg);
Thread.Sleep(200);
}
}
private void routeLoad(string path)
{
string str;
try
{
str = "";
StreamReader sr = new StreamReader(path, Encoding.Default);
str = sr.ReadLine();
if (isPicBoxClick)
{
RoutePoint pointScan = PointHead;
Xmax = -9999999;
Xmin = 9999999;
Ymax = -9999999;
Ymin = 9999999;
Zmax = -9999999;
Zmin = 9999999;
while (str != null)
{
string[] temparray = str.Split(',');
RoutePoint pointNew = new RoutePoint();
pointNew.ID = temparray[0];
pointNew.X = Convert.ToDouble(temparray[1]);
pointNew.Y = Convert.ToDouble(temparray[2]);
pointNew.Z = Convert.ToDouble(temparray[3]);
pointNew.RX = Convert.ToDouble(temparray[4]);
pointNew.RY = Convert.ToDouble(temparray[5]);
pointNew.RZ = Convert.ToDouble(temparray[6]);
//pointNew.shiftX = Convert.ToDouble(temparray[7]);
//pointNew.shiftY = Convert.ToDouble(temparray[8]);
//pointNew.shiftZ = Convert.ToDouble(temparray[9]);
//pointNew.shiftA = Convert.ToDouble(temparray[10]);
//pointNew.shiftB = Convert.ToDouble(temparray[11]);
//pointNew.shiftC = Convert.ToDouble(temparray[12]);
pointScan.Next = pointNew;
pointNew.Prev = pointScan;
pointNew.Next = null;
pointScan = pointNew;
if (pointNew.X > Xmax)
{
Xmax = pointNew.X;
}
if (pointNew.X < Xmin)
{
Xmin = pointNew.X;
}
if (-pointNew.Y > Ymax)
{
Ymax = -pointNew.Y;
}
if (-pointNew.Y < Ymin)
{
Ymin = -pointNew.Y;
}
if (-pointNew.Z > Zmax)
{
Zmax = -pointNew.Z;
}
if (-pointNew.Z < Zmin)
{
Zmin = -pointNew.Z;
}
str = sr.ReadLine();
}
picBox_route.Enabled = true;
}
}
catch (Exception ex) { }
System.GC.Collect();
}
