GearedMotors700W/UserSource/UserCan.c

/*
 * UserCan.c
 *
 *  Created on: 2023年1月16日
 *      Author: senco
 */
#include "var.h"    // DSP2803x Examples Include File
#include "UserCan.h"      // DSP2803x Headerfile Include File
//#include "DSP2803x_Device.h"     // DSP28 Headerfile Include File
#include "DSP2803x_Examples.h"   // DSP28 Examples Include File
#define Reverse 0
#define Normal 1
// Global variable for this example
#define TX_PERIOD 20 //ms

//绘制曲线
#pragma DATA_SECTION(CanCurveData,"MemoryFile");
#pragma DATA_SECTION(CanCurveData2,"MemoryFile2");
int16 CanCurveData[0x400];
int16 CanCurveFlag = 0;
int16 CanDebugFlag = 1;
int16 CanCurveData2[0x400];

struct CANMBOX_USER_REGS CanMboxUserRegsT;      //用户CAN数据发送结构体
struct CANMBOX_USER_REGS CanMboxUserRegsT2;      //用户CAN数据发送结构体
struct CANMBOX_USER_REGS CanMboxUserRegsR;      //用户CAN接收数据结构体1
struct CANMBOX_USER_REGS CanMboxUserRegsR2;     //用户CAN接收数据结构体2
struct CANMBOX_USER_REGS CanMboxUserRegsR3;     //用户CAN接收数据结构体2




#define ID_MOTOR1_COMMAND 0x014E540A
#define ID_MOTOR2_COMMAND 0x014E560A
#define ID_MOTOR3_COMMAND 0x014E5A0A

#define ID_MOTOR1_SET 0x014E5C0A


