Ejemplo n.º 1
0
        /// <summary>
        /// Apply ActionIOAnalog write action to this cursor.
        /// </summary>
        /// <param name="action"></param>
        /// <returns></returns>
        public bool ApplyAction(ActionIOAnalog action)
        {
            if (action.pin < 0 || action.pin >= analogOutputs.Length)
            {
                Console.WriteLine("Cannot write to analog IO: robot has no analog pin #" + action.pin);
                return(false);
            }

            this.analogOutputs[action.pin] = action.value;
            return(true);
        }
Ejemplo n.º 2
0
        /// <summary>
        /// Apply ActionIOAnalog write action to this cursor.
        /// </summary>
        /// <param name="action"></param>
        /// <returns></returns>
        public bool ApplyAction(ActionIOAnalog action)
        {
            //if (action.pinName < 0 || action.pinName >= analogOutputs.Length)
            //{
            //    Console.WriteLine("Cannot write to analog IO: robot has no analog pin #" + action.pinName);
            //    return false;
            //}

            //this.analogOutputs[action.pinName] = action.value;
            //return true;

            if (analogOutputs.ContainsKey(action.pinName))
            {
                analogOutputs[action.pinName] = action.value;
            }
            else
            {
                analogOutputs.Add(action.pinName, action.value);
            }

            return(true);
        }
Ejemplo n.º 3
0
        internal bool GenerateInstructionDeclaration(
            Action action,
            RobotCursor cursor,
            Dictionary <double, string> velNames,
            Dictionary <double, string> zoneNames,
            Dictionary <Tool, string> toolNames,
            out string declaration)
        {
            string dec = null;

            switch (action.Type)
            {
            case ActionType.Acceleration:
                bool zero = cursor.acceleration < Geometry.EPSILON;
                dec = string.Format("    WorldAccLim {0};",
                                    zero ? "\\Off" : "\\On := " + Math.Round(0.001 * cursor.acceleration, Geometry.STRING_ROUND_DECIMALS_M)).ToString(CultureInfo.InvariantCulture);
                break;

            //case ActionType.JointSpeed:
            //case ActionType.JointAcceleration:
            //    dec = string.Format("    {0} WARNING: {1}() has no effect in ABB robots.",
            //        commChar,
            //        action.Type);
            //    break;

            // @TODO: push/pop management should be done PROGRAMMATICALLY, not this CHAPUZa...
            case ActionType.PushPop:
                // Find if there was a change in acceleration, and set the corresponsing instruction...
                ActionPushPop app = action as ActionPushPop;
                if (app.push)
                {
                    break;                // only necessary for pops
                }
                if (Math.Abs(cursor.acceleration - cursor.settingsBuffer.SettingsBeforeLastPop.Acceleration) < Geometry.EPSILON)
                {
                    break;                                                                                                                   // no change
                }
                // If here, there was a change, so...
                bool zeroAcc = cursor.acceleration < Geometry.EPSILON;
                dec = string.Format("    WorldAccLim {0};",
                                    zeroAcc
                                ? "\\Off"
                                : "\\On := " + Math.Round(0.001 * cursor.acceleration,
                                                          Geometry.STRING_ROUND_DECIMALS_M)).ToString(CultureInfo.InvariantCulture);
                break;

            case ActionType.Translation:
            case ActionType.Rotation:
            case ActionType.Transformation:
                dec = string.Format("    {0} {1}, {2}, {3}, {4}\\{5};",
                                    cursor.motionType == MotionType.Joint ? "MoveJ" : "MoveL",
                                    GetRobTargetValue(cursor),
                                    velNames[cursor.speed],
                                    zoneNames[cursor.precision],
                                    cursor.tool == null ? "Tool0" : toolNames[cursor.tool],
                                    "WObj:=WObj0");
                break;

            case ActionType.Axes:
                dec = string.Format("    MoveAbsJ {0}, {1}, {2}, {3}\\{4};",
                                    GetJointTargetValue(cursor),
                                    velNames[cursor.speed],
                                    zoneNames[cursor.precision],
                                    cursor.tool == null ? "Tool0" : toolNames[cursor.tool],
                                    "WObj:=WObj0");
                break;

            case ActionType.Message:
                ActionMessage am = (ActionMessage)action;
                dec = string.Format("    TPWrite \"{0}\";",
                                    am.message.Length <= 80 ?
                                    am.message :
                                    am.message.Substring(0, 80)); // ABB strings can only be 80 chars long
                break;

            case ActionType.Wait:
                ActionWait aw = (ActionWait)action;
                dec = string.Format(CultureInfo.InvariantCulture,
                                    "    WaitTime {0};",
                                    0.001 * aw.millis);
                break;

            case ActionType.Comment:
                ActionComment ac = (ActionComment)action;
                dec = string.Format("    {0} {1}",
                                    CC,
                                    ac.comment);
                break;

            case ActionType.DefineTool:
                ActionDefineTool adt = action as ActionDefineTool;
                dec = string.Format("    {0} Tool \"{1}\" defined",      // this action has no actual RAPID instruction, just add a comment
                                    CC,
                                    adt.tool.name);
                break;

            case ActionType.AttachTool:
                ActionAttachTool aa = (ActionAttachTool)action;
                dec = string.Format("    {0} Tool \"{1}\" attached",      // this action has no actual RAPID instruction, just add a comment
                                    CC,
                                    aa.toolName);
                break;

            case ActionType.DetachTool:
                ActionDetachTool ad = (ActionDetachTool)action;
                dec = string.Format("    {0} All tools detached",      // this action has no actual RAPID instruction, just add a comment
                                    CC);
                break;

            case ActionType.IODigital:
                ActionIODigital aiod = (ActionIODigital)action;
                dec = $"    SetDO {aiod.pinName}, {(aiod.on ? "1" : "0")};";
                break;

            case ActionType.IOAnalog:
                ActionIOAnalog aioa = (ActionIOAnalog)action;
                dec = $"    SetAO {aioa.pinName}, {aioa.value};";
                break;

            case ActionType.CustomCode:
                ActionCustomCode acc = action as ActionCustomCode;
                if (!acc.isDeclaration)
                {
                    dec = "    " + acc.statement;
                }
                break;

                //default:
                //    dec = string.Format("    ! ACTION \"{0}\" NOT IMPLEMENTED", action);
                //    break;
            }

            if (addActionString && action.Type != ActionType.Comment)
            {
                dec = string.Format("{0}{1}  {2} [{3}]",
                                    dec,
                                    dec == null ? "  " : "", // add indentation to align with code
                                    CC,
                                    action.ToString());
            }
            else if (addActionID)
            {
                dec = string.Format("{0}{1}  {2} [{3}]",
                                    dec,
                                    dec == null ? "  " : "", // add indentation to align with code
                                    CC,
                                    action.Id);
            }

            declaration = dec;
            return(dec != null);
        }
