public void RobotArmInit()
{
TcpserverEx.net_port_initial();
//TcpserverEx.card_number_connect(114);
if (robot1.is_connected())
{
int ret = robot1.initial(3, 310);//修改自己机器的型号，参数具体意义参考sdk说明文档
if (ret == 1)
{
robot1.unlock_position();//解锁
//DisplayMessage(robot1.get_card_num().ToString() + "初始化完成");
robot1.get_scara_param();
Debug.WriteLine(robot1.get_card_num().ToString() + "初始化完成");
//Debug.WriteLine(robot1.y);
}
else
{
Debug.WriteLine(robot1.get_card_num().ToString() + "初始化失败，返回值 = " + ret.ToString());
//DisplayMessage(robot1.get_card_num().ToString() + "初始化失败，返回值 = " + ret.ToString());
}
}
else
{
Debug.WriteLine(robot1.get_card_num().ToString() + "未连接");
//DisplayMessage(robot1.get_card_num().ToString() + "未连接");
}
}
public void Arminit()
{
TcpserverEx.net_port_initial();
//int card_number = int.Parse("114");
//robot1 = TcpserverEx.get_robot(card_number);
if (robot1.is_connected())
{
int ret = robot1.joint_home(1);
robot1.joint_home(3);
ret = robot1.joint_home(2);
ret = robot1.joint_home(4);
if (ret == 0)
{
DisplayMessage("ARM init error");
}
else
{
DisplayMessage(string.Format("Arm init succes {0}", ret));
}
}
else
{
DisplayMessage("Arm not connected");
}
}
public async Task Initialize()
{
CancellationTokenSource tcs = new CancellationTokenSource(TimeSpan.FromSeconds(5));
try
{
await Task.Factory.StartNew(() => TcpserverEx.net_port_initial(), tcs.Token);
await Task.Delay(1200);
this.serverStarted = true;
}
catch (TaskCanceledException)
{
throw new TaskCanceledException("TcpserverEx.net_port_initial Failed, task cancelled due to timeout");
}
catch (Exception ex)
{
throw new Exception("TcpserverEx.net_port_initial Failed", ex);
}
this.robot = TcpserverEx.get_robot(this.id);
int attempts = 0;
bool isSucccess = false;
while (attempts++ < 3)
{
isSucccess = robot.is_connected();
if (isSucccess)
{
break;
}
await Task.Delay(1000);
}
if (!isSucccess)
{
throw new Exception("Could not connect to Z-Arm with id: " + this.id);
}
isSucccess = false;
int returnCode = -1;
while (attempts++ < 3)
{
returnCode = robot.initial(generation, verticalReach);
await Task.Delay(1000);
}
if (returnCode != 1)
{
throw new Exception($"Could not initialize Z-Arm Id {this.id}, Code: {returnCode}, Message: {this.GetDebugMessage(returnCode)}");
}
robot.unlock_position();
robot.set_arm_length(armLength, armLength);
robot.set_catch_or_release_accuracy(0.5f);
}
private void Form1_FormClosing(object sender, FormClosingEventArgs e)
{
try
{
TcpserverEx.close_tcpserver();
}
catch
{ }
}
private async void idButton_Click(object sender, EventArgs e)
{
try
{
Task t = Task.Factory.StartNew(() => TcpserverEx.net_port_initial());
await t;
if (t.IsFaulted)
{
debugBox.AppendText("No net port initial\n");
}
if (t.IsCompleted)
{
debugBox.AppendText("Net port initial completed\n");
}
if (t.IsCanceled)
{
debugBox.AppendText("Cancelled");
}
if (t.Exception != null)
{
throw t.Exception;
}
}
catch (Exception ex)
{
debugBox.AppendText(ex.Message);
}
Thread.Sleep(1000);
bool found = false;
try
{
for (int i = 0; i < 256; i++)
{
if (TcpserverEx.card_number_connect(i) == 1)
{
zArmId = i;
found = true;
debugBox.AppendText(string.Format("Zarm id: {0}\n", i));
break;
}
}
}
catch (Exception ex)
{
debugBox.AppendText(ex.Message);
}
if (!found)
{
debugBox.AppendText("No arm found\n");
}
}
public IReadOnlyList <int> GetConnectedZArms()
{
List <int> ids = new List <int>();
for (int i = 0; i < 256; i++)
{
if (TcpserverEx.card_number_connect(i) == 1)
{
ids.Add(i);
}
}
return(ids);
}
public async Task Initialize(bool goHome = true)
{
Console.WriteLine("Initializing Server");
CancellationTokenSource tcs = new CancellationTokenSource(TimeSpan.FromSeconds(5));
try
{
await Task.Factory.StartNew(() => TcpserverEx.net_port_initial(), tcs.Token);
}
catch (TaskCanceledException)
{
throw new TaskCanceledException("TcpserverEx.net_port_initial Failed, task cancelled due to timeout");
}
catch (Exception ex)
{
throw new Exception("TcpserverEx.net_port_initial Failed", ex);
}
Console.WriteLine("Connecting to ZArm");
this.robot = TcpserverEx.get_robot(this.id);
Stopwatch watch = Stopwatch.StartNew();
while (!robot.is_connected())
{
if (watch.ElapsedMilliseconds > 5000)
{
throw new TimeoutException("Could not connect to Z-Arm");
}
Console.Write(".");
await Task.Delay(50);
}
Console.WriteLine($"Connected in {watch.ElapsedMilliseconds}ms");
if (goHome)
{
await this.GoHome(TimeSpan.FromSeconds(10));
}
Console.WriteLine("Initialize ZArm, unlock position, and set arm length and accuracy");
robot.initial(this.generation, this.verticalReach);
robot.set_arm_length(armLength, armLength);
robot.set_catch_or_release_accuracy(0.5f);
robot.unlock_position();
}
static void Main(string[] args)
{
ZArm robot2 = new ZArm(59);
robot2.Initialize(false).Wait();
robot2.SetAngles(20, 20, 20, -20, 20);
while (robot2.IsMoving)
{
Console.Write(".");
Thread.Sleep(50);
}
robot2.SetAngles(40, 40, 40, -50, 50);
//robot2.SetPosition(0, 0, 50, 0, 20, 20, PositionMoveMode.NonLinear);
Console.ReadKey();
TcpserverEx.close_tcpserver();
}
public static async Task <int> FindZArmId()
{
CancellationTokenSource tcs = new CancellationTokenSource(TimeSpan.FromSeconds(5));
await Task.Factory.StartNew(() => TcpserverEx.net_port_initial(), tcs.Token);
await Task.Delay(1200);
return(await Task.Factory.StartNew <int>(() =>
{
for (int i = 0; i < 256; i++)
{
if (TcpserverEx.card_number_connect(i) == 1)
{
return i;
}
}
return -1;
},
tcs.Token));
}
private async void buttonConnect_Click(object sender, EventArgs e)
{
robot = TcpserverEx.get_robot(zArmId);
bool isConnected = false;
while (!isConnected)
{
isConnected = await Task.Factory.StartNew(() => robot.is_connected());
debugBox.AppendText(string.Format("Attempting to connect to Z-Arm {0}... {1}\n", zArmId, isConnected));
await Task.Delay(1000);
}
debugBox.AppendText(string.Format("IsConnected: {0}\n", robot.is_connected()));
//debugBox.AppendText($"initial_finish: {robot.initial_finish}, communicate_success: {robot.communicate_success}\n");
int ret = -1;
while (ret != 1)
{
ret = await Task.Factory.StartNew(() => robot.initial(1, 310));
debugBox.AppendText($"ret: {ret}\n");
await Task.Delay(1000);
}
if (ret != 1)
{
debugBox.AppendText(string.Format("Could not initialize Z Arm id {0}, return value: {1}\n", zArmId, ret));
}
else
{
robot.unlock_position();
robot.set_arm_length(200, 200);
robot.set_catch_or_release_accuracy(0.5f);
debugBox.AppendText(string.Format("Z Arm id: {0} connected!\n", zArmId));
}
}
public bool ConnectToZArm(int zArmId)
{
this.robot = TcpserverEx.get_robot(zArmId);
return(this.robot.is_connected());
}
private int verticalReach = 310; // mm
public void InitializeTcpServer()
{
TcpserverEx.net_port_initial();
}
private async void connectButton_Click(object sender, EventArgs e)
{
try
{
Task task = Task.Factory.StartNew(() => TcpserverEx.net_port_initial());
await task;
if (task.IsFaulted)
{
debugTextbox.AppendText("No net port initial\r\n");
}
if (task.IsCanceled)
{
debugTextbox.AppendText("Cancelled\r\n");
}
if (task.IsCompleted)
{
debugTextbox.AppendText("Net port initial completed\r\n");
}
if (task.Exception != null)
{
throw task.Exception;
}
}
catch (Exception ex)
{
debugTextbox.AppendText(ex.Message);
return;
}
// Wait for net_port_initial to complete
await Task.Delay(1200);
debugTextbox.AppendText("Finding Z-Arm. \r\n");
bool found = false;
try
{
for (int i = 0; i < 256; i++)
{
if (TcpserverEx.card_number_connect(i) == 1)
{
zArmId = i;
found = true;
debugTextbox.AppendText($"Id: {i}\r\n");
break;
}
}
}
catch (Exception ex)
{
debugTextbox.AppendText(ex.Message);
return;
}
if (!found)
{
debugTextbox.AppendText("No arm found. Retry.\r\n");
return;
}
// Connect to the Z-Arm
robot = TcpserverEx.get_robot(zArmId);
int retryCount = 0;
bool isConnected = false;
while (!isConnected)
{
isConnected = await Task.Factory.StartNew(() => robot.is_connected());
debugTextbox.AppendText($"Attempting to connect to Z-Arm {zArmId}: {isConnected}\r\n");
if (++retryCount > 3)
{
break;
}
await Task.Delay(1000);
}
if (retryCount >= 3)
{
debugTextbox.AppendText($"Could not connect. Retry.\r\n");
return;
}
int ret = -1;
retryCount = 0;
while (ret != 1)
{
ret = await Task.Factory.StartNew(() => robot.initial(generation, verticalReach));
debugTextbox.AppendText($"Initializing: {ret} = {initialDebugMap[ret]}\r\n");
if (++retryCount > 3)
{
break;
}
await Task.Delay(1000);
}
if (ret != 1)
{
debugTextbox.AppendText($"Could not initialize Z-Arm Id {zArmId}, return value: {ret}\r\n");
return;
}
else
{
robot.unlock_position();
robot.set_arm_length(armLength, armLength);
robot.set_catch_or_release_accuracy(0.5f);
debugTextbox.AppendText($"Z Arm Id {zArmId} Initialized!\r\n");
SetManualControls(true);
}
}
public Controller()
{
robot1 = TcpserverEx.get_robot(114);
}
public static void mach()
{
Console.WriteLine("Init");
TcpserverEx.net_port_initial();
ControlBeanEx robot = TcpserverEx.get_robot(59);
Stopwatch watch = Stopwatch.StartNew();
while (!robot.is_connected())
{
Console.Write(".");
Thread.Sleep(1);
}
Console.WriteLine();
Console.WriteLine($"Took {watch.ElapsedMilliseconds}ms to connect!");
//watch.Restart();
Console.WriteLine("Joint home 1: " + robot.joint_home(1));
Console.WriteLine("Joint home 2: " + robot.joint_home(2));
Console.WriteLine("Joint home 3: " + robot.joint_home(3));
Console.WriteLine("Joint home 4: " + robot.joint_home(4));
//Console.ReadKey();
//Console.WriteLine($"Time to Home: {watch.ElapsedMilliseconds}");
//Console.ReadKey();
//while (true)
//{
// if (watch.ElapsedMilliseconds > 15000) break;
// robot.get_scara_param();
// Console.WriteLine(
// $"target: {robot.is_robot_goto_target()}, " +
// $"move flag: {robot.move_flag}, " +
// $"Angle 1: {robot.angle1}, " +
// $"Angle 1 Judge: {robot.angle1_after_judge}, " +
// $"Angle 2: {robot.angle2}, " +
// $"Angle 2 Judge: {robot.angle2_after_judge}, " +
// $"Init Finished: {robot.initial_finish}");
// Thread.Sleep(100);
//}
Thread.Sleep(3000);
// robot.set_arm_length(200, 200); DO NOT SET ARM LENGTH WHEN JOINT_HOME IS RUNNING!!!
Console.WriteLine("Initialize: " + robot.initial(1, 310));
robot.get_scara_param();
Console.WriteLine(robot.initial_finish);
Console.WriteLine("Unlock Position: " + robot.unlock_position());
watch.Restart();
while (true)
{
if (watch.ElapsedMilliseconds > 15000)
{
break;
}
robot.get_scara_param();
Console.WriteLine(
//$"target: {robot.is_robot_goto_target()}, " +
$"move flag: {robot.move_flag}, " +
$"Angle 1: {robot.angle1}, " +
$"Angle 1 Judge: {robot.angle1_after_judge}, " +
$"Angle 2: {robot.angle2}, " +
$"Angle 2 Judge: {robot.angle2_after_judge}, " +
$"Angle 3: {robot.rotation}, " +
$"Init Finished: {robot.initial_finish}");
Thread.Sleep(100);
}
robot.set_catch_or_release_accuracy(0.5f);
//Console.WriteLine("At Home");
//Console.WriteLine("Going Home");
//while (robot.move_flag)
//{
// Console.Write(".");
// Thread.Sleep(1);
//}
//robot.set_arm_length(200, 200);
//robot.set_catch_or_release_accuracy(0.5f);
//Console.WriteLine("Unlock Position: " + robot.unlock_position());
//Console.WriteLine("xyz move: " + robot.xyz_move(1, -10, 50));
// Console.WriteLine("Cooperation State: " + robot.set_cooperation_fun_state(true));
//Console.WriteLine("Drag Teach: " + robot.set_drag_teach(true));
}
