public static subactInfo Pick(Object cup)
{
subactInfo rtn = new subactInfo();
URCoordinates up = new URCoordinates(cup.gripPos());
URCoordinates debug = new URCoordinates(up);
URCoordinates down = new URCoordinates(up);
debug.Y -= 0.02f;//上升
rtn.infotxt.Add("position");
rtn.infotxt.Add(debug.ToPos());
rtn.infotxt.Add(up.ToPos());
rtn.infotxt.Add("gripper");
rtn.infotxt.Add("0");
rtn.infotxt.Add("sleep");
rtn.infotxt.Add("1000");
down.Y += 0.07f;//下降
rtn.infotxt.Add("position");
rtn.infotxt.Add(down.ToPos());
rtn.infotxt.Add("gripper");
rtn.infotxt.Add("35");
rtn.infotxt.Add("sleep");
rtn.infotxt.Add("1000");
up.Y -= 0.02f;//上升
rtn.infotxt.Add("position");
rtn.infotxt.Add(up.ToPos());
return(rtn);
}
public static subactInfo Stir(Object toCup)
{
subactInfo rtn = new subactInfo();
URCoordinates up = new URCoordinates(toCup.gripPos());
up.X -= 0.015f;
up.Z += 0.015f;
up.Y -= 0.06f;//上升
URCoordinates now = new URCoordinates(up);
rtn.infotxt.Add("pmovej");
rtn.infotxt.Add(up.ToPos());
now.Y += 0.05f;//下去
now.X -= 0.01f;
rtn.infotxt.Add(now.ToPos());
now.X += 0.01f;
now.Z += 0.01f;
rtn.infotxt.Add(now.ToPos());
now.X += 0.01f;
now.Z -= 0.01f;
rtn.infotxt.Add(now.ToPos());
now.X -= 0.01f;
now.Z -= 0.01f;
rtn.infotxt.Add(now.ToPos());
now.X -= 0.01f;
now.Z += 0.01f;
rtn.infotxt.Add(now.ToPos());
rtn.infotxt.Add(up.ToPos());
return(rtn);
}
public static subactInfo AddaSpoon(Object toCup)
{
subactInfo rtn = new subactInfo();
URCoordinates up = new URCoordinates(toCup.gripPos());
up.Rx = 2.2f;
up.Ry = -2.2f;
up.Rz = 0;
up.X -= 0.09f;
up.Z += 0.02f;
up.Y += 0.03f;//下降
URCoordinates upper = new URCoordinates(up);
upper.Y -= 0.07f;//上
rtn.infotxt.Add("pmovej");
rtn.infotxt.Add(upper.ToPos());
URCoordinates add = new URCoordinates(up);
add.Rx = 3.14f;
add.Ry = 0f;
add.Rz = 0f;
add.Y -= 0.09f;//上升
add.X += 0.05f;
rtn.infotxt.Add("pmovej");
rtn.infotxt.Add(up.ToPos());
rtn.infotxt.Add("pmovej");
rtn.infotxt.Add(add.ToPos());
rtn.infotxt.Add("pmovej");
add.Y -= 0.03f;//上升
rtn.infotxt.Add(add.ToPos());
return(rtn);
}
public static subactInfo Place(Object cup, URCoordinates Wpoint)
{
cup.setNowPos(Wpoint);
subactInfo rtn = new subactInfo();
URCoordinates up = new URCoordinates(cup.gripPos());
URCoordinates debug = new URCoordinates(up);
URCoordinates down = new URCoordinates(up);
down.Y += 0.07f; //下降
debug.Y -= 0.02f; //上升
rtn.infotxt.Add("position");
rtn.infotxt.Add(debug.ToPos());
rtn.infotxt.Add(up.ToPos());
rtn.infotxt.Add(down.ToPos());
rtn.infotxt.Add("gripper");
rtn.infotxt.Add("0");
rtn.infotxt.Add("sleep");
rtn.infotxt.Add("1000");
rtn.infotxt.Add("position");
rtn.infotxt.Add(up.ToPos());
return(rtn);
}
public static subactInfo Pour(Object toCup)
{
subactInfo rtn = new subactInfo();
URCoordinates up = new URCoordinates(toCup.gripPos());
up.Y -= 0.03f;//上升
URCoordinates debug = new URCoordinates(up);
debug.Y -= 0.01f;//上升
URCoordinates now = new URCoordinates(up);
rtn.infotxt.Add("position");
rtn.infotxt.Add(debug.ToPos());
rtn.infotxt.Add(up.ToPos());
now.X -= 0.09f;
rtn.infotxt.Add(now.ToPos());
now.Rx -= 1.57f;
now.Ry += 1.57f;
rtn.infotxt.Add(now.ToPos());
now.Rx -= 1.13f;
now.Ry += 1.13f;
now.X += 0.06f;
rtn.infotxt.Add(now.ToPos());
up.Y -= 0.02f;//上升
rtn.infotxt.Add(up.ToPos());
rtn.infotxt.Add(debug.ToPos());
return(rtn);
}
public URCoordinates(URCoordinates input)
{
X = input.X;
Y = input.Y;
Z = input.Z;
Rx = input.Rx;
Ry = input.Ry;
Rz = input.Rz;
}
public void goToPos(URCoordinates goC, bool waitDone = false)
{
StreamWriter txt;
txt = new StreamWriter("Pos//GoTo.pos", false);
txt.WriteLine("position");
txt.WriteLine(goC.ToPos());
txt.Flush();
txt.Close();
ReadingFile = "Pos//GoTo.pos";
cmd = mode.moveByFile;
if (waitDone)
{
while (cmd == mode.stop)
{
;
}
}
}
}//on click
private void button_Larm_goPos_Click(object sender, EventArgs e)
{
if (comboBox_LselectPos.Text == "" || comboBox_LselectPos.Text == "goPos")
{
URCoordinates goC = new URCoordinates(textBox_Larm_Xpos.toPos_M(), textBox_Larm_Ypos.toPos_M(), textBox_Larm_Zpos.toPos_M(), textBox_Larm_Rxrad.toRad(), textBox_Larm_Ryrad.toRad(), textBox_Larm_Rzrad.toRad());
if (textBox_endDeg.Text == "0")//水平
{
goC.Rx = (float)(Math.PI);
goC.Ry = 0;
goC.Rz = 0;
}
else if (textBox_endDeg.Text == "1")//垂直
{
goC.Rx = (float)(Math.PI * 1.5);
goC.Ry = 0;
goC.Rz = 0;
}
else if (textBox_endDeg.Text == "2")//45度
{
goC.Rx = (float)(Math.PI * 1.75);
goC.Ry = 0;
goC.Rz = 0;
}
UR.goToPos(goC);
}
else//看檔案
{
string Extension = comboBox_LselectPos.Text.Substring(comboBox_LselectPos.Text.IndexOf("."));
if (Extension == ".pos")
{
UR.goToFilePos(comboBox_LselectPos.Text);
}
if (Extension == ".path")
{
UR.goFilePath(comboBox_LselectPos.Text);
}
}
}
public void setNowPos(URCoordinates urc)
{
nowPos = new URCoordinates(urc);
}
private URCoordinates nowPos = new URCoordinates();//再action中 可能因為中間移動過 所以必須記錄移動後的位置 以免之後抓空
public void saveNowPos()
{
nowPos = new URCoordinates(getX_m(), getY_m(), getZ_m(), 0, 0, 0);
}
