[筆記]讀取台達 ASDA-A2 伺服扭力值

 狀態監控暫存器

狀態監控暫存器 P0-09 ~ P0-13 為唯讀暫存器，各自對應到選擇狀態監控暫存器 P0-17~P0-21，將要讀取的運轉的監視變數設定到 P0-17 ~ P0-21 中可從 P0-09 ~ P0-13讀取到。

例如：將  P0-17 值設為 12，可透 Modbus 讀取 P0-09(0012H 0013H) 負載平均值。

    class MyModbus
    {
     	public enum ErrorCode
        {
            NormalCompletion,
            LrcError,
            FormatError,
            FrameLengthError,
            SerialPortNotOpen,
            SerialPortWriteFailure,
            SerialPortReadFailure,
            ReadTimeout,
        }

        private string mSTX = ":";
        private string mETX = "\r\n";
        
    	public ErrorCode Send(SerialPort _SerialPort, string Cmd)
        {
            if (_SerialPort.IsOpen == false) return ErrorCode.SerialPortNotOpen;
            _SerialPort.DiscardInBuffer();
            _SerialPort.DiscardOutBuffer();
            _SerialPort.Write(Cmd);
            return ErrorCode.NormalCompletion;
        }

        public ErrorCode Receive(SerialPort _SerialPort, ref string Respen, int _Timeout)
        {
            if (_SerialPort.IsOpen == false) return ErrorCode.SerialPortNotOpen;
            try
            {
                _SerialPort.ReadTimeout = _Timeout;
                Respen = _SerialPort.ReadTo(mETX);
            }
            catch
            {
                Respen = _SerialPort.ReadExisting();
                return ErrorCode.ReadTimeout;
            }
            return ErrorCode.NormalCompletion;
        }

        public string LrcClec(string Massage)
        {
            byte sum = 0;
            for (int idx = 0; idx < Massage.Length; idx += 2)
            {
                string str = Massage.Substring(idx, 2);
                byte value = Convert.ToByte(str, 16);
                sum = (byte)((sum + value) & 0xff);

            }
            //取補數
            byte checksum = (byte)((~sum + 1) & 0xff);
            string Hex = checksum.ToString("X2");
            return Hex;

        }
        
        public ErrorCode ReadHoldingRegisters(SerialPort _SerialPort, string _Address, string _StartAddress, ushort _PointNumber, int _Timeout, out string[] Result)
        {
            Result = new string[_PointNumber];

            string Msg = _Address + "03" + _StartAddress + _PointNumber.ToString("X4");
            string Cmd = mSTX + Msg + LrcClec(Msg) + mETX;
            ErrorCode Err;
            if ((Err = Send(_SerialPort, Cmd)) != ErrorCode.NormalCompletion) return Err;
            string Response = string.Empty;
            if ((Err = Receive(_SerialPort, ref Response, _Timeout)) != ErrorCode.NormalCompletion) return Err;
            //比對開頭碼是否為':'
            if (Response[0] != ':') return ErrorCode.FormatError;
            //移除符號
            Response = Response.Trim(':', '\r', '\n');
            //檢查 LRC
            string checksum = Response.Substring(Response.Length - 2, 2);
            string Data = Response.Substring(0, Response.Length - 2);
            if (checksum != LrcClec(Data)) return ErrorCode.LrcError;
            //檢查 資料長度
            int ResponseByteCnt = 2 + 2 + 2 + _PointNumber * 4 + 2;
            if (ResponseByteCnt != Response.Length) return ErrorCode.FrameLengthError;

            //轉換資料
            for (int i = 0; i < _PointNumber; i++)
            {
                Result[i] = Response.Substring(6 + (i * 4), 4);
            }

            return ErrorCode.NormalCompletion;
        }
    ｝

   public class ASDA_A2
    {
     	private Int32 mTorque = 0x00;
        public double Torque
        {
            get { return mRatedTorque * (mTorque / 100); }
        }

        public bool ReadTorque()
        {
            string[] Result;
            if (mSerialPort.IsOpen == false) return false;
            MyModbus.ErrorCode Err = MyModbus.ErrorCode.NormalCompletion;
            if ((Err = mModbus.ReadHoldingRegisters(mSerialPort, mAddress, "0012", 2, 3000, out Result)) != MyModbus.ErrorCode.NormalCompletion) return false;
            string DWord = Result[1] + Result[0];
            mTorque = Convert.ToInt32(DWord, 16);
            return true;
        }
    ｝