//TODO: fix keyboard loop.
public void keyboardLoop(string _keyInput, long _stepResolution, ref Socket _clientSocket, GH_Component component)
{
string presentValue = Util.ReadVariable(ref _clientSocket, "$POS_ACT", component);
E6POS currentPos = new E6POS();
currentPos.DeserializeE6POS(presentValue);
if (_keyInput != "None")
{
var k = keyToVectorCalc(_keyInput, _stepResolution);
currentPos.UpdateE6Pos(k[0], k[1], k[2], k[3], k[4], k[5]);
}
}
public static string calculateTargetE6Pos(Plane _targetPlane, Plane _worldPlane)
{
double _targetX = _targetPlane.OriginX;
double _targetY = _targetPlane.OriginY;
double _targetZ = _targetPlane.OriginZ;
Transform rotMatrix = Transform.ChangeBasis(_targetPlane, _worldPlane);
double _targetA = Rhino.RhinoMath.ToDegrees(Math.Atan2(rotMatrix[2, 1], rotMatrix[2, 2]));
double _targetB = Rhino.RhinoMath.ToDegrees(-Math.Atan2(rotMatrix[2, 0], Math.Sqrt(rotMatrix[2, 1] * rotMatrix[2, 1] + rotMatrix[2, 2] * rotMatrix[2, 2])));
double _targetC = Rhino.RhinoMath.ToDegrees(Math.Atan2(rotMatrix[1, 0], rotMatrix[0, 0]));
E6POS pos = new E6POS();
return(pos.SerializeE6POS(_targetX, _targetY, _targetZ, _targetA, _targetB, _targetC));
}
/// <summary>
/// This is the method that actually does the work.
/// </summary>
/// <param name="DA">The DA object is used to retrieve from inputs and store in outputs.</param>
protected override void SolveInstance(IGH_DataAccess DA)
{
// Fields
GH_ObjectWrapper abstractSocket = new GH_ObjectWrapper();
bool triggerRead = false;
int refreshRate = 0;
E6POS currentPos = new E6POS();
E6AXIS currentAngles = new E6AXIS();
bool getSocket = DA.GetData(0, ref abstractSocket);
//Check input
if (_clientSocket == null)
{
if (!getSocket)
{
return;
}
abstractSocket.CastTo(ref _clientSocket);
}
else if (_clientSocket != null && !getSocket)
{
try
{
_clientSocket = null;
return;
}
catch
{
AddRuntimeMessage(GH_RuntimeMessageLevel.Warning, "Waiting For Connection...");
return;
}
}
if (!DA.GetData(1, ref triggerRead))
{
return;
}
if (!DA.GetData(2, ref refreshRate))
{
return;
}
if (refreshRate < 15)
{
AddRuntimeMessage(GH_RuntimeMessageLevel.Warning,
"WARNING: Refresh rate too low, this can cause performance issues for grasshopper. The maximum robot read speed is 5ms (for all messages)");
}
if (refreshRate < 5)
{
AddRuntimeMessage(GH_RuntimeMessageLevel.Error,
"Refresh Rate too low. Absolute maximum speed is 5ms. This is not recommended. Try to stay in the region of ~20-70 ms");
return;
}
//If trigger is pressed, read data and output.
if (triggerRead)
{
string response = Util.ReadVariable(ref _clientSocket, "$POS_ACT", this);
currentPos.DeserializeE6POS(response);
CurrentPos = currentPos;
string response2 = Util.ReadVariable(ref _clientSocket, "$AXIS_ACT", this);
currentAngles.DeserializeE6AXIS(response2);
CurrentAngles = currentAngles;
}
if (!CurrentAngles.IsNull() && !CurrentPos.IsNull())
{
DA.SetDataList(0, CurrentAngles.GetAxisValues());
DA.SetData(1, new GH_Plane(CurrentPos.GetPlane()));
DA.SetDataList(2, new List <double> {
CurrentPos.X, CurrentPos.Y, CurrentPos.Z
});
DA.SetDataList(3, new List <double> {
CurrentPos.A, CurrentPos.B, CurrentPos.C
});
DA.SetData(4, CurrentPos.SerializedString);
DA.SetData(5, CurrentAngles.SerializedString);
}
if (this.Params.Input[1].Sources[0].GetType() == typeof(GH_BooleanToggle) && triggerRead)
{
GH_Document doc = OnPingDocument();
doc?.ScheduleSolution(refreshRate, ScheduleCallback);
}
}