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;
}
}
}
);
}
}
