private void Form1_Load(object sender, EventArgs e)
{
var ws = new WebSocket("wss://test.chinf.me/listener/ur/");
ws.OnMessage += (_, ee) =>
{
switch (ee.Data)
{
case "A":
//uRServerAction.Move(new float[] { -0.394f, 0.059f, -0.093f, 0.28f, -3.2f, -0.15f });
Console.WriteLine($"Run A");
uRServerAction_left.GripperOpen();
uRServerAction_right.GripperOpen();
uRServerAction_right.Move(RobotInitial.robot_initial_pos_r);
uRServerAction_left.Move(RobotInitial.robot_initial_pos_l);
clear_all();
mode_1 = false;
mode_2 = false;
//mode_3 = false;
drink_1 = true;
cap = new Emgu.CV.VideoCapture(1, VideoCapture.API.DShow);
objectWrapper_side = new YoloWrapper("yolov3.cfg", "yolov3.weights", "coco.names");
while (mode_3)
{
Show_capture(sender, e);
}
break;
case "B":
//uRServerAction.Move(new float[] { -0.252f, -0.303f, -0.090f, 1.77f, -2.7f, 0.19f });
Console.WriteLine($"Run B");
break;
default:
Console.WriteLine($"{ee.Data} isn't know");
break;
}
};
ws.Connect();
ws.Send("BALUS");
}
public void rotate_bottle(float[] bottle_r, float[] cup_r, float[] bottle_l)
{
if (uRServerAction_right == null)
{
throw new System.IO.DriveNotFoundException("完蛋了。");
}
uRServerAction_left.GripperOpen();
uRServerAction_right.GripperOpen();
uRServerAction_right.Move(RobotInitial.robot_initial_pos_r);
uRServerAction_left.Move(RobotInitial.robot_initial_pos_l);
//uRServerAction_left.GripperCloseForceMIN();
//uRServerAction_left.Move(new float[] { bottle_l[0] + ImagePosition.image_left_x, 0.01F, bottle_l[1] + ImagePosition.image_left_y - 0.03F, 2.4F, 2.5F, 1.5F });
//uRServerAction_left.Move(new float[] { bottle_l[0] + ImagePosition.image_left_x, 0.04F, bottle_l[1] + ImagePosition.image_left_y - 0.04F, 2.4F, 2.5F, 1.5F });
//uRServerAction_left.ForceMode(0, 30);
uRServerAction_right.Move(new float[] { bottle_r[0] + ImagePosition.image_right_x, 0.250F, bottle_r[1] + ImagePosition.image_right_y + 0.04F, 0.015F, 3.18F, 0.04F });
uRServerAction_right.Move(new float[] { bottle_r[0] + ImagePosition.image_right_x, 0.250F, bottle_r[1] + ImagePosition.image_right_y + 0.01F, 0.015F, 3.18F, 0.04F });
//Thread.Sleep(2000);
//uRServerAction_left.EndForceMode();
//uRServerAction_left.Move(new float[] { bottle_l[0] + ImagePosition.image_left_x, 0.01F, bottle_l[1] + ImagePosition.image_left_y - 0.04F, 2.4F, 2.5F, 1.5F });
//uRServerAction_left.Move(RobotInitial.robot_initial_pos_l);
//uRServerAction_left.GripperOpen();
uRServerAction_right.GripperClose();
uRServerAction_right.Move(new float[] { bottle_r[0] + ImagePosition.image_right_x, 0.110F, bottle_r[1] + ImagePosition.image_right_y, 0.015F, 3.18F, 0.04F });
uRServerAction_right.Move(new float[] { RobotInitial.robot_initial_pos_rc[0], 0.110F, bottle_r[1] + ImagePosition.image_right_y, 0.015F, 3.18F, 0.04F });
uRServerAction_right.Move(RobotInitial.robot_initial_pos_rc);
RobotInitial.robot_initial_pos_lc[1] -= 0.02F;
uRServerAction_left.Move(RobotInitial.robot_initial_pos_lc);
RobotInitial.robot_initial_pos_lc[1] += 0.02F;
uRServerAction_left.Move(RobotInitial.robot_initial_pos_lc);
uRServerAction_left.TurnJoint(4, -15, 2);
uRServerAction_left.Move(RobotInitial.robot_initial_pos_lc);
uRServerAction_left.GripperClose();
RobotInitial.robot_initial_pos_lc[1] -= 0.020F;
uRServerAction_left.Move(RobotInitial.robot_initial_pos_lc);
uRServerAction_right.Move(new float[] { cup_r[0] + 0.035F, 0.110F, cup_r[1] + 0.030F, 0.015F, 3.18F, 0.04F });
uRServerAction_right.Move(new float[] { cup_r[0] + 0.035F, 0.140F, cup_r[1] + 0.030F, 0.015F, 3.18F, 0.04F });
uRServerAction_right.MoveJoint(5, 1.75F);
Thread.Sleep(3000);
uRServerAction_right.MoveJoint(5, -1.75F);
uRServerAction_right.Move(RobotInitial.robot_initial_pos_rc);
RobotInitial.robot_initial_pos_lc[1] += 0.020F;
uRServerAction_left.Move(RobotInitial.robot_initial_pos_lc);
uRServerAction_left.GripperOpen();
RobotInitial.robot_initial_pos_lc[1] -= 0.01F;
uRServerAction_left.Move(RobotInitial.robot_initial_pos_lc);
RobotInitial.robot_initial_pos_lc[1] += 0.01F;
uRServerAction_left.TurnJoint(-4, 25, 2);
RobotInitial.robot_initial_pos_rc[1] += 0.02F;
uRServerAction_right.Move(RobotInitial.robot_initial_pos_rc);
RobotInitial.robot_initial_pos_rc[1] -= 0.02F;
uRServerAction_left.Move(RobotInitial.robot_initial_pos_l);
uRServerAction_right.Move(new float[] { RobotInitial.robot_initial_pos_rc[0], 0.110F, bottle_r[1] + ImagePosition.image_right_y, 0.015F, 3.18F, 0.04F });
uRServerAction_right.Move(new float[] { bottle_r[0] + ImagePosition.image_right_x, 0.110F, bottle_r[1] + ImagePosition.image_right_y + 0.01F, 0.015F, 3.18F, 0.04F });
uRServerAction_right.Move(new float[] { bottle_r[0] + ImagePosition.image_right_x, 0.249F, bottle_r[1] + ImagePosition.image_right_y + 0.01F, 0.015F, 3.18F, 0.04F });
uRServerAction_right.GripperOpen();
uRServerAction_right.Move(new float[] { bottle_r[0] + ImagePosition.image_right_x, 0.249F, bottle_r[1] + ImagePosition.image_right_y + 0.03F, 0.015F, 3.18F, 0.04F });
uRServerAction_right.Move(RobotInitial.robot_initial_pos_r);
}