Uint32 motor_command[]={ID_MOTOR1_COMMAND,ID_MOTOR2_COMMAND,ID_MOTOR3_COMMAND};
void CanVar(void){

    CanMboxUserRegsR.MboxRxFlg = 0;
    CanMboxUserRegsR.MboxNum = 0;
    CanMboxUserRegsR.MboxDLC = 0;
    CanMboxUserRegsR.MboxID = 0;
    CanMboxUserRegsR.MboxWord1 = 0;
    CanMboxUserRegsR.MboxWord2 = 0;
    CanMboxUserRegsR.MboxWord3 = 0;
    CanMboxUserRegsR.MboxWord4 = 0;

    CanMboxUserRegsR2.MboxRxFlg = 0;
    CanMboxUserRegsR2.MboxNum = 0;
    CanMboxUserRegsR2.MboxDLC = 0;
    CanMboxUserRegsR2.MboxID = 0;
    CanMboxUserRegsR2.MboxWord1 = 0;
    CanMboxUserRegsR2.MboxWord2 = 0;
    CanMboxUserRegsR2.MboxWord3 = 0;
    CanMboxUserRegsR2.MboxWord4 = 0;

    CanMboxUserRegsR3.MboxRxFlg = 0;
    CanMboxUserRegsR3.MboxNum = 0;
    CanMboxUserRegsR3.MboxDLC = 0;
    CanMboxUserRegsR3.MboxID = 0;
    CanMboxUserRegsR3.MboxWord1 = 0;
    CanMboxUserRegsR3.MboxWord2 = 0;
    CanMboxUserRegsR3.MboxWord3 = 0;
    CanMboxUserRegsR3.MboxWord4 = 0;

    CanMboxUserRegsT.MboxRxFlg = 0;
    CanMboxUserRegsT.MboxNum = 0;
    CanMboxUserRegsT.MboxDLC = 0;
    CanMboxUserRegsT.MboxID = 0;
    CanMboxUserRegsT.MboxWord1 = 0;
    CanMboxUserRegsT.MboxWord2 = 0;
    CanMboxUserRegsT.MboxWord3 = 0;
    CanMboxUserRegsT.MboxWord4 = 0;


}
void InitCan(void){
    struct ECAN_REGS ECanaShadow;
    InitECanaGpio();//引脚初始化
    //设置波特率
    InitECana(); // Initialize eCAN-A module

    // Mailboxes can be written to 16-bits or 32-bits at a time
        // Write to the MSGID field of TRANSMIT mailboxes MBOX0 - 15
        ECanaMboxes.MBOX0.MSGID.all = 0x014E600B|EXTFRAME;//定时上传
        ECanaMboxes.MBOX1.MSGID.all = 0x014E600B|EXTFRAME;//定时上传
        ECanaMboxes.MBOX2.MSGID.all = 0x014E600B|EXTFRAME;//定时上传
        ECanaMboxes.MBOX3.MSGID.all = 0x014E5D0B|EXTFRAME;//飞控参数返回
        ECanaMboxes.MBOX4.MSGID.all = 0x014E5D0B|EXTFRAME;//飞控参数返回
        ECanaMboxes.MBOX5.MSGID.all = 0x9555AAA5;//调试参数返回1 给定速度 目标速度 反馈速度 正反转
        ECanaMboxes.MBOX6.MSGID.all = 0x9555AAA6;//调试参数返回2 速度PID输出  运行占空比
        ECanaMboxes.MBOX7.MSGID.all = 0x9555AAA7;//调试参数返回3
        ECanaMboxes.MBOX8.MSGID.all = 0x9555AAA8;//调试参数返回4
        ECanaMboxes.MBOX9.MSGID.all = 0x9555AAA9;//调试参数返回5
        ECanaMboxes.MBOX10.MSGID.all = 0x9555AAAA;
        ECanaMboxes.MBOX11.MSGID.all = 0x9555AAAB;
        ECanaMboxes.MBOX12.MSGID.all = 0x9555AAAC;
        ECanaMboxes.MBOX13.MSGID.all = 0x9555AAAD;
        ECanaMboxes.MBOX14.MSGID.all = 0x9555AAAE;
        ECanaMboxes.MBOX15.MSGID.all = 0x9555AAAF;

        // Write to the MSGID field of RECEIVE mailboxes MBOX16 - 31
        ECanaMboxes.MBOX16.MSGID.all = 0xC0000000;//014E540A esc1 esc2
        ECanaMboxes.MBOX17.MSGID.all = 0x814E540A;
        ECanaMboxes.MBOX18.MSGID.all = 0x814E560A;
        ECanaMboxes.MBOX19.MSGID.all = 0x814E5A0A;
        ECanaMboxes.MBOX20.MSGID.all = 0x814E5C0A;
        ECanaMboxes.MBOX21.MSGID.all = 0xC0000000;
        ECanaMboxes.MBOX22.MSGID.all = 0xC0000000;
        ECanaMboxes.MBOX23.MSGID.all = 0xC0000000;
        ECanaMboxes.MBOX24.MSGID.all = 0x9555AAA8;
        ECanaMboxes.MBOX25.MSGID.all = 0x9555AAA9;
        ECanaMboxes.MBOX26.MSGID.all = 0x9555AAAA;
        ECanaMboxes.MBOX27.MSGID.all = 0x9555AAAB;
        ECanaMboxes.MBOX28.MSGID.all = 0x9555AAAC;
        ECanaMboxes.MBOX29.MSGID.all = 0x9555AAAD;
        ECanaMboxes.MBOX30.MSGID.all = 0x9555AAAE;
        ECanaMboxes.MBOX31.MSGID.all = 0x9555AAAF;

        // Configure Mailboxes 0-15 as Tx, 16-31 as Rx
        // Since this write is to the entire register (instead of a bit
        // field) a shadow register is not required.
        ECanaRegs.CANMD.all = 0xFFFF0000;// 0 transmit or 1 receive

        // Enable all Mailboxes
        // Since this write is to the entire register (instead of a bit
        // field) a shadow register is not required.
       /* ECanaLAMRegs.LAM20.all = 0x9FFFFFFF;//20号邮箱不在乎ID号
        // ECanaLAMRegs.LAM21.all = 0x9FFFFFFF;//21号邮箱不在乎ID号，当没有设置优先级的时候高位邮箱优先接收
        //ECanaRegs.CANME.all = 0x000FC3FF;//16-19号接收邮箱必须完全等于ID才能接收
         ECanaRegs.CANME.all = 0x001FC3FF;//由于设置20号邮箱不在乎ID，接收信息优先进入20号邮箱*/
        ECanaLAMRegs.LAM16.all = 0x9FFFFFFF;//16号邮箱不在乎ID号 不在乎ID的放在低位
        //ECanaRegs.CANME.all = 0x001FC3FF;//1 Mailbox enable bits 0-4 飞控协议 5-9用于发送调试上位机代码  14-15 用于发送绘图数据
        //17-20 用于飞控格式接收  16 用于调试上位机数据接收
       ECanaRegs.CANME.all = 0x0001C7E0;//屏蔽飞控相关传输


        // Specify that 8 bits will be sent/received
        ECanaMboxes.MBOX0.MSGCTRL.bit.DLC = 8;
        ECanaMboxes.MBOX1.MSGCTRL.bit.DLC = 8;
        ECanaMboxes.MBOX2.MSGCTRL.bit.DLC = 8;
        ECanaMboxes.MBOX3.MSGCTRL.bit.DLC = 8;
        ECanaMboxes.MBOX4.MSGCTRL.bit.DLC = 8;
        ECanaMboxes.MBOX5.MSGCTRL.bit.DLC = 8;
        ECanaMboxes.MBOX6.MSGCTRL.bit.DLC = 8;
        ECanaMboxes.MBOX7.MSGCTRL.bit.DLC = 8;
        ECanaMboxes.MBOX8.MSGCTRL.bit.DLC = 8;
        ECanaMboxes.MBOX9.MSGCTRL.bit.DLC = 8;
        ECanaMboxes.MBOX10.MSGCTRL.bit.DLC = 8;
        ECanaMboxes.MBOX11.MSGCTRL.bit.DLC = 8;
        ECanaMboxes.MBOX12.MSGCTRL.bit.DLC = 8;
        ECanaMboxes.MBOX13.MSGCTRL.bit.DLC = 8;
        ECanaMboxes.MBOX14.MSGCTRL.bit.DLC = 8;
        ECanaMboxes.MBOX15.MSGCTRL.bit.DLC = 8;
        ECanaMboxes.MBOX16.MSGCTRL.all = 0x00000008;
        ECanaMboxes.MBOX17.MSGCTRL.all = 0x00000008;
        ECanaMboxes.MBOX18.MSGCTRL.all = 0x00000008;
        ECanaMboxes.MBOX19.MSGCTRL.all = 0x00000008;
        ECanaMboxes.MBOX20.MSGCTRL.all = 0x00000008;
        ECanaMboxes.MBOX21.MSGCTRL.all = 0x00000008;

        // Write to the mailbox RAM field of MBOX0 - 15
        //  Message Data Registers MBOX0.MDL低字节和MBOX0.MDH高字节

        // Since this write is to the entire register (instead of a bit
        // field) a shadow register is not required.
        EALLOW;
        ECanaRegs.CANMIM.all = 0xFFFFFFFF;//中断使能

        // Configure the eCAN for self test mode
        // Enable the enhanced features of the eCAN.
        EALLOW;
        ECanaShadow.CANMC.all = ECanaRegs.CANMC.all;
        ECanaShadow.CANMC.bit.STM = 0;    // Configure CAN for self-test mode //1回环模式
        ECanaRegs.CANMC.all = ECanaShadow.CANMC.all;



        EDIS;

        CanVar();//数据初始化

}