Ejemplo n.º 4
0
        //  ╦ ╦╔╦╗╦╦  ╔═╗
        //  ║ ║ ║ ║║  ╚═╗
        //  ╚═╝ ╩ ╩╩═╝╚═╝
        internal bool GenerateInstructionDeclaration(
            Action action, RobotCursor cursor,
            out string declaration)
        {
            string dec = null;

            switch (action.Type)
            {
            case ActionType.Speed:
                dec = string.Format(CultureInfo.InvariantCulture,
                                    "G1 F{0}",
                                    Math.Round(60.0 * cursor.speed, Geometry.STRING_ROUND_DECIMALS_MM));
                break;

            case ActionType.Translation:
            case ActionType.Transformation:
                dec = string.Format("G1 {0}{1}",
                                    GetPositionTargetValue(cursor),
                                    cursor.isExtruding ? " " + GetExtrusionTargetValue(cursor) : "");
                break;

            // Only available in MakerBot? http://reprap.org/wiki/G-code#M70:_Display_message
            case ActionType.Message:
                ActionMessage am = (ActionMessage)action;
                dec = string.Format("M70 P1000 ({0})",
                                    am.message);
                break;

            // In GCode, this is called "Dwell"
            case ActionType.Wait:
                ActionWait aw = (ActionWait)action;
                dec = string.Format(CultureInfo.InvariantCulture,
                                    "G4 P{0}",
                                    aw.millis);
                break;

            case ActionType.Comment:
                ActionComment ac = (ActionComment)action;
                dec = string.Format("{0} {1}",
                                    commChar,
                                    ac.comment);
                break;

            // Untested, but oh well...
            // http://reprap.org/wiki/G-code#M42:_Switch_I.2FO_pin
            case ActionType.IODigital:
                ActionIODigital aiod = (ActionIODigital)action;
                if (!aiod.isDigit)
                {
                    dec = $"{commChar} ERROR on \"{aiod}\": only integer pin names allowed";
                }
                else
                {
                    dec = $"M42 P{aiod.pinNum} S{(aiod.on ? "1" : "0")}";
                }
                break;


            case ActionType.IOAnalog:
                ActionIOAnalog aioa = (ActionIOAnalog)action;
                if (!aioa.isDigit)
                {
                    dec = $"{commChar} ERROR on \"{aioa}\": only integer pin names allowed";
                }
                else if (aioa.value < 0 || aioa.value > 255)
                {
                    dec = $"{commChar} ERROR on \"{aioa.ToString()}\": value out of range [0..255]";
                }
                else
                {
                    //dec = $"M42 P{aioa.pinNum} S{Math.Round(aioa.value, 0)}";
                    dec = string.Format(CultureInfo.InvariantCulture,
                                        "M42 P{0} S{1}",
                                        aioa.pinNum,
                                        Math.Round(aioa.value, 0));
                }
                break;

            case ActionType.Temperature:
                ActionTemperature at = (ActionTemperature)action;
                //dec = $"{tempToGCode[new Tuple<RobotPartType, bool>(at.robotPart, at.wait)]} S{Math.Round(cursor.partTemperature[at.robotPart], Geometry.STRING_ROUND_DECIMALS_TEMPERATURE)}";
                dec = string.Format(CultureInfo.InvariantCulture,
                                    "{0} S{1}",
                                    tempToGCode[new Tuple <RobotPartType, bool>(at.robotPart, at.wait)],
                                    Math.Round(cursor.partTemperature[at.robotPart], Geometry.STRING_ROUND_DECIMALS_TEMPERATURE));
                break;

            case ActionType.Extrusion:
            case ActionType.ExtrusionRate:
                dec = $"{commChar} {action.ToString()}";      // has no direct G-code, simply annotate it as a comment
                break;

            case ActionType.Initialization:
                ActionInitialization ai = (ActionInitialization)action;
                if (ai.initialize == true)
                {
                    StartCodeBoilerplate(cursor);
                }
                else
                {
                    EndCodeBoilerplate(cursor);
                }

                break;

            case ActionType.CustomCode:
                ActionCustomCode acc = action as ActionCustomCode;
                if (!acc.isDeclaration)
                {
                    dec = $"{acc.statement}";
                }
                break;

            // If action wasn't implemented before, then it doesn't apply to this device
            default:
                dec = $"{commChar} ACTION \"{action}\" NOT APPLICABLE TO THIS DEVICE";
                break;

                //case ActionType.Rotation:
                //case ActionType.Zone:
                //case ActionType.Joints:
                //case ActionType.Attach:
                //case ActionType.Detach:
                //    dec = string.Format("; ACTION \"{0}\" NOT IMPLEMENTED IN THIS DEVICE", action);
                //    break;
            }

            // Add trailing comments or ids if speficied
            if (ADD_ACTION_STRING && action.Type != ActionType.Comment)
            {
                dec = string.Format("{0}  {1} [{2}]",
                                    dec,
                                    commChar,
                                    action.ToString());
            }
            else if (ADD_ACTION_ID)
            {
                dec = string.Format("{0}  {1} [{2}]",
                                    dec,
                                    commChar,
                                    action.Id);
            }

            declaration = dec;
            return(dec != null);
        }
