public static subactInfo AddaSpoon(Objects 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 Pick(Objects cup)
{
subactInfo rtn = new subactInfo();
URCoordinates grip = new URCoordinates(cup.gripPos());
URCoordinates up = new URCoordinates(grip);
up.Y -= 0.1f;
URCoordinates debug = new URCoordinates(up);
debug.Y -= 0.02f;//在高一點以Rxyz有問題
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");
rtn.infotxt.Add("position");
rtn.infotxt.Add(grip.ToPos());
rtn.infotxt.Add("gripper");
rtn.infotxt.Add("31");
rtn.infotxt.Add("sleep");
rtn.infotxt.Add("1000");
rtn.infotxt.Add("position");
rtn.infotxt.Add(up.ToPos());
return(rtn);
}
public static subactInfo Stir(Objects 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 Pour(Objects toCup)
{
subactInfo rtn = new subactInfo();
URCoordinates grip = new URCoordinates(toCup.gripPos());
URCoordinates up = new URCoordinates(grip);
up.Y -= 0.1f;
URCoordinates debug = new URCoordinates(up);
debug.Y -= 0.02f;//在高一點以Rxyz有問題
URCoordinates now = new URCoordinates(up);
rtn.infotxt.Add("position");
rtn.infotxt.Add(debug.ToPos());
rtn.infotxt.Add(up.ToPos());
now.X -= 0.08f;
rtn.infotxt.Add(now.ToPos());
rtn.infotxt.Add("Rmovej");
rtn.infotxt.Add("[0,0,0,0,0,-2]");
rtn.infotxt.Add("position");
rtn.infotxt.Add(debug.ToPos());
return(rtn);
}
public static subactInfo Pick(Objects 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 Place(Objects 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(Objects 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.0f;
now.Ry += 1.0f;
rtn.infotxt.Add(now.ToPos());
now.X += 0.04f;
rtn.infotxt.Add(now.ToPos());
now.Rx -= 0.57f;
now.Ry += 0.57f;
rtn.infotxt.Add(now.ToPos());
now.X += 0.02f;
rtn.infotxt.Add(now.ToPos());
now.Rx -= 1.13f;
now.Ry += 1.13f;
rtn.infotxt.Add(now.ToPos());
up.Y -= 0.02f;//上升
rtn.infotxt.Add(up.ToPos());
rtn.infotxt.Add(debug.ToPos());
return(rtn);
}