public override byte[] GetBytesForNextAction(RobotCursor cursor) { if (!cursor.ApplyNextAction(out _action)) { return(null); // cursor buffer is empty } _params = null; switch (_action.Type) { case ActionType.Translation: case ActionType.Rotation: case ActionType.Transformation: RotationVector rv = cursor.rotation.AA.ToRotationVector(); _params = new int[] { _action.Id, cursor.motionType == MotionType.Joint ? INST_MOVEJ_P : INST_MOVEL, (int)Math.Round(cursor.position.X * 0.001 * FACTOR_M), (int)Math.Round(cursor.position.Y * 0.001 * FACTOR_M), (int)Math.Round(cursor.position.Z * 0.001 * FACTOR_M), (int)Math.Round(rv.X * Geometry.TO_RADS * FACTOR_RAD), (int)Math.Round(rv.Y * Geometry.TO_RADS * FACTOR_RAD), (int)Math.Round(rv.Z * Geometry.TO_RADS * FACTOR_RAD) }; // Workaround to SW3.0 crash problem: https://github.com/RobotExMachina/Machina.NET/issues/7 if (ZERO_PRECISION_ON_SAME_POSITION_MOTION && cursor.precision != 0 && cursor.prevPosition != null && cursor.position.IsSimilar(cursor.prevPosition)) { Logger.Debug("Applying ZERO_PRECISION_ON_SAME_POSITION_MOTION"); _params = WrapParamsWithZeroPrecision(_params, cursor); } break; case ActionType.Axes: _params = new int[] { _action.Id, INST_MOVEJ_Q, (int)Math.Round(cursor.axes.J1 * Geometry.TO_RADS * FACTOR_RAD), (int)Math.Round(cursor.axes.J2 * Geometry.TO_RADS * FACTOR_RAD), (int)Math.Round(cursor.axes.J3 * Geometry.TO_RADS * FACTOR_RAD), (int)Math.Round(cursor.axes.J4 * Geometry.TO_RADS * FACTOR_RAD), (int)Math.Round(cursor.axes.J5 * Geometry.TO_RADS * FACTOR_RAD), (int)Math.Round(cursor.axes.J6 * Geometry.TO_RADS * FACTOR_RAD), }; // Another ZERO_PRECISION_ON_SAME_POSITION_MOTION check should happen here for 6th axis rotation only... break; case ActionType.Wait: ActionWait aa = _action as ActionWait; _params = new int[] { _action.Id, INST_SLEEP, (int)Math.Round(aa.millis * 0.001 * FACTOR_SEC) }; break; // Not implemented yet //case ActionType.Message: // ActionMessage am = (ActionMessage)action; // dec = string.Format(" popup(\"{0}\", title=\"Machina Message\", warning=False, error=False)", // am.message); // break; case ActionType.AttachTool: //ActionAttachTool aatt = _action as ActionAttachTool; //Tool t = aatt.tool; Tool t = cursor.tool; // can cursor.tool be null? The action would have not gone through if there wasn't a tool available for attachment, right? RotationVector trv = t.TCPOrientation.ToRotationVector(); _params = new int[] { _action.Id, INST_SET_TOOL, (int)Math.Round(t.TCPPosition.X * 0.001 * FACTOR_M), (int)Math.Round(t.TCPPosition.Y * 0.001 * FACTOR_M), (int)Math.Round(t.TCPPosition.Z * 0.001 * FACTOR_M), (int)Math.Round(trv.X * Geometry.TO_RADS * FACTOR_RAD), (int)Math.Round(trv.Y * Geometry.TO_RADS * FACTOR_RAD), (int)Math.Round(trv.Z * Geometry.TO_RADS * FACTOR_RAD), (int)Math.Round(t.Weight * FACTOR_KG) }; break; case ActionType.DetachTool: _params = new int[] { _action.Id, INST_SET_TOOL, 0, 0, 0, 0, 0, 0, 0 }; break; case ActionType.IODigital: ActionIODigital aiod = _action as ActionIODigital; _params = new int[] { _action.Id, INST_SET_DIGITAL_OUT, aiod.pinNum, aiod.on ? 1 : 0, aiod.isToolPin ? 1 : 0 }; break; case ActionType.IOAnalog: ActionIOAnalog aioa = _action as ActionIOAnalog; _params = new int[] { _action.Id, INST_SET_ANALOG_OUT, aioa.pinNum, (int)Math.Round(aioa.value * FACTOR_VOLT) //aioa.isToolPin ? 1 : 0 // there is no analog out on the tool }; break; // ╔═╗╔═╗╔╦╗╔╦╗╦╔╗╔╔═╗╔═╗ // ╚═╗║╣ ║ ║ ║║║║║ ╦╚═╗ // ╚═╝╚═╝ ╩ ╩ ╩╝╚╝╚═╝╚═╝ // Speed is now set globally, and the driver takes care of translating that internally case ActionType.Speed: _params = new int[] { _action.Id, INST_ALL_SPEED, (int)Math.Round(cursor.speed * 0.001 * FACTOR_M) }; break; // Idem for acceleration case ActionType.Acceleration: _params = new int[] { _action.Id, INST_ALL_ACC, (int)Math.Round(cursor.acceleration * 0.001 * FACTOR_M) }; break; case ActionType.Precision: _params = new int[] { _action.Id, INST_BLEND, (int)Math.Round(cursor.precision * 0.001 * FACTOR_M) }; break; case ActionType.PushPop: ActionPushPop app = _action as ActionPushPop; if (app.push) { break; } Settings beforePop = cursor.settingsBuffer.SettingsBeforeLastPop; Dictionary <int, int> poppedSettings = new Dictionary <int, int>(); // These are the states kept in the controller as of v1.0 of the driver //if (beforePop.Speed != cursor.speed) // poppedSettings.Add(INST_TCP_SPEED, (int)Math.Round(cursor.speed * 0.001 * FACTOR_M)); //if (beforePop.Acceleration != cursor.acceleration) // poppedSettings.Add(INST_TCP_ACC, (int)Math.Round(cursor.acceleration * 0.001 * FACTOR_M)); //if (beforePop.JointSpeed != cursor.jointSpeed) // poppedSettings.Add(INST_Q_SPEED, (int)Math.Round(cursor.jointSpeed * Geometry.TO_RADS * FACTOR_RAD)); //if (beforePop.JointAcceleration != cursor.jointAcceleration) // poppedSettings.Add(INST_Q_ACC, (int)Math.Round(cursor.jointAcceleration * Geometry.TO_RADS * FACTOR_RAD)); // These are the states kept in the controller as of v1.0 of the driver if (beforePop.Speed != cursor.speed) { poppedSettings.Add(INST_ALL_SPEED, (int)Math.Round(cursor.speed * 0.001 * FACTOR_M)); } if (beforePop.Acceleration != cursor.acceleration) { poppedSettings.Add(INST_ALL_ACC, (int)Math.Round(cursor.acceleration * 0.001 * FACTOR_M)); } if (beforePop.Precision != cursor.precision) { poppedSettings.Add(INST_BLEND, (int)Math.Round(cursor.precision * 0.001 * FACTOR_M)); } // Generate a buffer with all instructions, ids of -1 except for the last one. _params = new int[3 * poppedSettings.Count]; int it = 0; foreach (var setting in poppedSettings) { _params[3 * it] = it == poppedSettings.Count - 1 ? app.Id : -1; // only attach the real id to the last instruction _params[3 * it + 1] = setting.Key; _params[3 * it + 2] = setting.Value; it++; } break; case ActionType.Coordinates: throw new NotImplementedException(); // @TODO: this should also change the WObj, but not on it yet... //// Send comma-separated integers //case ActionType.CustomCode: // ActionCustomCode acc = _action as ActionCustomCode; // int[] values; // if (Numeric.CommaSeparatedStringToInts(out values)) // { // } // else // { // Logger.Warning("Invalid CustomCode: please use a string of comma-separated integers, like \"1,); // } // break; // If the Action wasn't on the list above, it doesn't have a message representation... default: Logger.Verbose("Cannot stream action `" + _action + "`"); return(null); } if (_params == null) { return(null); } _buffer = Utilities.Conversion.Int32ArrayToByteArray(_params, false); return(_buffer); }
public override byte[] GetBytesForNextAction(RobotCursor cursor) { if (!cursor.ApplyNextAction(out _action)) { return(null); // cursor buffer is empty } _params = null; switch (_action.type) { case ActionType.Translation: case ActionType.Rotation: case ActionType.Transformation: RotationVector rv = cursor.rotation.AA.ToRotationVector(); _params = new int[] { _action.id, cursor.motionType == MotionType.Joint ? INST_MOVEJ_P : INST_MOVEL, (int)Math.Round(cursor.position.X * 0.001 * FACTOR_M), (int)Math.Round(cursor.position.Y * 0.001 * FACTOR_M), (int)Math.Round(cursor.position.Z * 0.001 * FACTOR_M), (int)Math.Round(rv.X * Geometry.TO_RADS * FACTOR_RAD), (int)Math.Round(rv.Y * Geometry.TO_RADS * FACTOR_RAD), (int)Math.Round(rv.Z * Geometry.TO_RADS * FACTOR_RAD) }; break; case ActionType.Axes: _params = new int[] { _action.id, INST_MOVEJ_Q, (int)Math.Round(cursor.joints.J1 * Geometry.TO_RADS * FACTOR_RAD), (int)Math.Round(cursor.joints.J2 * Geometry.TO_RADS * FACTOR_RAD), (int)Math.Round(cursor.joints.J3 * Geometry.TO_RADS * FACTOR_RAD), (int)Math.Round(cursor.joints.J4 * Geometry.TO_RADS * FACTOR_RAD), (int)Math.Round(cursor.joints.J5 * Geometry.TO_RADS * FACTOR_RAD), (int)Math.Round(cursor.joints.J6 * Geometry.TO_RADS * FACTOR_RAD), }; break; case ActionType.Wait: ActionWait aa = _action as ActionWait; _params = new int[] { _action.id, INST_SLEEP, (int)Math.Round(aa.millis * 0.001 * FACTOR_SEC) }; break; // Not implemented yet //case ActionType.Message: // ActionMessage am = (ActionMessage)action; // dec = string.Format(" popup(\"{0}\", title=\"Machina Message\", warning=False, error=False)", // am.message); // break; case ActionType.Attach: ActionAttach aatt = _action as ActionAttach; Tool t = aatt.tool; RotationVector trv = t.TCPOrientation.ToRotationVector(); _params = new int[] { _action.id, INST_SET_TOOL, (int)Math.Round(t.TCPPosition.X * 0.001 * FACTOR_M), (int)Math.Round(t.TCPPosition.Y * 0.001 * FACTOR_M), (int)Math.Round(t.TCPPosition.Z * 0.001 * FACTOR_M), (int)Math.Round(trv.X * Geometry.TO_RADS * FACTOR_RAD), (int)Math.Round(trv.Y * Geometry.TO_RADS * FACTOR_RAD), (int)Math.Round(trv.Z * Geometry.TO_RADS * FACTOR_RAD), (int)Math.Round(t.Weight * FACTOR_M) }; break; case ActionType.Detach: _params = new int[] { _action.id, INST_SET_TOOL, 0, 0, 0, 0, 0, 0, 0 }; break; case ActionType.IODigital: ActionIODigital aiod = _action as ActionIODigital; _params = new int[] { _action.id, INST_SET_DIGITAL_OUT, aiod.pinNum, aiod.on ? 1 : 0 }; break; case ActionType.IOAnalog: ActionIOAnalog aioa = _action as ActionIOAnalog; _params = new int[] { _action.id, INST_SET_DIGITAL_OUT, aioa.pinNum, (int)Math.Round(aioa.value * FACTOR_VOLT) }; break; // ╔═╗╔═╗╔╦╗╔╦╗╦╔╗╔╔═╗╔═╗ // ╚═╗║╣ ║ ║ ║║║║║ ╦╚═╗ // ╚═╝╚═╝ ╩ ╩ ╩╝╚╝╚═╝╚═╝ case ActionType.Speed: _params = new int[] { _action.id, INST_TCP_SPEED, (int)Math.Round(cursor.speed * 0.001 * FACTOR_M) }; break; case ActionType.Acceleration: _params = new int[] { _action.id, INST_TCP_ACC, (int)Math.Round(cursor.acceleration * 0.001 * FACTOR_M) }; break; case ActionType.JointSpeed: _params = new int[] { _action.id, INST_Q_SPEED, (int)Math.Round(cursor.jointSpeed * Geometry.TO_RADS * FACTOR_RAD) }; break; case ActionType.JointAcceleration: _params = new int[] { _action.id, INST_Q_ACC, (int)Math.Round(cursor.jointAcceleration * Geometry.TO_RADS * FACTOR_RAD) }; break; case ActionType.Precision: _params = new int[] { _action.id, INST_BLEND, (int)Math.Round(cursor.precision * 0.001 * FACTOR_M) }; break; case ActionType.PushPop: ActionPushPop app = _action as ActionPushPop; if (app.push) { break; } Settings beforePop = cursor.settingsBuffer.SettingsBeforeLastPop; Dictionary <int, int> poppedSettings = new Dictionary <int, int>(); // These are the states kept in the controller as of v1.0 of the driver if (beforePop.Speed != cursor.speed) { poppedSettings.Add(INST_TCP_SPEED, (int)Math.Round(cursor.speed * 0.001 * FACTOR_M)); } if (beforePop.Acceleration != cursor.acceleration) { poppedSettings.Add(INST_TCP_ACC, (int)Math.Round(cursor.acceleration * 0.001 * FACTOR_M)); } if (beforePop.JointSpeed != cursor.jointSpeed) { poppedSettings.Add(INST_Q_SPEED, (int)Math.Round(cursor.jointSpeed * Geometry.TO_RADS * FACTOR_RAD)); } if (beforePop.JointAcceleration != cursor.jointAcceleration) { poppedSettings.Add(INST_Q_ACC, (int)Math.Round(cursor.jointAcceleration * Geometry.TO_RADS * FACTOR_RAD)); } if (beforePop.Precision != cursor.precision) { poppedSettings.Add(INST_BLEND, (int)Math.Round(cursor.precision * 0.001 * FACTOR_M)); } // Generate a buffer with all instructions, ids of -1 except for the last one. _params = new int[3 * poppedSettings.Count]; int it = 0; foreach (var setting in poppedSettings) { _params[3 * it] = it == poppedSettings.Count - 1 ? app.id : -1; // only attach the real id to the last instruction _params[3 * it + 1] = setting.Key; _params[3 * it + 2] = setting.Value; it++; } break; case ActionType.Coordinates: throw new NotImplementedException(); // @TODO: this should also change the WObj, but not on it yet... } if (_params == null) { return(null); } _buffer = Util.Int32ArrayToByteArray(_params, false); return(_buffer); }
internal const char STR_MESSAGE_RESPONSE_CHAR = '$'; // Flags a message as a response to an information request(acknowledgments do not include it) /// <summary> /// Given an Action and a RobotCursor representing the state of the robot after application, /// return a List of strings with the messages necessary to perform this Action adhering to /// the Machina-ABB-Server protocol. /// </summary> /// <param name="action"></param> /// <param name="cursor"></param> /// <returns></returns> internal override List <string> GetActionMessages(Action action, RobotCursor cursor) { List <string> msgs = new List <string>(); switch (action.Type) { case ActionType.Translation: case ActionType.Rotation: case ActionType.Transformation: //// MoveL/J X Y Z QW QX QY QZ msgs.Add(string.Format(CultureInfo.InvariantCulture, "{0}{1} {2} {3} {4} {5} {6} {7} {8} {9}{10}", STR_MESSAGE_ID_CHAR, action.Id, cursor.motionType == MotionType.Linear ? INST_MOVEL : INST_MOVEJ, Math.Round(cursor.position.X, Geometry.STRING_ROUND_DECIMALS_MM), Math.Round(cursor.position.Y, Geometry.STRING_ROUND_DECIMALS_MM), Math.Round(cursor.position.Z, Geometry.STRING_ROUND_DECIMALS_MM), Math.Round(cursor.rotation.Q.W, Geometry.STRING_ROUND_DECIMALS_QUAT), Math.Round(cursor.rotation.Q.X, Geometry.STRING_ROUND_DECIMALS_QUAT), Math.Round(cursor.rotation.Q.Y, Geometry.STRING_ROUND_DECIMALS_QUAT), Math.Round(cursor.rotation.Q.Z, Geometry.STRING_ROUND_DECIMALS_QUAT), STR_MESSAGE_END_CHAR)); break; case ActionType.Axes: // MoveAbsJ J1 J2 J3 J4 J5 J6 msgs.Add(string.Format(CultureInfo.InvariantCulture, "{0}{1} {2} {3} {4} {5} {6} {7} {8}{9}", STR_MESSAGE_ID_CHAR, action.Id, INST_MOVEABSJ, Math.Round(cursor.axes.J1, Geometry.STRING_ROUND_DECIMALS_DEGS), Math.Round(cursor.axes.J2, Geometry.STRING_ROUND_DECIMALS_DEGS), Math.Round(cursor.axes.J3, Geometry.STRING_ROUND_DECIMALS_DEGS), Math.Round(cursor.axes.J4, Geometry.STRING_ROUND_DECIMALS_DEGS), Math.Round(cursor.axes.J5, Geometry.STRING_ROUND_DECIMALS_DEGS), Math.Round(cursor.axes.J6, Geometry.STRING_ROUND_DECIMALS_DEGS), STR_MESSAGE_END_CHAR)); break; case ActionType.Speed: // (setspeed V_TCP[V_ORI V_LEAX V_REAX]) msgs.Add(Invariant($"{STR_MESSAGE_ID_CHAR}{action.Id} {INST_SPEED} {cursor.speed}{STR_MESSAGE_END_CHAR}")); // this accepts more velocity params, but those are still not implemented in Machina... break; case ActionType.Acceleration: Logger.Debug("Acceleration not implemented in ABBCommunicationProtocol"); break; case ActionType.Precision: // (setzone FINE TCP[ORI EAX ORI LEAX REAX]) msgs.Add($"{STR_MESSAGE_ID_CHAR}{action.Id} {INST_ZONE} {cursor.precision}{STR_MESSAGE_END_CHAR}"); // this accepts more zone params, but those are still not implemented in Machina... break; case ActionType.Wait: // !WaitTime T ActionWait aw = (ActionWait)action; msgs.Add($"{STR_MESSAGE_ID_CHAR}{action.Id} {INST_WAITTIME} {0.001 * aw.millis}{STR_MESSAGE_END_CHAR}"); break; case ActionType.Message: // !TPWrite "MSG" ActionMessage am = (ActionMessage)action; msgs.Add($"{STR_MESSAGE_ID_CHAR}{action.Id} {INST_TPWRITE} \"{am.message}\"{STR_MESSAGE_END_CHAR}"); break; case ActionType.AttachTool: // !(settool X Y Z QW QX QY QZ KG CX CY CZ) //ActionAttachTool aa = (ActionAttachTool)action; //Tool t = aa.tool; Tool t = cursor.tool; // @TODO: should I just pull from the library? need to rethink the general approach: take info from cursor state (like motion actions) or action data... msgs.Add(string.Format("{0}{1} {2} {3} {4} {5} {6} {7} {8} {9} {10} {11} {12} {13}{14}", STR_MESSAGE_ID_CHAR, action.Id, INST_TOOL, Math.Round(t.TCPPosition.X, Geometry.STRING_ROUND_DECIMALS_MM), Math.Round(t.TCPPosition.Y, Geometry.STRING_ROUND_DECIMALS_MM), Math.Round(t.TCPPosition.Z, Geometry.STRING_ROUND_DECIMALS_MM), Math.Round(t.TCPOrientation.Q.W, Geometry.STRING_ROUND_DECIMALS_QUAT), Math.Round(t.TCPOrientation.Q.X, Geometry.STRING_ROUND_DECIMALS_QUAT), Math.Round(t.TCPOrientation.Q.Y, Geometry.STRING_ROUND_DECIMALS_QUAT), Math.Round(t.TCPOrientation.Q.Z, Geometry.STRING_ROUND_DECIMALS_QUAT), Math.Round(t.Weight, Geometry.STRING_ROUND_DECIMALS_KG), Math.Round(t.CenterOfGravity.X, Geometry.STRING_ROUND_DECIMALS_MM), Math.Round(t.CenterOfGravity.Y, Geometry.STRING_ROUND_DECIMALS_MM), Math.Round(t.CenterOfGravity.Z, Geometry.STRING_ROUND_DECIMALS_MM), STR_MESSAGE_END_CHAR)); break; case ActionType.DetachTool: // !(settool0) msgs.Add($"{STR_MESSAGE_ID_CHAR}{action.Id} {INST_NOTOOL}{STR_MESSAGE_END_CHAR}"); break; case ActionType.IODigital: // !SetDO "NAME" ON ActionIODigital aiod = (ActionIODigital)action; msgs.Add($"{STR_MESSAGE_ID_CHAR}{action.Id} {INST_SETDO} \"{aiod.pinName}\" {(aiod.on ? 1 : 0)}{STR_MESSAGE_END_CHAR}"); break; case ActionType.IOAnalog: // !SetAO "NAME" V ActionIOAnalog aioa = (ActionIOAnalog)action; msgs.Add($"{STR_MESSAGE_ID_CHAR}{action.Id} {INST_SETAO} \"{aioa.pinName}\" {aioa.value}{STR_MESSAGE_END_CHAR}"); break; case ActionType.PushPop: ActionPushPop app = action as ActionPushPop; if (app.push) { return(null); } else { // Only precision, speed and acceleration are states kept on the controller Settings beforePop = cursor.settingsBuffer.SettingsBeforeLastPop; if (beforePop.Speed != cursor.speed) { msgs.Add($"{STR_MESSAGE_ID_CHAR}{action.Id} {INST_SPEED} {cursor.speed}{STR_MESSAGE_END_CHAR}"); } if (beforePop.Acceleration != cursor.acceleration) { msgs.Add($"{STR_MESSAGE_ID_CHAR}{action.Id} {INST_ACCELERATION} {cursor.acceleration}{STR_MESSAGE_END_CHAR}"); } if (beforePop.Precision != cursor.precision) { msgs.Add($"{STR_MESSAGE_ID_CHAR}{action.Id} {INST_ZONE} {cursor.precision}{STR_MESSAGE_END_CHAR}"); } } break; case ActionType.Coordinates: throw new NotImplementedException(); // @TODO: this should also change the WObj, but not on it yet... case ActionType.ExternalAxis: string msg, @params, id; ActionExternalAxis aea = action as ActionExternalAxis; // Cartesian msg if (aea.target == ExternalAxesTarget.All || aea.target == ExternalAxesTarget.Cartesian) { if (aea.target == ExternalAxesTarget.All) { id = "0"; // If will need to send two messages, only add real id to the last one. } else { id = aea.Id.ToString(); } @params = ExternalAxesToParameters(cursor.externalAxesCartesian); msg = $"{STR_MESSAGE_ID_CHAR}{id} {INST_EXT_JOINTS_ROBTARGET} {@params}{STR_MESSAGE_END_CHAR}"; msgs.Add(msg); } // Joints msg if (aea.target == ExternalAxesTarget.All || aea.target == ExternalAxesTarget.Joint) { id = aea.Id.ToString(); // now use the real id in any case @params = ExternalAxesToParameters(cursor.externalAxesJoints); msg = $"{STR_MESSAGE_ID_CHAR}{id} {INST_EXT_JOINTS_JOINTTARGET} {@params}{STR_MESSAGE_END_CHAR}"; msgs.Add(msg); } break; case ActionType.ArmAngle: // Send a request to change only the robtarget portion of the external axes for next motion. ActionArmAngle aaa = action as ActionArmAngle; msgs.Add(string.Format("{0}{1} {2} {3} 9E9 9E9 9E9 9E9 9E9{4}", STR_MESSAGE_ID_CHAR, action.Id, INST_EXT_JOINTS_ROBTARGET, Math.Round(aaa.angle, Geometry.STRING_ROUND_DECIMALS_DEGS), STR_MESSAGE_END_CHAR)); break; // When connected live, this is used as a way to stream a message directly to the robot. Unsafe, but oh well... case ActionType.CustomCode: ActionCustomCode acc = action as ActionCustomCode; msgs.Add(acc.statement); break; // If the Action wasn't on the list above, it doesn't have a message representation... default: Logger.Warning("Cannot stream action `" + action + "`"); return(null); } return(msgs); }
internal const int RES_JOINTS = 22; // ">22 0 0 0 0 90 0;" // const int RES_EXTAX = 23; /// <summary> /// Given an Action and a RobotCursor representing the state of the robot after application, /// return a List of strings with the messages necessary to perform this Action adhering to /// the Machina-ABB-Server protocol. /// </summary> /// <param name="action"></param> /// <param name="cursor"></param> /// <returns></returns> internal override List <string> GetActionMessages(Action action, RobotCursor cursor) { List <string> msgs = new List <string>(); switch (action.type) { case ActionType.Translation: case ActionType.Rotation: case ActionType.Transformation: //// MoveL/J X Y Z QW QX QY QZ msgs.Add(string.Format("{0}{1} {2} {3} {4} {5} {6} {7} {8} {9}{10}", STR_MESSAGE_ID_CHAR, action.id, cursor.motionType == MotionType.Linear ? INST_MOVEL : INST_MOVEJ, Math.Round(cursor.position.X, Geometry.STRING_ROUND_DECIMALS_MM), Math.Round(cursor.position.Y, Geometry.STRING_ROUND_DECIMALS_MM), Math.Round(cursor.position.Z, Geometry.STRING_ROUND_DECIMALS_MM), Math.Round(cursor.rotation.Q.W, Geometry.STRING_ROUND_DECIMALS_QUAT), Math.Round(cursor.rotation.Q.X, Geometry.STRING_ROUND_DECIMALS_QUAT), Math.Round(cursor.rotation.Q.Y, Geometry.STRING_ROUND_DECIMALS_QUAT), Math.Round(cursor.rotation.Q.Z, Geometry.STRING_ROUND_DECIMALS_QUAT), STR_MESSAGE_END_CHAR)); break; case ActionType.Axes: // MoveAbsJ J1 J2 J3 J4 J5 J6 msgs.Add(string.Format("{0}{1} {2} {3} {4} {5} {6} {7} {8}{9}", STR_MESSAGE_ID_CHAR, action.id, INST_MOVEABSJ, Math.Round(cursor.joints.J1, Geometry.STRING_ROUND_DECIMALS_DEGS), Math.Round(cursor.joints.J2, Geometry.STRING_ROUND_DECIMALS_DEGS), Math.Round(cursor.joints.J3, Geometry.STRING_ROUND_DECIMALS_DEGS), Math.Round(cursor.joints.J4, Geometry.STRING_ROUND_DECIMALS_DEGS), Math.Round(cursor.joints.J5, Geometry.STRING_ROUND_DECIMALS_DEGS), Math.Round(cursor.joints.J6, Geometry.STRING_ROUND_DECIMALS_DEGS), STR_MESSAGE_END_CHAR)); break; case ActionType.Speed: // (setspeed V_TCP[V_ORI V_LEAX V_REAX]) msgs.Add($"{STR_MESSAGE_ID_CHAR}{action.id} {INST_SPEED} {cursor.speed}{STR_MESSAGE_END_CHAR}"); // this accepts more velocity params, but those are still not implemented in Machina... break; case ActionType.Precision: // (setzone FINE TCP[ORI EAX ORI LEAX REAX]) msgs.Add($"{STR_MESSAGE_ID_CHAR}{action.id} {INST_ZONE} {cursor.precision}{STR_MESSAGE_END_CHAR}"); // this accepts more zone params, but those are still not implemented in Machina... break; case ActionType.Wait: // !WaitTime T ActionWait aw = (ActionWait)action; msgs.Add($"{STR_MESSAGE_ID_CHAR}{action.id} {INST_WAITTIME} {0.001 * aw.millis}{STR_MESSAGE_END_CHAR}"); break; case ActionType.Message: // !TPWrite "MSG" ActionMessage am = (ActionMessage)action; msgs.Add($"{STR_MESSAGE_ID_CHAR}{action.id} {INST_TPWRITE} \"{am.message}\"{STR_MESSAGE_END_CHAR}"); break; case ActionType.Attach: // !(settool X Y Z QW QX QY QZ KG CX CY CZ) ActionAttach aa = (ActionAttach)action; Tool t = aa.tool; msgs.Add(string.Format("{0}{1} {2} {3} {4} {5} {6} {7} {8} {9} {10} {11} {12} {13}{14}", STR_MESSAGE_ID_CHAR, action.id, INST_TOOL, Math.Round(t.TCPPosition.X, Geometry.STRING_ROUND_DECIMALS_MM), Math.Round(t.TCPPosition.Y, Geometry.STRING_ROUND_DECIMALS_MM), Math.Round(t.TCPPosition.Z, Geometry.STRING_ROUND_DECIMALS_MM), Math.Round(t.TCPOrientation.Q.W, Geometry.STRING_ROUND_DECIMALS_QUAT), Math.Round(t.TCPOrientation.Q.X, Geometry.STRING_ROUND_DECIMALS_QUAT), Math.Round(t.TCPOrientation.Q.Y, Geometry.STRING_ROUND_DECIMALS_QUAT), Math.Round(t.TCPOrientation.Q.Z, Geometry.STRING_ROUND_DECIMALS_QUAT), Math.Round(t.Weight, Geometry.STRING_ROUND_DECIMALS_KG), Math.Round(t.centerOfGravity.X, Geometry.STRING_ROUND_DECIMALS_MM), Math.Round(t.centerOfGravity.Y, Geometry.STRING_ROUND_DECIMALS_MM), Math.Round(t.centerOfGravity.Z, Geometry.STRING_ROUND_DECIMALS_MM), STR_MESSAGE_END_CHAR)); break; case ActionType.Detach: // !(settool0) msgs.Add($"{STR_MESSAGE_ID_CHAR}{action.id} {INST_NOTOOL}{STR_MESSAGE_END_CHAR}"); break; case ActionType.IODigital: // !SetDO "NAME" ON ActionIODigital aiod = (ActionIODigital)action; msgs.Add($"{STR_MESSAGE_ID_CHAR}{action.id} {INST_SETDO} \"{aiod.pinName}\" {(aiod.on ? 1 : 0)}{STR_MESSAGE_END_CHAR}"); break; case ActionType.IOAnalog: // !SetAO "NAME" V ActionIOAnalog aioa = (ActionIOAnalog)action; msgs.Add($"{STR_MESSAGE_ID_CHAR}{action.id} {INST_SETAO} \"{aioa.pinName}\" {aioa.value}{STR_MESSAGE_END_CHAR}"); break; case ActionType.PushPop: ActionPushPop app = action as ActionPushPop; if (app.push) { return(null); } else { // Only precision and speed are states kept on the controller Settings beforePop = cursor.settingsBuffer.SettingsBeforeLastPop; if (beforePop.Speed != cursor.speed) { msgs.Add($"{STR_MESSAGE_ID_CHAR}{action.id} {INST_SPEED} {cursor.speed}{STR_MESSAGE_END_CHAR}"); } if (beforePop.Precision != cursor.precision) { msgs.Add($"{STR_MESSAGE_ID_CHAR}{action.id} {INST_ZONE} {cursor.precision}{STR_MESSAGE_END_CHAR}"); } } break; case ActionType.Coordinates: throw new NotImplementedException(); // @TODO: this should also change the WObj, but not on it yet... case ActionType.ExternalAxis: string msg = $"{STR_MESSAGE_ID_CHAR}{action.id} {INST_EXT_JOINTS} "; for (int i = 0; i < cursor.externalAxes.Length; i++) { // RAPID's StrToVal() will parse 9E9 into a 9E+9 num value, and ignore that axis on motions msg += cursor.externalAxes[i]?.ToString() ?? "9E9"; if (i < cursor.externalAxes.Length - 1) { msg += " "; } } msg += STR_MESSAGE_END_CHAR; msgs.Add(msg); break; // CustomCode --> is non-streamable // If the Action wasn't on the list above, it doesn't have a message representation... default: return(null); } return(msgs); }