Ejemplo n.º 5
0
        internal bool GenerateInstructionDeclaration(
            Action action, RobotCursor cursor,
            out string declaration)
        {
            string dec = null;

            switch (action.type)
            {
            // KUKA does explicit setting of velocities and approximate positioning, so these actions make sense as instructions
            case ActionType.Speed:
                dec = string.Format("  $VEL = {{CP {0}, ORI1 100, ORI2 100}}",
                                    Math.Round(0.001 * cursor.speed, 3 + Geometry.STRING_ROUND_DECIMALS_MM));
                break;

            case ActionType.Precision:
                dec = string.Format("  $APO.CDIS = {0}",
                                    cursor.precision);
                break;

            case ActionType.Translation:
            case ActionType.Rotation:
            case ActionType.Transformation:
                dec = string.Format("  {0} {1} {2}",
                                    cursor.motionType == MotionType.Joint ? "PTP" : "LIN",
                                    GetPositionTargetValue(cursor),
                                    cursor.precision >= 1 ? "C_DIS" : "");
                break;

            case ActionType.Axes:
                dec = string.Format("  {0} {1} {2}",
                                    "PTP",
                                    GetAxisTargetValue(cursor),
                                    cursor.precision >= 1 ? "C_DIS" : ""); // @TODO: figure out how to turn this into C_PTP
                break;

            // @TODO: apparently, messages in KRL are kind fo tricky, with several manuals just dedicated to it.
            // Will figure this out later.
            case ActionType.Message:
                ActionMessage am = (ActionMessage)action;
                dec = string.Format("  {0} MESSAGE: \"{1}\" (messages in KRL currently not supported in Machina)",
                                    commChar,
                                    am.message);
                break;

            case ActionType.Wait:
                ActionWait aw = (ActionWait)action;
                dec = string.Format("  WAIT SEC {0}",
                                    0.001 * aw.millis);
                break;

            case ActionType.Comment:
                ActionComment ac = (ActionComment)action;
                dec = string.Format("  {0} {1}",
                                    commChar,
                                    ac.comment);
                break;


            case ActionType.Attach:
                ActionAttach at = (ActionAttach)action;
                dec = string.Format("  $TOOL = {0}",
                                    GetToolValue(cursor));
                break;

            case ActionType.Detach:
                ActionDetach ad = (ActionDetach)action;
                dec = string.Format("  $TOOL = $NULLFRAME");
                break;

            case ActionType.IODigital:
                ActionIODigital aiod = (ActionIODigital)action;
                if (!aiod.isDigit)
                {
                    dec = $"  {commChar} ERROR on \"{aiod}\": only integer pin names are possible";
                }
                else if (aiod.pinNum < 1 || aiod.pinNum > 32)      // KUKA starts counting pins by 1
                {
                    dec = $"  {commChar} ERROR on \"{aiod}\": IO number not available";
                }
                else
                {
                    dec = $"  $OUT[{aiod.pinNum}] = {(aiod.on ? "TRUE" : "FALSE")}";
                }
                break;

            case ActionType.IOAnalog:
                ActionIOAnalog aioa = (ActionIOAnalog)action;
                if (!aioa.isDigit)
                {
                    dec = $"  {commChar} ERROR on \"{aioa}\": only integer pin names are possible";
                }
                else if (aioa.pinNum < 1 || aioa.pinNum > 16)        // KUKA: analog pins [1 to 16]
                {
                    dec = $"  {commChar} ERROR on \"{aioa}\": IO number not available";
                }
                else if (aioa.value < -1 || aioa.value > 1)
                {
                    dec = $"  {commChar} ERROR on \"{aioa}\": value out of range [-1.0, 1.0]";
                }
                else
                {
                    dec = $"  $ANOUT[{aioa.pinNum}] = {Math.Round(aioa.value, Geometry.STRING_ROUND_DECIMALS_VOLTAGE)}";
                }
                break;

            case ActionType.CustomCode:
                ActionCustomCode acc = action as ActionCustomCode;
                if (!acc.isDeclaration)
                {
                    dec = $"  {acc.statement}";
                }
                break;

                //default:
                //    dec = string.Format("  ; ACTION \"{0}\" NOT IMPLEMENTED", action);
                //    break;
            }

            if (ADD_ACTION_STRING && action.type != ActionType.Comment)
            {
                dec = string.Format("{0}  {1} [{2}]",
                                    dec,
                                    commChar,
                                    action.ToString());
            }
            else if (ADD_ACTION_ID)
            {
                dec = string.Format("{0}  {1} [{2}]",
                                    dec,
                                    commChar,
                                    action.id);
            }

            declaration = dec;
            return(dec != null);
        }
