CANopen协议【SDO】

SDO：

SDO是读写单个寄存器。主要用于配置伺服驱动器参数。

1  收发功能： 

       //public unsafe struct VCI_CAN_OBJ  //CAN数据帧 【单帧对象】
       //{
       //    public uint ID;// 帧ID。      【11bit】数据右对齐。 详情请参照： 《8.附件1： ID对齐方式.pdf》说明文档。
       //    public uint TimeStamp;        //设备接收到某一帧的时间标识。 时间标示从CAN卡上电开始计时，计时单位为0.1ms。
       //    public byte TimeFlag;         //时间标识，为1时TimeStamp有效
       //    public byte SendType;         //发送帧类型。=0时为正常发送（发送失败会自动重发，重发超时时间为4秒， 4秒内没有发出则取消）；=1时为单次发送
       //    public byte RemoteFlag;       //是否是远程帧【RTR】。 =0时为数据帧， =1时为远程帧（数据段空）。
       //    public byte ExternFlag;       //是否是扩展帧。 =0时为标准帧（11位ID）， =1时为扩展帧（29位ID）。
       //    public byte DataLen;          //数据长度 DLC (<=8)，即CAN帧Data有几个字节。约束了后面Data[8]中的有效字节。
       //    public fixed byte Data[8];    //如DataLen定义为3，即Data[0]、 Data[1]、 Data[2]是有效的。
       //    public fixed byte Reserved[3];//系统保留。
       //}

  unsafe public int Get_Receive(ref VCI_CAN_OBJ[] obj)
  {
      //===1 等待加载=================
      DelaySecond(0.005);//5ms   // USB速度3~5ms

      //===2 接口接收=================
      object obj2 = new object();//对象
      int num = Help_rtx.RXs(ref obj2);//  can.get_Receive(CAN_DeviceType, CAN_ind, CAN1, ref obj[0], 2000, 50);
      if (num < 1) return 0;// 空退

      //===3 取出帧，并且分发=========
      obj = (VCI_CAN_OBJ[])obj2;

      //====3界面显示=================
      // 分发数据帧，UI层显示
      if (Wt_get != null)
      {// 数组是引用类型
          VCI_CAN_OBJ[] uiObj = new VCI_CAN_OBJ[num];
          uiObj = obj;
          UIshow(num, uiObj);


          //    //COB - ID              Byte 0:1        Byte 2              Byte 3:7
          //    //0x080 + Node - ID     应急错误代码   错误寄存器(1001H)    厂商指定区域

          //    //byte0~1：  8120  【0x603F】
          //    //        应急错误代码
          //    //        0000H 无错误
          //    //        8110H CAN 溢出
          //    //        8120H 错误被动模式（0411）
          //    //        8130H 寿命保护/ 心跳错误
          //    //        8140H 被迫离线恢复故障
          //    //        8141H 被迫离线
          //    //        8150H 发送 COB - ID 冲突
          //    //        8210H PDO 长度错误未处理
          //    //        8220H PDO 超过长度

          //    //byte2：   11
          //    //        (1001H)位定义
          //    //         Bit0：generic error 一般错误
          //    //        Bit1：current 电流
          //    //        Bit2：voltage 电压 （04）
          //    //        Bit3：temperature 温度
          //    //         Bit4：通讯
          //    //        Bit5：协议402错误
          //    //        Bit6：保留
          //    //        Bit7：伺服厂家


          //    //————————————————
          //    //版权声明：本文为CSDN博主「cfqq1989」的原创文章，遵循CC 4.0 BY - SA版权协议，转载请附上原文出处链接及本声明。
          //    //原文链接：https://blog.csdn.net/cfqq1989/article/details/128308808
          //}// EMCY：故障（从站故障）80h + 从站

          //if (item.ID > 0x700 && item.ID < (0x700 + 127)) //从站状态
          //{
          //    if (Wt_get!=null)
          //    {
          //        NMTslave slave=(NMTslave)p[0];
          //        string sta = Enum.GetName(typeof(NMTslave), slave);
          //        //string str = $"从站：{item.ID - 0x700}状态{p[0].ToString("X2")}； ";
          //        string str = $"从站：{item.ID - 0x700}_{sta}； ";
          //        Wt_get(null, str);
          //    }
          //}



      }

      return num;
  }

  /// <summary>
  /// 【1 4 8】DLC长度，帧id，数据包64bit【小端】
  /// </summary>
  /// <param name="DLC"></param>
  /// <param name="CANid"></param>
  /// <param name="byte8"></param>
  /// <returns></returns>
  /// <exception cref="Exception"></exception>
  public bool Set_Send(Byte DLC, UInt32 CANid, Int64 byte8)//  1  4  8
  {
      //int hand = 0x88;// 设备：USR-CANET200
      //int cobid = 0x12345678;// 11bit或29bit
      //Int64 byte8 = 0x0011223344556677;
      //wait_dataRX数据池();
      //can.send(hand8, cobid, byte8);
      //wait_dataRX数据池();
      if (DLC > 8) DLC = 8;// <=8
      string cmd = madeCode((byte)DLC, (UInt32)CANid, byte8);// Code编码  "08   00000602    2B 5021 00 00000000"

      if (Help_rtx.TXs(cmd)==1)// 接口功能  "08000006022B50210000000000" // 单帧 13字节( 1 4 8 在接口解码发送)
      {
          return true;
      }
      else throw new Exception("发送失败。检查主站硬件状态（是否开启？）！");
      //return false;
  }