void ECanMboxSend(struct  CANMBOX_USER_REGS *pRegs)
{// Configure Mailboxes 0-15 as Tx, 16-31 as Rx
    struct ECAN_REGS ECanaShadow;
    volatile struct MBOX *Mailbox;

    ECanaShadow.CANME.all = ECanaRegs.CANME.all;
    ECanaShadow.CANME.all &= ~((Uint32)1 << pRegs->MboxNum);//DISABLE
    ECanaRegs.CANME.all = ECanaShadow.CANME.all;

    Mailbox = &ECanaMboxes.MBOX0 + pRegs->MboxNum;

    Mailbox->MSGID.all = pRegs->MboxID | 0x80000000;   //扩展帧
//  Mailbox->MSGID.all = pRegs->MboxID << 18;          //标准帧


    Mailbox->MDL.word.LOW_WORD = pRegs->MboxWord1;
    Mailbox->MDL.word.HI_WORD = pRegs->MboxWord2;
    Mailbox->MDH.word.LOW_WORD = pRegs->MboxWord3;
    Mailbox->MDH.word.HI_WORD = pRegs->MboxWord4;

    Mailbox->MSGCTRL.all = pRegs->MboxDLC;

    ECanaShadow.CANME.all = ECanaRegs.CANME.all;
    ECanaShadow.CANME.all |= (Uint32)1 << pRegs->MboxNum;//ENABLE
    ECanaRegs.CANME.all = ECanaShadow.CANME.all;

    ECanaRegs.CANTRS.all = (Uint32)1 << pRegs->MboxNum;
    ECanaRegs.CANTA.all = (Uint32)1 << pRegs->MboxNum;
}
// MBXnbr 16-31
struct CANMBOX_USER_REGS ECanMboxRead(int16 MBXnbr)
{//Mailboxes 0-15 as Tx, 16-31 as Rx
    struct CANMBOX_USER_REGS CanMboxUserRegs;
    volatile struct MBOX *Mailbox;
    Mailbox = &ECanaMboxes.MBOX0 + MBXnbr;