Ejemplo n.º 6
0
        internal bool GenerateInstructionDeclaration(
            Action action, RobotCursor cursor, bool humanComments,
            out string declaration)
        {
            string dec = null;

            switch (action.Type)
            {
            case ActionType.Translation:
            case ActionType.Rotation:
            case ActionType.Transformation:
                // Accelerations and velocities have different meaning for movej and movel instructions: they are either angular or linear respectively.
                // Use speed and acceleration values as deg/s or mm/s (converted to rad and m) in either case.
                if (cursor.motionType == MotionType.Joint)
                {
                    dec = string.Format(CultureInfo.InvariantCulture,
                                        "  movej({0}, a={1}, v={2}, r={3})",
                                        GetPoseTargetValue(cursor),
                                        Math.Round(Geometry.TO_RADS * cursor.acceleration, Geometry.STRING_ROUND_DECIMALS_RADS),
                                        Math.Round(Geometry.TO_RADS * cursor.speed, Geometry.STRING_ROUND_DECIMALS_RADS),
                                        Math.Round(0.001 * cursor.precision, Geometry.STRING_ROUND_DECIMALS_M));
                }
                else
                {
                    dec = string.Format(CultureInfo.InvariantCulture,
                                        "  movep({0}, a={1}, v={2}, r={3})",
                                        GetPoseTargetValue(cursor),
                                        Math.Round(0.001 * cursor.acceleration, Geometry.STRING_ROUND_DECIMALS_M),
                                        Math.Round(0.001 * cursor.speed, Geometry.STRING_ROUND_DECIMALS_M),
                                        Math.Round(0.001 * cursor.precision, Geometry.STRING_ROUND_DECIMALS_M));
                }
                break;

            //// Accelerations and velocoties have different meaning for moveJ and moveL instructions.
            //// Joint motion is essentially the same as Axes motion, just the input is a pose instead of a joints vector.
            //if (cursor.motionType == MotionType.Joint)
            //{
            //    dec = string.Format("  movej({0}, a={1}, v={2}, r={3})",
            //        GetPoseTargetValue(cursor),
            //        cursor.jointAcceleration > Geometry.EPSILON ? Math.Round(Geometry.TO_RADS * cursor.jointAcceleration, Geometry.STRING_ROUND_DECIMALS_RADS) : DEFAULT_JOINT_ACCELERATION,
            //        cursor.jointSpeed > Geometry.EPSILON ? Math.Round(Geometry.TO_RADS * cursor.jointSpeed, Geometry.STRING_ROUND_DECIMALS_RADS) : DEFAULT_JOINT_SPEED,
            //        Math.Round(0.001 * cursor.precision, Geometry.STRING_ROUND_DECIMALS_M));
            //}
            //else
            //{
            //    dec = string.Format("  movel({0}, a={1}, v={2}, r={3})",
            //        GetPoseTargetValue(cursor),
            //        cursor.acceleration > Geometry.EPSILON ? Math.Round(0.001 * cursor.acceleration, Geometry.STRING_ROUND_DECIMALS_M) : DEFAULT_TOOL_ACCELERATION,
            //        cursor.speed > Geometry.EPSILON ? Math.Round(0.001 * cursor.speed, Geometry.STRING_ROUND_DECIMALS_M) : DEFAULT_TOOL_SPEED,
            //        Math.Round(0.001 * cursor.precision, Geometry.STRING_ROUND_DECIMALS_M));
            //}
            //break;

            case ActionType.Axes:
                dec = string.Format(CultureInfo.InvariantCulture,
                                    "  movej({0}, a={1}, v={2}, r={3})",
                                    GetJointTargetValue(cursor),
                                    Math.Round(Geometry.TO_RADS * cursor.acceleration, Geometry.STRING_ROUND_DECIMALS_RADS),
                                    Math.Round(Geometry.TO_RADS * cursor.speed, Geometry.STRING_ROUND_DECIMALS_RADS),
                                    Math.Round(0.001 * cursor.precision, Geometry.STRING_ROUND_DECIMALS_M));

                //dec = string.Format("  movej({0}, a={1}, v={2}, r={3})",
                //    GetJointTargetValue(cursor),
                //    cursor.jointAcceleration > Geometry.EPSILON ? Math.Round(Geometry.TO_RADS * cursor.jointAcceleration, Geometry.STRING_ROUND_DECIMALS_RADS) : DEFAULT_JOINT_ACCELERATION,
                //    cursor.jointSpeed > Geometry.EPSILON ? Math.Round(Geometry.TO_RADS * cursor.jointSpeed, Geometry.STRING_ROUND_DECIMALS_RADS) : DEFAULT_JOINT_SPEED,
                //    Math.Round(0.001 * cursor.precision, Geometry.STRING_ROUND_DECIMALS_M));
                break;

            case ActionType.Message:
                ActionMessage am = (ActionMessage)action;
                dec = string.Format("  popup(\"{0}\", title=\"Machina Message\", warning=False, error=False)",
                                    am.message);
                break;

            case ActionType.Wait:
                ActionWait aw = (ActionWait)action;
                dec = string.Format(CultureInfo.InvariantCulture,
                                    "  sleep({0})",
                                    0.001 * aw.millis);
                break;

            case ActionType.Comment:
                ActionComment ac = (ActionComment)action;
                dec = string.Format("  {0} {1}",
                                    CC,
                                    ac.comment);
                break;

            case ActionType.DefineTool:
                ActionDefineTool adt = action as ActionDefineTool;
                dec = string.Format("  {0} Tool \"{1}\" defined",      // this action has no actual instruction, just add a comment
                                    CC,
                                    adt.tool.name);
                break;

            case ActionType.AttachTool:
                ActionAttachTool aa = (ActionAttachTool)action;
                dec = string.Format("  set_tcp({0})",       // @TODO: should need to add a "set_payload(m, CoG)" dec afterwards...
                                    GetToolValue(cursor));
                break;

            case ActionType.DetachTool:
                ActionDetachTool ad = (ActionDetachTool)action;
                dec = string.Format("  set_tcp(p[0,0,0,0,0,0])");       // @TODO: should need to add a "set_payload(m, CoG)" dec afterwards...
                break;

            case ActionType.IODigital:
                ActionIODigital aiod = (ActionIODigital)action;
                if (!aiod.isDigit)
                {
                    dec = $"  {CC} ERROR on \"{aiod}\": only integer pin names are possible";
                }
                else if (aiod.pinNum < 0 || aiod.pinNum > 7)
                {
                    dec = $"  {CC} ERROR on \"{aiod}\": IO number not available";
                }
                else
                {
                    dec = $"  set_{(aiod.isToolPin ? "tool" : "standard")}_digital_out({aiod.pinNum}, {(aiod.on ? "True" : "False")})";
                }
                break;

            case ActionType.IOAnalog:
                ActionIOAnalog aioa = (ActionIOAnalog)action;
                if (!aioa.isDigit)
                {
                    dec = $"  {CC} ERROR on \"{aioa}\": only integer pin names are possible";
                }
                else if (aioa.pinNum < 0 || aioa.pinNum > 1)
                {
                    dec = $"  {CC} ERROR on \"{aioa}\": IO number not available";
                }
                else if (aioa.value < 0 || aioa.value > 1)
                {
                    dec = $"  {CC} ERROR on \"{aioa}\": value out of range [0.0, 1.0]";
                }
                else
                {
                    //dec = $"  set_{(aioa.isToolPin ? "tool" : "standard")}_analog_out({aioa.pinNum}, {Math.Round(aioa.value, Geometry.STRING_ROUND_DECIMALS_VOLTAGE)})";
                    dec = string.Format(CultureInfo.InvariantCulture,
                                        "  set_{0}_analog_out({1}, {2})",
                                        aioa.isToolPin ? "tool" : "standard",
                                        aioa.pinNum,
                                        Math.Round(aioa.value, Geometry.STRING_ROUND_DECIMALS_VOLTAGE));
                }
                break;

            case ActionType.CustomCode:
                ActionCustomCode acc = action as ActionCustomCode;
                if (!acc.isDeclaration)
                {
                    dec = $"  {acc.statement}";
                }
                break;

                //default:
                //    dec = string.Format("  # ACTION \"{0}\" NOT IMPLEMENTED", action);
                //    break;
            }

            if (humanComments && action.Type != ActionType.Comment)
            {
                dec = string.Format("{0}  {1} [{2}]",
                                    dec,
                                    CC,
                                    action.ToString());
            }
            //else if (ADD_ACTION_ID)
            //{
            //    dec = string.Format("{0}  {1} [{2}]",
            //        dec,
            //        CC,
            //        action.id);
            //}

            declaration = dec;
            return(dec != null);
        }