2  SDO功能码：

     void SDO(Region_Type regtype, UInt16 reg, Byte son, Int32 value)// 1操作类型，2寄存器编号，3子索引，4参数值
     {// SDO操作  600h+
         //========================================================
         // 准备发送
         Byte by0 = (Byte)regtype;//1 操作码【u8】
         UInt16 by1 = (UInt16)reg;//2 寄存器【u16】
         Byte by2 = (Byte)son;//3 子索引【u8】
         Int64 by3 = (Int32)value;//4设定值【u32】

         // b0  b1b2  b3   b4b5b6b7
         Byte8 = by0 + (by1 << 8) + (by2 << 24) + (by3 << 32); // 23 2000 00 11223344     

         Set_Send(0x08, 0x600 + Address, Byte8);//  40  2000  00  11223344

     }

3  SDO读：

读的数据帧发出后，伺服回帧包含【寄存器数据类型】【寄存器值】

这里  寄存器数据类型  需要标记，因为写寄存器时需要用到  【寄存器数据类型】

      public enum Region_Type : byte
      {
          读 = 0x40, // 回帧0x60成功
          成功 = 0x60,
          失败 = 0x80,
          写b8 = 0x2F,
          写b16 = 0x2B,
          写b24 = 0x27,
          写b32 = 0x23,
          //寄存器bit
          b8 = 0x4F,//回帧：4F 寄存器有效长度 8bit
          b16 = 0x4B,
          b24 = 0x47,
          b32 = 0x43
      }

 /// <summary>
 /// 【读】寄存器
 /// </summary>
 /// <param name="reg"></param>
 /// <param name="son"></param>
 /// <returns></returns>
  unsafe public Int32 SDO(UInt16 reg, Byte son)// 【读】寄存器，子索引【0x0000~0xFFFF】
 {// 读
     busy = true;
     //===1 get_数据类型=============//===2 get_Value===================
     SDO(Region_Type.读, reg, son, 0);//读

     //===2 get_帧===================
     #region get_Value
     VCI_CAN_OBJ[] obj = new VCI_CAN_OBJ[2000];// 创芯科技CAN分析仪最大支持2000帧接收，广成10239帧
     Rx13 = new byte[13];// 提取数据帧

     DelaySecond(0.010);//10ms   // USB速度3~5ms

     int num = Get_Receive(ref obj);
     busy = false;
     if (num > 0)// 数据帧
     {
         for (int i = num - 1; i >= 0; i--)
         {
             if (obj[i].ID == 0x580 + Address && obj[i].Data[0] != 0x60)//  SDO 600h+的回信【580h+】
             {// 580h+从站地址   回帧
                 //=====580h  寄存器类型====================
                 Rx13[5] = obj[i].Data[0];// 2001.00=50000； // 4B，01 20，00, 50 C3 00 00  // 
                 // u16寄存器
                 Rx13[6] = obj[i].Data[1];// 寄存器4B（b16位）  2001-00= 0x 0000C350
                 Rx13[7] = obj[i].Data[2];
                 // u8子索引
                 Rx13[8] = obj[i].Data[3];
                 // int32数据值
                 Rx13[9] = obj[i].Data[4];// 小端
                 Rx13[10] = obj[i].Data[5];
                 Rx13[11] = obj[i].Data[6];
                 Rx13[12] = obj[i].Data[7];
                 //=====2======================
                 //VCI_CAN_OBJ one = obj[i]; //
                 break;
             }// SDO应答帧(580h+)
         }
     }
     else
     {
         Region_Obj = 0x80;// 故障
         return -1;
     }
     // 回帧结果
     Region_Obj = Rx13[5];// 标记： b8 = 0x4F,   b16 = 0x4B,    b24 = 0x47,    b32 = 0x43    //0x6040是16bit
     int value = get_Value_U32(Rx13);// 解析   43,4060,00,0000 0000
     //  日志====================================
     if (Region_Obj < 0x43 | Region_Obj > 0x4F)// u8~32 读取失败 // 参考 enum region_Type  //| Region_Type>(int)region_Type.b32
     {//  0x05030000  80 
         string error = getError(value, reg, son, -1); // 故障码，寄存器，子，值
         if (Wt_get != null)
         {
             Wt_get(new byte[] { 0x00 }, "读取寄存器错误：" + error);
         }
         // throw new Exception("读取错误：" + error);// 提前终止
     }//错误
     else
     {//成功

         return value;
     }//正确
     #endregion
     return -1;
 }

4  SDO写：

    unsafe public bool SDO(UInt16 reg, Byte son, Int32 value)//【写】
    {// 写
        busy = true;
        //===1 get寄存器数据类型=============
        Int32 data = SDO(reg, son);//读

        //===2 set_Value=====================
        if (Region_Obj >= 0x43 || Region_Obj <= 0x4F)
        {
            SDO((Region_Type)Region_Obj - 0x20, reg, son, value);
        }
        else { return false; }
        //===2 get_帧===================
        #region get_Value
        VCI_CAN_OBJ[] obj = new VCI_CAN_OBJ[2000];// 创芯科技CAN分析仪最大支持2000帧接收，广成10239帧
        Rx13 = new byte[13];// 提取数据帧

        DelaySecond(0.010);//10ms   // USB速度3~5ms

        int num = Get_Receive(ref obj);
        busy = false;
        if (num > 0)// 数据帧
        {
            for (int i = num - 1; i >= 0; i--)
            {
                if (obj[i].ID == 0x580 + Address && obj[i].Data[0] == 0x60)//  SDO 600h+的回信【580h+】
                {// 580h+从站地址   回帧
                    return true;
                }// SDO应答帧(580h+)
            }
        }
        #endregion
        Region_Obj = 0x80;// 故障
        return false;
    }