    if ((ECanaRegs.CANRMP.all & ((Uint32)1<<MBXnbr))!=0 ) //if (ECanaRegs.CANRMP.bit.RMP20 ==1 )
    {
        CanMboxUserRegs.MboxRxFlg = TRUE;
        CanMboxUserRegs.MboxNum = MBXnbr;
        CanMboxUserRegs.MboxID = Mailbox->MSGID.all & 0x1FFFFFFF; //扩展帧
        CanMboxUserRegs.MboxDLC = Mailbox->MSGCTRL.bit.DLC;
        CanMboxUserRegs.MboxWord1 = Mailbox->MDL.word.LOW_WORD;
        CanMboxUserRegs.MboxWord2 = Mailbox->MDL.word.HI_WORD;
        CanMboxUserRegs.MboxWord3 = Mailbox->MDH.word.LOW_WORD;
        CanMboxUserRegs.MboxWord4 = Mailbox->MDH.word.HI_WORD;

        ECanaRegs.CANRMP.all = ECanaRegs.CANRMP.all|((Uint32)1<<MBXnbr);//写1 clear
    }
    else
    {
        CanMboxUserRegs.MboxRxFlg = FALSE;
    }
    return CanMboxUserRegs;
}


void CanDebugTx(void)
{// Configure Mailboxes 0-15 as Tx, 16-31 as Rx
    static int16 TxCnt = 0;
    static int16 TxPeriod = 0;
    static int16 step = 0;
    TxCnt++;

    if (TxCnt >= TxPeriod)
    {
        TxPeriod = TX_PERIOD*MAINLOOPRATE;// =TX_PERIOD/1000/(1/mainloop)  mainloop = 8k

       TxCnt = 0;
        step++;
        if(step ==1){
        //给定速度 目标速度 反馈速度 正反转
        CanMboxUserRegsT.MboxID = 0x11111303;
        CanMboxUserRegsT.MboxNum = 5;
        CanMboxUserRegsT.MboxDLC = 8;
        CanMboxUserRegsT.MboxWord1 = _IQtoIQ15(rc1.TargetValue);//Q15主控给的速度
        CanMboxUserRegsT.MboxWord2 = _IQtoIQ15(pid1_spd.Ref);//Q15 pid给定的速度
        CanMboxUserRegsT.MboxWord3 = _IQtoIQ15(pid1_spd.Fbk);//Q15 pid反馈速度
        CanMboxUserRegsT.MboxWord4 = _IQtoIQ12(pid1_spd.Ref)*speed1.BaseRpm>>12;//rpm PID给定速度

        ECanMboxSend(&CanMboxUserRegsT);
        }else if(step ==2){
        //速度PID输出  运行占空比 速度闭环 相位
        CanMboxUserRegsT.MboxID = 0x11111304;
        CanMboxUserRegsT.MboxNum = 6;
        CanMboxUserRegsT.MboxDLC = 8;
        CanMboxUserRegsT.MboxWord1 = speed1.SpeedRpm;//pid反馈速度 rpm
        CanMboxUserRegsT.MboxWord2 = _IQtoIQ15(pid1_spd.Out);//PID输出
        CanMboxUserRegsT.MboxWord3 = pwm1.DutyFunc;//占空比
        CanMboxUserRegsT.MboxWord4 = test4;//0-5

        ECanMboxSend(&CanMboxUserRegsT);
        }else if(step ==3){
        //是否触发过零 速度闭环标志 运行方向 0
        CanMboxUserRegsT.MboxID = 0x11111305;
        CanMboxUserRegsT.MboxNum = 7;
        CanMboxUserRegsT.MboxDLC = 8;
        CanMboxUserRegsT.MboxWord1 = pwmlimit;//过零点
        CanMboxUserRegsT.MboxWord2 = IsrTime;
        CanMboxUserRegsT.MboxWord3 = test2;
        CanMboxUserRegsT.MboxWord4 =0;

        ECanMboxSend(&CanMboxUserRegsT);
        }else if(step ==4){
        //数据内容：UA UB UC IDC
        CanMboxUserRegsT.MboxID = 0x11111306;
        CanMboxUserRegsT.MboxNum = 8;
        CanMboxUserRegsT.MboxDLC = 8;
        CanMboxUserRegsT.MboxWord1 = hall1.HallGpioAccepted;//放大10倍
        CanMboxUserRegsT.MboxWord2 = test4;
        CanMboxUserRegsT.MboxWord3 = test3;
        CanMboxUserRegsT.MboxWord4 = XintTime>>16;

        ECanMboxSend(&CanMboxUserRegsT);

        }else if(step ==5){
//速度PI 电流PI

        CanMboxUserRegsT.MboxID = 0x11111307;
        CanMboxUserRegsT.MboxNum = 9;
        CanMboxUserRegsT.MboxDLC = 8;
        CanMboxUserRegsT.MboxWord1 = (int16)(pid1_spd.Kp);
        CanMboxUserRegsT.MboxWord2 = (int16)(pid1_spd.Ki);
        CanMboxUserRegsT.MboxWord3 = (int16)(pid1_idc.Kp);
        CanMboxUserRegsT.MboxWord4 = (int16)(pid1_idc.Ki);

        ECanMboxSend(&CanMboxUserRegsT);
        }else if(step ==6){
//电流给定 电流反馈 平均电流 滤波后的平均电流
        CanMboxUserRegsT.MboxID = 0x11111308;
        CanMboxUserRegsT.MboxNum = 10;
        CanMboxUserRegsT.MboxDLC = 8;
        CanMboxUserRegsT.MboxWord1 = _IQtoIQ15(pid1_idc.Ref);//电流给定
        CanMboxUserRegsT.MboxWord2 = _IQtoIQ15(pid1_idc.Fbk);//电流反馈pid1_idc.Fbk
        CanMboxUserRegsT.MboxWord3 = (DC_current_avr*1000)>>12;
        CanMboxUserRegsT.MboxWord4 = (DC_current_filter_avr*1000)>>12;

        ECanMboxSend(&CanMboxUserRegsT);
        }else if(step ==7){
            step = 0;
        //
        CanMboxUserRegsT.MboxID = 0x11111309;
        CanMboxUserRegsT.MboxNum = 11;
        CanMboxUserRegsT.MboxDLC = 8;
        CanMboxUserRegsT.MboxWord1 = DCbus_voltage;//直流电压
        CanMboxUserRegsT.MboxWord2 = Tmotor;//电机温度
        CanMboxUserRegsT.MboxWord3 = Tvot;//电调温度
        CanMboxUserRegsT.MboxWord4 = FaultFlag.all;//故障

        ECanMboxSend(&CanMboxUserRegsT);
        }

    }
}
void carveData1(){
    //当发送标志位为0时，采集并记录模拟数据，但不发送
     if (CanCurveFlag == 0)
     {
         DebugData1(0,0);
         DebugData1(1,0);
         DebugData1(2,0);
         DebugData1(3,0);

         DebugData2(0,test2*10);
         DebugData2(1,speed1.SpeedRpm*10);
         DebugData2(2,0);
         DebugData2(3,0);
     }

}
void CanCurve(void)
{// Configure Mailboxes 0-15 as Tx, 16-31 as Rx
    static int16 i = 0;
    static int16 DubugCnt = 0;
    static int16 DubugPer = 0;


    //当数据大于发送间隔时，发送绘图数据
    DubugCnt++;
    if (DubugCnt >= DubugPer)
    {
        //发送间隔数据
        DubugPer = TX_PERIOD*MAINLOOPRATE;
        DubugCnt = 0;
        //当发送标志位为1时，发送绘图波形数据
        if (CanCurveFlag == 1)
        {
            CanMboxUserRegsT.MboxID = 0x1111130E;
            CanMboxUserRegsT.MboxNum = 14;
            CanMboxUserRegsT.MboxDLC = 8;
            CanMboxUserRegsT.MboxWord1 = CanCurveData[i];
            CanMboxUserRegsT.MboxWord2 = CanCurveData[i+0x100];
            CanMboxUserRegsT.MboxWord3 = CanCurveData[i+0x200];
            CanMboxUserRegsT.MboxWord4 = CanCurveData[i+0x300];
            ECanMboxSend(&CanMboxUserRegsT);

            CanMboxUserRegsT.MboxID = 0x1111130F;
            CanMboxUserRegsT.MboxNum = 15;
            CanMboxUserRegsT.MboxDLC = 8;
            CanMboxUserRegsT.MboxWord1 = CanCurveData2[i];
            CanMboxUserRegsT.MboxWord2 = CanCurveData2[i+0x100];
            CanMboxUserRegsT.MboxWord3 = CanCurveData2[i+0x200];
            CanMboxUserRegsT.MboxWord4 = CanCurveData2[i+0x300];
            ECanMboxSend(&CanMboxUserRegsT);

            i++;
            if (i >= 0x100)
            {
                i = 0;
                CanCurveFlag = 0;
            }

        }
    }

}