Ejemplo n.º 7
0
        internal static bool GenerateInstructionDeclaration(
            Action action, RobotCursor cursor, bool humanComments,
            out string declaration)
        {
            string dec = null;

            switch (action.type)
            {
            case ActionType.Translation:
            case ActionType.Rotation:
            case ActionType.Transformation:
                // Accelerations and velocoties have different meaning for moveJ and moveL instructions.
                // Joint motion is essentially the same as Axes motion, just the input is a pose instead of a joints vector.
                if (cursor.motionType == MotionType.Joint)
                {
                    dec = string.Format("  movej({0}, a={1}, v={2}, r={3})",
                                        GetPoseTargetValue(cursor),
                                        cursor.jointAcceleration > Geometry.EPSILON2 ? Math.Round(Geometry.TO_RADS * cursor.jointAcceleration, Geometry.STRING_ROUND_DECIMALS_RADS) : DEFAULT_JOINT_ACCELERATION,
                                        cursor.jointSpeed > Geometry.EPSILON2 ? Math.Round(Geometry.TO_RADS * cursor.jointSpeed, Geometry.STRING_ROUND_DECIMALS_RADS) : DEFAULT_JOINT_SPEED,
                                        Math.Round(0.001 * cursor.precision, Geometry.STRING_ROUND_DECIMALS_M));
                }
                else
                {
                    dec = string.Format("  movel({0}, a={1}, v={2}, r={3})",
                                        GetPoseTargetValue(cursor),
                                        cursor.acceleration > Geometry.EPSILON2 ? Math.Round(0.001 * cursor.acceleration, Geometry.STRING_ROUND_DECIMALS_M) : DEFAULT_TOOL_ACCELERATION,
                                        cursor.speed > Geometry.EPSILON2 ? Math.Round(0.001 * cursor.speed, Geometry.STRING_ROUND_DECIMALS_M) : DEFAULT_TOOL_SPEED,
                                        Math.Round(0.001 * cursor.precision, Geometry.STRING_ROUND_DECIMALS_M));
                }
                break;

            case ActionType.RotationSpeed:
                dec = string.Format("  {0} WARNING: RotationSpeed() has no effect in UR robots, try JointSpeed() or JointAcceleration() instead", COMMENT_CHAR);
                break;

            case ActionType.Axes:
                // HAL generates a "set_tcp(p[0,0,0,0,0,0])" call here which I find confusing...
                dec = string.Format("  movej({0}, a={1}, v={2}, r={3})",
                                    GetJointTargetValue(cursor),
                                    cursor.jointAcceleration > Geometry.EPSILON2 ? Math.Round(Geometry.TO_RADS * cursor.jointAcceleration, Geometry.STRING_ROUND_DECIMALS_RADS) : DEFAULT_JOINT_ACCELERATION,
                                    cursor.jointSpeed > Geometry.EPSILON2 ? Math.Round(Geometry.TO_RADS * cursor.jointSpeed, Geometry.STRING_ROUND_DECIMALS_RADS) : DEFAULT_JOINT_SPEED,
                                    Math.Round(0.001 * cursor.precision, Geometry.STRING_ROUND_DECIMALS_M));
                break;

            case ActionType.Message:
                ActionMessage am = (ActionMessage)action;
                dec = string.Format("  popup(\"{0}\", title=\"Machina Message\", warning=False, error=False)",
                                    am.message);
                break;

            case ActionType.Wait:
                ActionWait aw = (ActionWait)action;
                dec = string.Format("  sleep({0})",
                                    0.001 * aw.millis);
                break;

            case ActionType.Comment:
                ActionComment ac = (ActionComment)action;
                dec = string.Format("  {0} {1}",
                                    COMMENT_CHAR,
                                    ac.comment);
                break;

            case ActionType.Attach:
                ActionAttach aa = (ActionAttach)action;
                dec = string.Format("  set_tcp({0})",       // @TODO: should need to add a "set_payload(m, CoG)" dec afterwards...
                                    GetToolValue(cursor));
                break;

            case ActionType.Detach:
                ActionDetach ad = (ActionDetach)action;
                dec = string.Format("  set_tcp(p[0,0,0,0,0,0])");       // @TODO: should need to add a "set_payload(m, CoG)" dec afterwards...
                break;

            case ActionType.IODigital:
                ActionIODigital aiod = (ActionIODigital)action;
                if (!aiod.isDigit)
                {
                    dec = $"  {COMMENT_CHAR} ERROR on \"{aiod}\": only integer pin names are possible";
                }
                else if (aiod.pinNum < 0 || aiod.pinNum > 7)
                {
                    dec = $"  {COMMENT_CHAR} ERROR on \"{aiod}\": IO number not available";
                }
                else
                {
                    dec = $"  set_{(aiod.isToolPin ? "tool" : "standard")}_digital_out({aiod.pinNum}, {(aiod.on ? "True" : "False")})";
                }
                break;

            case ActionType.IOAnalog:
                ActionIOAnalog aioa = (ActionIOAnalog)action;
                if (!aioa.isDigit)
                {
                    dec = $"  {COMMENT_CHAR} ERROR on \"{aioa}\": only integer pin names are possible";
                }
                else if (aioa.pinNum < 0 || aioa.pinNum > 1)
                {
                    dec = $"  {COMMENT_CHAR} ERROR on \"{aioa}\": IO number not available";
                }
                else if (aioa.value < 0 || aioa.value > 1)
                {
                    dec = $"  {COMMENT_CHAR} ERROR on \"{aioa}\": value out of range [0.0, 1.0]";
                }
                else
                {
                    dec = $"  set_{(aioa.isToolPin ? "tool" : "standard")}_analog_out({aioa.pinNum}, {Math.Round(aioa.value, Geometry.STRING_ROUND_DECIMALS_VOLTAGE)})";
                }
                break;

                //default:
                //    dec = string.Format("  # ACTION \"{0}\" NOT IMPLEMENTED", action);
                //    break;
            }

            if (humanComments && action.type != ActionType.Comment)
            {
                dec = string.Format("{0}  {1} [{2}]",
                                    dec,
                                    COMMENT_CHAR,
                                    action.ToString());
            }
            //else if (ADD_ACTION_ID)
            //{
            //    dec = string.Format("{0}  {1} [{2}]",
            //        dec,
            //        COMMENT_CHAR,
            //        action.id);
            //}

            declaration = dec;
            return(dec != null);
        }
