private async void ObjectFollow(string msg) { string [] data = msg.Split('*'); int speed = Convert.ToInt32(data[2]); string command = ""; if (data[0] == "1") { command = (movement.baseMovemend.AngleInDegree - speed).ToString() + "*"; } else if (data[0] == "2") { command = (movement.baseMovemend.AngleInDegree + speed).ToString() + "*"; } else { command = movement.baseMovemend.AngleInDegree.ToString() + "*"; } command += movement.elbow0.AngleInDegree.ToString() + "*"; if (data[1] == "2") { if (movement.elbow2.AngleInDegree - speed < Arm.PwmToDegree(movement.elbow2.startfrom)) { command += (movement.elbow1.AngleInDegree - speed).ToString() + "*" + movement.elbow2.AngleInDegree.ToString() + "*"; } else { command += movement.elbow1.AngleInDegree.ToString() + "*" + (movement.elbow2.AngleInDegree - speed).ToString() + "*"; } } else if (data[1] == "1") { if (movement.elbow2.AngleInDegree + speed > Arm.PwmToDegree(movement.elbow2.EndAt)) { command += (movement.elbow1.AngleInDegree + speed).ToString() + "*" + movement.elbow2.AngleInDegree.ToString() + "*"; } else { command += movement.elbow1.AngleInDegree.ToString() + "*" + (movement.elbow2.AngleInDegree + speed).ToString() + "*"; } } else { command += movement.elbow1.AngleInDegree.ToString() + "*" + movement.elbow2.AngleInDegree.ToString() + "*"; } command += movement.griperRotation.AngleInDegree.ToString() + "*" + movement.griper.AngleInDegree.ToString() + "*0*0*1.5"; await AutoModeTemplate.ScriptDefaultMovemend(command, movement); SendACK(); }
public async void Communication() { if (connected == true) { await Task.Run(() => { while (ScriptRunning) { string msg_raw = ReceiveData(); if (msg_raw == "") { break; } string ScriptMessage = msg_raw.Substring(0, 1); string msg = msg_raw.Substring(1, msg_raw.IndexOf('\0')); if (msg == "") { break; } switch (ScriptMessage) { case "1": SendTriggerStatus(); break; case "2": OnStraightLine(EventArgs.Empty); break; case "3": AutoModeTemplate.ScriptDefaultMovemend(msg, movement); SendACK(); break; case "4": MovingStatus(); break; case "5": SetMovSpeed(msg); break; case "6": SetOutput(msg); OnNewOutput(EventArgs.Empty); break; case "7": SendArmPosition(); break; case "8": Global.ScriptTargets = msg; OnDrawTargets(EventArgs.Empty); SendACK(); break; case "9": SendStreamAddress(); break; case "q": SendStreamRes(); break; case "w": ObjectFollow(msg); break; case "e": StopMovement(); break; default: break; } } } ); } }