/**********采集并存储绘图数据1**********/
void DebugData1(int16 num,int16 data)
{

    static int16 cnt[4] ={0,0,0,0};
    CanCurveData[cnt[num]+ 0x100*num] = data;

    cnt[num]++;
    if (cnt[num] >= 0x100)
    {
        cnt[num]=0;
        CanCurveFlag = 1;
    }
}

/*********采集并存储绘图数据2**********/
void DebugData2(int16 num,int16 data)
{
    static int16 cnt[4] ={0,0,0,0};

    CanCurveData2[cnt[num]+0x100*num] = data;

    cnt[num]++;
    if (cnt[num] >= 0x100)
    {
        cnt[num] = 0;
    }
}

void CanReceive(void){
    struct CANMBOX_USER_REGS CanMboxTempRegs;


//CanDebugFlag  为真表示CAN通信正确

 //------------------------------调试上位机----------------------
     CanMboxTempRegs = ECanMboxRead(16);
     if (CanMboxTempRegs.MboxRxFlg == TRUE)
     {
          if (CanMboxTempRegs.MboxID == ID_DMC_DEBUG)
         {//握手 可以不握手
             if (CanMboxTempRegs.MboxWord1 == 0x5A5A && CanMboxTempRegs.MboxWord2 == 0x2013 &&
                 CanMboxTempRegs.MboxWord3 == 0x0218 && CanMboxTempRegs.MboxWord4 == 0xA5A5)
             {
                 CanDebugFlag = TRUE;
             }
             else
             {
                 CanDebugFlag = TRUE;//FALSE;
             }
         }

         else if (CanMboxTempRegs.MboxID == ID_DMC_SET || CanMboxTempRegs.MboxID == ID_DMC_CLEAR)
         {
             if (CanDebugFlag == TRUE)
             {
                 CanMboxUserRegsR2 = CanMboxTempRegs;//
             }
         }
         else if (CanMboxTempRegs.MboxID == ID_DMC_SET1 || CanMboxTempRegs.MboxID == ID_DMC_SET2\
                 ||CanMboxTempRegs.MboxID == ID_DMC_SET3 || CanMboxTempRegs.MboxID == ID_DMC_SET4|| CanMboxTempRegs.MboxID == ID_DMC_SET5)
         {
             if (CanDebugFlag == TRUE)
             {
                 CanMboxUserRegsR3 = CanMboxTempRegs;
             }
         }
     }

}

