From c3a662bc8a8f5442562780da244020949eedcd1c Mon Sep 17 00:00:00 2001 From: ziteh Date: Fri, 12 Aug 2022 15:24:21 +0800 Subject: [PATCH] UpdaFix the bug for parse motion state #108 --- RASDK.Arm/Hiwin/Exception.cs | 2 +- RASDK.Arm/Hiwin/RoboticArm.cs | 104 ++++++++++++++++++++++++---------- 2 files changed, 75 insertions(+), 31 deletions(-) diff --git a/RASDK.Arm/Hiwin/Exception.cs b/RASDK.Arm/Hiwin/Exception.cs index b6489de..6e8605c 100644 --- a/RASDK.Arm/Hiwin/Exception.cs +++ b/RASDK.Arm/Hiwin/Exception.cs @@ -12,7 +12,7 @@ public HiwinRobotControlException(int code) : base($"HIWIN robot error code: {co { } public HiwinRobotControlException(int code, string message) - : base($"HIWIN robot error code: {code}. {message}.") + : base($"HIWIN robot error code: {code:0000}. {message}.") { } public HiwinRobotControlException(string message, System.Exception innerException) diff --git a/RASDK.Arm/Hiwin/RoboticArm.cs b/RASDK.Arm/Hiwin/RoboticArm.cs index 56f008f..0b4bea5 100644 --- a/RASDK.Arm/Hiwin/RoboticArm.cs +++ b/RASDK.Arm/Hiwin/RoboticArm.cs @@ -19,6 +19,8 @@ namespace RASDK.Arm.Hiwin /// public class RoboticArm : RASDK.Arm.RoboticArm { + public static bool ShowCallBackInfo = false; + /// /// 正在動作中。 /// @@ -366,6 +368,42 @@ public override void MoveRelative(double j1X, _message.LogMethodEnd(nameof(MoveRelative)); } + private static bool ParseMovingState(int motionStateCode, bool srcState) + { + var moving = srcState; + + // Ref: HRSDK get_motion_state(). + switch (motionStateCode) + { + case 0: // 解激磁。 + case 1: // 等待、閒置。 + case 3: // 暫停。 + moving = false; + break; + + case 2: // 運行。 + case 5: // 移動。 + moving = true; + break; + + case 4: // 延遲。 + default: + /* Do nothing. */ + break; + } + + return moving; + } + + private void SoftEnableMotor() + { + // If motor disable, enable motor. + if (HRobot.get_motor_state(_id) == 0) + { + HRobot.set_motor_state(_id, 1); + } + } + private int GetSmoothTypeCode(SmoothType smoothType, MotionType motionType) { int code = 0; @@ -403,15 +441,11 @@ private void WaitForMotionComplete() // Active update motion state. var motionState = HRobot.get_motion_state(_id); - if (motionState == 1) // Idle. + _moving = ParseMovingState(motionState, _moving); + if (!_moving) { - _moving = false; break; } - else if (motionState == 2 || motionState == 4 || motionState == 5) - { - _moving = true; - } } } while (_moving || !_receivedCallBack); @@ -535,38 +569,48 @@ private static void EventFun(UInt16 cmd, UInt16 rlt, ref UInt16 Msg, int len) // 此處不受 IMessage 影響。 // Show in Console. - Console.WriteLine($"Command:{cmd}, Result:{rlt}"); + if (ShowCallBackInfo) + { + Console.WriteLine($"Command:{cmd}, Result:{rlt}"); + } + switch (cmd) { case 0 when rlt == 4702: _receivedCallBack = true; // Set received flag. - Console.WriteLine($"HRSS Mode:{infos[0]}\r\n" + - $"Operation Mode:{infos[1]}\r\n" + - $"Override Ratio:{infos[2]}\r\n" + - $"Motor State:{infos[3]}\r\n" + - $"Exe File Name:{infos[4]}\r\n" + - $"Function Output:{infos[5]}\r\n" + - $"Alarm Count:{infos[6]}\r\n" + - $"Keep Alive:{infos[7]}\r\n" + - $"Motion Status:{infos[8]}\r\n" + - $"Payload:{infos[9]}\r\n" + - $"Speed:{infos[10]}\r\n" + - $"Position:{infos[11]}\r\n" + - $"Coor:{infos[14]},{infos[15]},{infos[16]},{infos[17]},{infos[18]},{infos[19]}\r\n" + - $"Joint:{infos[20]},{infos[21]},{infos[22]},{infos[23]},{infos[24]},{infos[25]}\r\n"); - - // Motion state=1: Idle. - _moving = int.Parse(infos[8]) != 1; + var motionState = int.Parse(infos[8]); + _moving = ParseMovingState(motionState, _moving); + + if (ShowCallBackInfo) + { + Console.WriteLine($"HRSS Mode:{infos[0]}\r\n" + + $"Operation Mode:{infos[1]}\r\n" + + $"Override Ratio:{infos[2]}\r\n" + + $"Motor State:{infos[3]}\r\n" + + $"Exe File Name:{infos[4]}\r\n" + + $"Function Output:{infos[5]}\r\n" + + $"Alarm Count:{infos[6]}\r\n" + + $"Keep Alive:{infos[7]}\r\n" + + $"Motion Status:{infos[8]}\r\n" + + $"Payload:{infos[9]}\r\n" + + $"Speed:{infos[10]}\r\n" + + $"Position:{infos[11]}\r\n" + + $"Coor:{infos[14]},{infos[15]},{infos[16]},{infos[17]},{infos[18]},{infos[19]}\r\n" + + $"Joint:{infos[20]},{infos[21]},{infos[22]},{infos[23]},{infos[24]},{infos[25]}\r\n"); + } break; case 4011 when rlt != 0: - MessageBox.Show("Update fail. " + rlt, - "HRSS update callback", - MessageBoxButtons.OK, - MessageBoxIcon.Warning, - MessageBoxDefaultButton.Button1, - MessageBoxOptions.DefaultDesktopOnly); + if (ShowCallBackInfo) + { + MessageBox.Show("Update fail. " + rlt, + "HRSS update callback", + MessageBoxButtons.OK, + MessageBoxIcon.Warning, + MessageBoxDefaultButton.Button1, + MessageBoxOptions.DefaultDesktopOnly); + } break; } }