Ejemplo n.º 8
0
        internal bool GenerateInstructionDeclarationFromVariable(
            Action action,
            RobotCursor cursor,
            int id,
            Dictionary <double, string> velNames,
            Dictionary <double, string> zoneNames,
            Dictionary <Tool, string> toolNames,
            out string declaration)
        {
            string dec = null;

            switch (action.type)
            {
            case ActionType.Acceleration:
                bool zero = cursor.acceleration < Geometry.EPSILON2;
                dec = string.Format("    WorldAccLim {0};",
                                    zero ? "\\Off" : "\\On := " + Math.Round(0.001 * cursor.acceleration, Geometry.STRING_ROUND_DECIMALS_M));
                break;

            case ActionType.JointSpeed:
            case ActionType.JointAcceleration:
                dec = string.Format("    {0} WARNING: {1}() has no effect in ABB robots.",
                                    commChar,
                                    action.type);
                break;

            // @TODO: push/pop management should be done PROGRAMMATICALLY, not this CHAPUZA...
            case ActionType.PushPop:
                // Find if there was a change in acceleration, and set the corresponsing instruction...
                ActionPushPop app = action as ActionPushPop;
                if (app.push)
                {
                    break;                // only necessary for pops
                }
                if (Math.Abs(cursor.acceleration - cursor.settingsBuffer.SettingsBeforeLastPop.Acceleration) < Geometry.EPSILON2)
                {
                    break;                                                                                                                    // no change
                }
                // If here, there was a change, so...
                bool zeroAcc = cursor.acceleration < Geometry.EPSILON2;
                dec = string.Format("    WorldAccLim {0};",
                                    zeroAcc ? "\\Off" : "\\On := " + Math.Round(0.001 * cursor.acceleration, Geometry.STRING_ROUND_DECIMALS_M));
                break;

            case ActionType.Translation:
            case ActionType.Rotation:
            case ActionType.Transformation:
                dec = string.Format("    {0} target{1}, {2}, {3}, {4}\\{5};",
                                    cursor.motionType == MotionType.Joint ? "MoveJ" : "MoveL",
                                    id,
                                    velNames[cursor.speed],
                                    zoneNames[cursor.precision],
                                    cursor.tool == null ? "Tool0" : toolNames[cursor.tool],
                                    "WObj:=WObj0");
                break;

            case ActionType.Axes:
                dec = string.Format("    MoveAbsJ target{0}, {1}, {2}, {3}\\{4};",
                                    id,
                                    velNames[cursor.speed],
                                    zoneNames[cursor.precision],
                                    cursor.tool == null ? "Tool0" : toolNames[cursor.tool],
                                    "WObj:=WObj0");
                break;

            case ActionType.Message:
                ActionMessage am = (ActionMessage)action;
                dec = string.Format("    TPWrite \"{0}\";",
                                    am.message.Length <= 80 ?
                                    am.message :
                                    am.message.Substring(0, 80)); // ABB TPWrite messages can only be 80 chars long
                break;

            case ActionType.Wait:
                ActionWait aw = (ActionWait)action;
                dec = string.Format("    WaitTime {0};",
                                    0.001 * aw.millis);
                break;

            case ActionType.Comment:
                ActionComment ac = (ActionComment)action;
                dec = string.Format("    {0} {1}",
                                    commChar,
                                    ac.comment);
                break;

            case ActionType.Attach:
                ActionAttach aa = (ActionAttach)action;
                dec = string.Format("    {0} Tool \"{1}\" attached",      // this action has no actual RAPID instruction, just add a comment
                                    commChar,
                                    aa.tool.name);
                break;

            case ActionType.Detach:
                ActionDetach ad = (ActionDetach)action;
                dec = string.Format("    {0} Tool detached",      // this action has no actual RAPID instruction, just add a comment
                                    commChar);
                break;

            case ActionType.IODigital:
                ActionIODigital aiod = (ActionIODigital)action;
                if (aiod.pin < 0 || aiod.pin >= cursor.digitalOutputs.Length)
                {
                    dec = string.Format("    {0} ERROR on \"{1}\": IO number not available",
                                        commChar,
                                        aiod.ToString());
                }
                else
                {
                    dec = string.Format("    SetDO {0}, {1};",
                                        cursor.digitalOutputNames[aiod.pin],
                                        aiod.on ? "1" : "0");
                }
                break;

            case ActionType.IOAnalog:
                ActionIOAnalog aioa = (ActionIOAnalog)action;
                if (aioa.pin < 0 || aioa.pin >= cursor.analogOutputs.Length)
                {
                    dec = string.Format("    {0} ERROR on \"{1}\": IO number not available",
                                        commChar,
                                        aioa.ToString());
                }
                else
                {
                    dec = string.Format("    SetAO {0}, {1};",
                                        cursor.analogOutputNames[aioa.pin],
                                        aioa.value);
                }
                break;

                //default:
                //    dec = string.Format("    ! ACTION \"{0}\" NOT IMPLEMENTED", action);
                //    break;
            }

            if (ADD_ACTION_STRING && action.type != ActionType.Comment)
            {
                dec = string.Format("{0}  {1} [{2}]",
                                    dec,
                                    commChar,
                                    action.ToString());
            }
            else if (ADD_ACTION_ID)
            {
                dec = string.Format("{0}  {1} [{2}]",
                                    dec,
                                    commChar,
                                    action.id);
            }

            declaration = dec;
            return(dec != null);
        }