void CanMaster(void){
    CanReceive();
    CanDebugTx();
   // CanCurve();
}
void UserCANprocess(void){
    if (CanMboxUserRegsR2.MboxID == ID_DMC_SET)
    {
        EnableFlag = 1;
        if (CanMboxUserRegsR3.MboxID == ID_DMC_SET1)
           {

               SpeedRef = _IQdiv(CanMboxUserRegsR3.MboxWord1,speed1.BaseRpm);//速度给定
               //test2 = CanMboxUserRegsR3.MboxWord1,speed1.BaseRpm;
              /* if(CanMboxUserRegsR3.MboxWord1<0){
                   direction = Reverse;
               }else{
                   direction = Normal;
               }*/

           }
           else if (CanMboxUserRegsR3.MboxID == ID_DMC_SET2)
           {
              // CurrentSet = _IQ((float)CanMboxUserRegsR3.MboxWord1/1000);
               test = CanMboxUserRegsR3.MboxWord1;
           }
           else if (CanMboxUserRegsR3.MboxID == ID_DMC_SET3)
           {
           }

    }
    else if (CanMboxUserRegsR2.MboxID == ID_DMC_CLEAR)
    {
        SpeedRef = _IQabs(0.0);
        CurrentSet = _IQabs(0.0);
        EnableFlag = 0;
        if (CanMboxUserRegsR3.MboxID == ID_DMC_SET4)
       {
           pid1_spd.Kp = (int32)CanMboxUserRegsR3.MboxWord1;
           pid1_spd.Ki = (int32)(CanMboxUserRegsR3.MboxWord2);
           pid1_idc.Kp = (int32)(CanMboxUserRegsR3.MboxWord3);
           pid1_idc.Ki = (int32)(CanMboxUserRegsR3.MboxWord4);

       }

    }
    if (CanMboxUserRegsR3.MboxID == ID_DMC_SET5)
    {
       //
        Fault_clear = 1;
        CanMboxUserRegsR3.MboxID = 0;
    }

}