RP-01/cva_asw_m0146/src/MotorCtrl.c

#include "MotorCtrl.h"
#include "hwctrl.h"
#include "scm_canmatrix-binutil.h"
#include "MemoryTask.h"
/*******************************************************************************
 * the defines
 ******************************************************************************/
#define START_FLAG  0x55aa
#define STOP_FLAG   0xaa55

#define AUTOCAL_STOP    0
#define AUTOCAL_START   1
#define AUTOCAL_ACT1    2
#define AUTOCAL_ACT2    3
#define AUTOCAL_END     4

#define HALLDELAYMAX 2
#define OC_DELAY_TIME   200
/*******************************************************************************
 * the typedefs
 ******************************************************************************/

typedef struct 
{
    uint16_t MotorStopLoc1;
    uint16_t MotorStopLoc2;
    uint16_t MotorNowLoc;
}MOTOR_DATA;//6 byte

typedef struct 
{
    uint16_t start_flag;//2
    MOTOR_DATA MotorData[6];// 6 * 6
    uint16_t MemoryLoc[3][6];//36
    uint16_t checksum;//2
    uint16_t stop_flag;//2
}MEMORY_DATA;

/*******************************************************************************
 * the globals
 ******************************************************************************/

static MOTOR_DATA MotorData[6];
static uint16_t MemoryLoc[3][6];
static MEMORY_DATA MemoryData;
static uint8_t OC1flag,OC2flag,OC3flag;
static uint16_t OC1Count,OC2Count,OC3Count;
static uint8_t MotorState[6],MotorStateReal[6];
static uint8_t MotorHallIO[6];
static uint16_t MotorHallLoc[6],MotorHardStop1[6],MotorHardStop2[6];
static uint8_t MotorLearnState[6];
static uint8_t MotorErr[6];
static uint16_t MotorTarget[6] = {0};
static uint16_t current1,current2,current3;
static uint8_t stopflag = 0;
static uint8_t AutoCalState;
static uint16_t HallErrorCount[6];
static uint8_t MotorMemoryReadState,MotorDataChangeFlag;
static uint16_t MotorDataChangeDelay;
/*******************************************************************************
 * the const
 ******************************************************************************/

/*******************************************************************************
 * the local function 
 ******************************************************************************/
static void MotorCtrl(void);
static void AutoCalCtrl(void);
static void MotorValueInit(void);

static uint16_t getOverCurrentTh(uint8_t ch);
static uint16_t GetCheckSum(uint16_t * addr,uint8_t len);
static void MotorMemoryDataUnpack(void);
static void MotorMemoryDataPack(void);
static void MotorDataChange(void);

/*******************************************************************************
 * the local functions
 ******************************************************************************/
static uint16_t GetCheckSum(uint16_t * addr,uint8_t len)
{
    uint16_t sum=0;
    while (len --)
    {
        sum += *addr;
    }
    sum ^= 0xffff;

    return sum;
}

static void MotorValueInit(void)
{
    uint8_t i;
    for (i = 0; i < 6; i++)
    {
        MotorHallLoc[i] = 0x8000;
        MotorErr[i] = 0;
    }
    MemoryTask_ReqRead(FEE_BLOCK_MOTORDATA_DATASET0,(uint8_t *)&MemoryData,sizeof(MemoryData));
    MotorMemoryReadState = 0;
}

static void MotorMemoryDataUnpack(void)
{
    uint8_t i;
    uint16_t checksum;
    checksum = GetCheckSum(&MemoryData.MotorData[0].MotorStopLoc1,72);
    if (MemoryData.start_flag == START_FLAG && MemoryData.stop_flag == STOP_FLAG && checksum == MemoryData.checksum)//
    {
        for (i = 0; i < 6; i++)
        {
            MotorHallLoc[i] = MemoryData.MotorData[i].MotorNowLoc;
            MotorHardStop1[i] = MemoryData.MotorData[i].MotorStopLoc1;
            MotorHardStop2[i] = MemoryData.MotorData[i].MotorStopLoc2;
            MemoryLoc[0][i] = MemoryData.MemoryLoc[0][i];
            MemoryLoc[1][i] = MemoryData.MemoryLoc[1][i];
            MemoryLoc[2][i] = MemoryData.MemoryLoc[2][i];
        }
        
    }
    else
    {
        for (i = 0; i < 6; i++)
        {
            MotorHallLoc[i] = 0x8000;
            MotorHardStop1[i] = 0;
            MotorHardStop2[i] = 0;
            MemoryLoc[0][i] = 0;
            MemoryLoc[1][i] = 0;
            MemoryLoc[2][i] = 0;
        }
        
    }
}

static void MotorMemoryDataPack(void)
{
    uint8_t i;

    MemoryData.start_flag = START_FLAG;
    MemoryData.stop_flag = STOP_FLAG;
    for (i = 0; i < 6; i++)
    {
        MemoryData.MotorData[i].MotorNowLoc = MotorHallLoc[i];
        MemoryData.MotorData[i].MotorStopLoc1 = MotorHardStop1[i];
        MemoryData.MotorData[i].MotorStopLoc2 = MotorHardStop2[i];
        MemoryData.MemoryLoc[0][i] = MemoryLoc[0][i];
        MemoryData.MemoryLoc[1][i] = MemoryLoc[1][i];
        MemoryData.MemoryLoc[2][i] = MemoryLoc[2][i];
    }
    MemoryData.checksum = GetCheckSum(&MemoryData.MotorData[0].MotorStopLoc1,72);
}

static void MotorMemoryTask(void)//10ms
{
    FEERWSTATE_t ret;
    if (MotorMemoryReadState == 0)
    {
        ret = MemoryTask_getReadState();
        if (ret == FEERWSTATE_FINISHED)
        {
            MotorMemoryDataUnpack();
            MotorMemoryReadState = 1;
        }
        else if (ret == FEERWSTATE_ERROR)
        {
            MotorMemoryReadState = 1;
            MotorDataChange();
        }
    }
    
    if (MotorDataChangeFlag > 0 )
    {
        if (MotorDataChangeDelay > 0)
        {
            MotorDataChangeDelay--;
        }
        else
        {
            MotorDataChangeDelay = 0;
            MotorDataChangeFlag = 0;
            MotorMemoryDataPack();
            MemoryTask_ReqWrite(FEE_BLOCK_MOTORDATA_DATASET0,(uint8_t *)&MemoryData,sizeof(MemoryData));
        }
        
    }
    
    
}

static void AutoCalCtrl(void)
{
    static uint16_t autocalcounter[3];
    static uint8_t MotorArr1state,MotorArr2state,MotorArr3state;
    static uint8_t wait1,wait2,wait3;
    uint8_t i;
    uint32_t temp;
    switch (AutoCalState)
    {
    case AUTOCAL_STOP:
        MotorArr1state = AUTOCAL_STOP;
        MotorArr2state = AUTOCAL_STOP;
        MotorArr3state = AUTOCAL_STOP;
        return;
    case AUTOCAL_START:
        MotorArr1state = 1;
        MotorArr2state = 1;
        MotorArr3state = 1;
        setMotorState(MotorHG,Motor_ACT_CW);
        setMotorState(MotorTT,Motor_ACT_CW);
        autocalcounter[0] = 0;
        autocalcounter[1] = 0;
        autocalcounter[2] = 0;
        AutoCalState++;
        for (i = 0; i < 4; i++)
        {
            MotorHardStop1[i] = 0;
            MotorHardStop2[i] = 0;
            MotorHallLoc[i] = 0x8000;
        }
        break;
    case AUTOCAL_ACT1:
        if (MotorArr1state == 0 && MotorArr2state == 0)
        {
            for (i = 0; i < 6; i++)
            {
                temp = MotorHardStop1[i];
                temp += MotorHardStop2[i];
                setMotorTarget(i,temp/2);
            }
            
        }
        
        
        
        break;
    case AUTOCAL_ACT2:

        break;
    
    default:
        break;
    }
    //1
    switch (MotorArr1state)
    {
    case 1://Motor1 xq
        autocalcounter[0]++;
        if (MotorHardStop1[MotorHG] != 0)
        {
            MotorArr1state++;
            wait1 = 0;
            setMotorState(MotorHG,Motor_ACT_NOACT);
        }
        if (autocalcounter[0] > 3000 || MotorErr[MotorHG] != 0)
        {
            MotorArr1state = 4;
            wait1 = 0;
            setMotorState(MotorHG,Motor_ACT_NOACT);
        }
        break;
    case 2://wait
        wait1++;
        if (wait1 > 50)
        {
            MotorArr1state++;
            setMotorState(MotorHG,Motor_ACT_CCW);
            autocalcounter[0] = 0;
        }
        break;
    case 3://Motor1 xh 
        autocalcounter[0]++;
        if (MotorHardStop2[MotorHG] != 0)
        {
            MotorArr1state++;
            wait1 = 0;
            setMotorState(MotorHG,Motor_ACT_NOACT);
        }
        if (autocalcounter[0] > 3000 || MotorErr[MotorHG] != 0)
        {
            MotorArr1state = 4;
            wait1 = 0;
            setMotorState(MotorHG,Motor_ACT_NOACT);
        }
        break;
    case 4://wait
        wait1++;
        if (wait1 > 50)
        {
            MotorArr1state++;
            setMotorState(MotorKB,Motor_ACT_CW);
            autocalcounter[0] = 0;
        }
        break;
    case 5://Motor2 xq
        autocalcounter[0]++;
        if (MotorHardStop1[MotorKB] != 0)
        {
            MotorArr1state++;
            wait1 = 0;
            setMotorState(MotorKB,Motor_ACT_NOACT);
        }
        if (autocalcounter[0] > 3000 || MotorErr[MotorKB] != 0)
        {
            MotorArr1state = 0;
            setMotorState(MotorKB,Motor_ACT_NOACT);
        }
        break;
    case 6://wait
        wait1++;
        if (wait1 > 50)
        {
            MotorArr1state++;
            setMotorState(MotorKB,Motor_ACT_CCW);
            autocalcounter[0] = 0;
        }
        break;
    case 7:
        autocalcounter[0]++;
        if (MotorHardStop2[MotorKB] != 0)
        {
            MotorArr1state=0;
            wait1 = 0;
            setMotorState(MotorKB,Motor_ACT_NOACT);
        }
        if (autocalcounter[0] > 3000 || MotorErr[MotorKB] != 0)
        {
            MotorArr1state = 0;
            setMotorState(MotorKB,Motor_ACT_NOACT);
        }
        break;
    default:
        break;
    }
    
    //2
     switch (MotorArr2state)
    {
    case 1://Motor3 xq
        autocalcounter[1]++;
        if (MotorHardStop1[MotorTT] != 0)
        {
            MotorArr2state++;
            wait2 = 0;
	    setMotorState(MotorTT,Motor_ACT_NOACT);	
        }
        if (autocalcounter[1] > 3000 || MotorErr[MotorTT] != 0)
        {
            MotorArr2state = 4;
            wait2 = 0;
            setMotorState(MotorTT,Motor_ACT_NOACT);
        }
        break;
    case 2://wait
        wait2++;
        if (wait2 > 50)
        {
            MotorArr2state++;
            setMotorState(MotorTT,Motor_ACT_CCW);
            autocalcounter[1] = 0;
        }
        break;
    case 3://Motor3 xh 
        autocalcounter[1]++;
        if (MotorHardStop2[MotorTT] != 0)
        {
            MotorArr2state++;
            wait2 = 0;
            setMotorState(MotorTT,Motor_ACT_NOACT);
        }
        if (autocalcounter[1] > 3000 || MotorErr[MotorTT] != 0)
        {
            MotorArr2state = 4;
            wait2 = 0;
            setMotorState(MotorTT,Motor_ACT_NOACT);
        }
        break;
    case 4://wait
        wait2++;
        if (wait2 > 50)
        {
	        MotorArr2state++;
            setMotorState(MotorZY,Motor_ACT_CW);
            autocalcounter[1] = 0;
        }
        break;
    case 5://Motor4 xq
        autocalcounter[1]++;
        if (MotorHardStop1[MotorZY] != 0)
        {
            MotorArr2state++;
            wait2 = 0;
            setMotorState(MotorZY,Motor_ACT_NOACT);
        }
        if (autocalcounter[1] > 3000 || MotorErr[MotorZY] != 0)
        {
            MotorArr2state = 0;
            setMotorState(MotorZY,Motor_ACT_NOACT);
        }
        break;
    case 6://wait
        wait2++;
        if (wait2 > 50)
        {
            MotorArr2state++;
            setMotorState(MotorZY,Motor_ACT_CCW);
            autocalcounter[1] = 0;
        }
        break;
    case 7:
        autocalcounter[1]++;
        if (MotorHardStop2[MotorZY] != 0)
        {
            MotorArr2state=0;
            wait2 = 0;
            setMotorState(MotorZY,Motor_ACT_NOACT);
        }
        if (autocalcounter[1] > 3000 || MotorErr[MotorZY] != 0)
        {
            MotorArr2state = 0;
            setMotorState(MotorZY,Motor_ACT_NOACT);
        }
        break;
    default:
        break;
    }
    
}

static void MotorCtrl(void)//10ms
{
    uint8_t i;
    MotorMemoryTask(); //TODO
    AutoCalCtrl();
    
    
    if (OC1flag == 1)
    {
        OC1flag = 0;
        if (MotorState[0] == Motor_ACT_CW)
        {
            MotorHardStop1[0] = MotorHallLoc[0];
        }
        else if (MotorState[0] == Motor_ACT_CCW)
        {
            MotorHardStop2[0] = MotorHallLoc[0];
        }
        else if (MotorState[1] == Motor_ACT_CW)
        {
            MotorHardStop1[1] = MotorHallLoc[1];
        }
        else if (MotorState[1] == Motor_ACT_CCW)
        {
            MotorHardStop2[1] = MotorHallLoc[1];
        }
        setMotorTarget(MotorHG,0);
        setMotorTarget(MotorKB,0);
        MotorState[MotorHG] = Motor_ACT_NOACT;
        MotorState[MotorKB] = Motor_ACT_NOACT;
    }
    if (OC2flag == 1)
    {
        OC2flag = 0;
        if (MotorState[2] == Motor_ACT_CW)
        {
            MotorHardStop1[2] = MotorHallLoc[2];
        }
        else if (MotorState[2] == Motor_ACT_CCW)
        {
            MotorHardStop2[2] = MotorHallLoc[2];
        }
        else if (MotorState[3] == Motor_ACT_CW)
        {
            MotorHardStop1[3] = MotorHallLoc[3];
        }
        else if (MotorState[3] == Motor_ACT_CCW)
        {
            MotorHardStop2[3] = MotorHallLoc[3];
        }
        setMotorTarget(MotorTT,0);
        setMotorTarget(MotorZY,0);
        MotorState[MotorTT] = Motor_ACT_NOACT;
        MotorState[MotorZY] = Motor_ACT_NOACT;
    }
    if (OC3flag == 1)
    {
        OC3flag = 0;
        if (MotorState[4] == Motor_ACT_CW)
        {
            MotorHardStop1[4] = MotorHallLoc[4];
        }
        else if (MotorState[4] == Motor_ACT_CCW)
        {
            MotorHardStop2[4] = MotorHallLoc[4];
        }
        else if (MotorState[5] == Motor_ACT_CW)
        {
            MotorHardStop1[5] = MotorHallLoc[5];
        }
        else if (MotorState[5] == Motor_ACT_CCW)
        {
            MotorHardStop2[5] = MotorHallLoc[5];
        }
        setMotorTarget(Motor5,0);
        setMotorTarget(Motor6,0);
        MotorState[Motor5] = Motor_ACT_NOACT;
        MotorState[Motor6] = Motor_ACT_NOACT;
    }

    for (i = 0; i < 6; i++)
    {
        MotorStateReal[i] = MotorState[i];
        if (MotorState[i] == Motor_ACT_NOACT  && MotorHardStop1[i] != 0 && MotorHardStop2[i] != 0 )
        {
            if (MotorTarget[i]!=0 && MotorHardStop1[i] > MotorTarget[i] && MotorTarget[i] > MotorHardStop2[i])
            {
                if (MotorTarget[i] > MotorHallLoc[i]+10)
                {
                    MotorStateReal[i] = Motor_ACT_CW;
                }
                else if (MotorTarget[i] < MotorHallLoc[i]-10)
                {
                    MotorStateReal[i] = Motor_ACT_CCW;
                }
                else
                {
                    MotorTarget[i] = 0;
                }
            }
            
        }
        else if (MotorHardStop1[i] != 0 && MotorHardStop2[i] != 0)
        {
            if (MotorHallLoc[i] > (MotorHardStop1[i]-20) && MotorStateReal[i] == Motor_ACT_CW)
            {
                MotorStateReal[i] = Motor_ACT_NOACT;
            }
            if (MotorHallLoc[i] < (MotorHardStop2[i] + 20) && MotorStateReal[i] == Motor_ACT_CCW)
            {
                MotorStateReal[i] = Motor_ACT_NOACT;
            }
            
        }
        
        else
        {
            MotorTarget[i] = 0;
        }
    }
    
    
}

static void MotorDataChange(void)
{
    MotorDataChangeFlag = 1;
    MotorDataChangeDelay = 100;
}

static void SetMotorMsg(void)
{
    scm_canmatrix_tx.SCM_STATE.MOTOR_HG_STATE = MotorState[MotorHG];
    scm_canmatrix_tx.SCM_STATE.MOTOR_KB_STATE = MotorState[MotorKB];
    scm_canmatrix_tx.SCM_STATE.MOTOR_TT_STATE = MotorState[MotorTT];
    scm_canmatrix_tx.SCM_STATE.MOTOR_ZY_STATE = MotorState[MotorZY];

    scm_canmatrix_tx.SCM_DEBUG1.DEBUG_MOTOR_HG_STATUS  = MotorStateReal[MotorHG];
    scm_canmatrix_tx.SCM_DEBUG2.DEBUG_MOTOR_KB_STATUS  = MotorStateReal[MotorKB];
    scm_canmatrix_tx.SCM_DEBUG3.DEBUG_MOTOR_TT_STATUS  = MotorStateReal[MotorTT];
    scm_canmatrix_tx.SCM_DEBUG4.DEBUG_MOTOR_ZY_STATUS  = MotorStateReal[MotorZY];

    scm_canmatrix_tx.SCM_DEBUG1.DEBUG_MOTOR_HG_LOC = MotorHallLoc[MotorHG];
    scm_canmatrix_tx.SCM_DEBUG2.DEBUG_MOTOR_KB_LOC = MotorHallLoc[MotorKB];
    scm_canmatrix_tx.SCM_DEBUG3.DEBUG_MOTOR_TT_LOC = MotorHallLoc[MotorTT];
    scm_canmatrix_tx.SCM_DEBUG4.DEBUG_MOTOR_ZY_LOC = MotorHallLoc[MotorZY];
    scm_canmatrix_tx.SCM_DEBUG1.DEBUG_MOTOR_HG_STOP_1 = MotorHardStop1[MotorHG];
    scm_canmatrix_tx.SCM_DEBUG1.DEBUG_MOTOR_HG_STOP_2 = MotorHardStop2[MotorHG];
    scm_canmatrix_tx.SCM_DEBUG2.DEBUG_MOTOR_KB_STOP_1 = MotorHardStop1[MotorKB];
    scm_canmatrix_tx.SCM_DEBUG2.DEBUG_MOTOR_KB_STOP_2 = MotorHardStop2[MotorKB];
    scm_canmatrix_tx.SCM_DEBUG3.DEBUG_MOTOR_TT_STOP_1 = MotorHardStop1[MotorTT];
    scm_canmatrix_tx.SCM_DEBUG3.DEBUG_MOTOR_TT_STOP_2 = MotorHardStop2[MotorTT];
    scm_canmatrix_tx.SCM_DEBUG4.DEBUG_MOTOR_ZY_STOP_1 = MotorHardStop1[MotorZY];
    scm_canmatrix_tx.SCM_DEBUG4.DEBUG_MOTOR_ZY_STOP_2 = MotorHardStop2[MotorZY];

    scm_canmatrix_tx.SCM_DEBUG1.DEBUG_MOTOR_HG_HALLIO = MotorHallIO[MotorHG];
    scm_canmatrix_tx.SCM_DEBUG2.DEBUG_MOTOR_KB_HALLIO = MotorHallIO[MotorKB];
    scm_canmatrix_tx.SCM_DEBUG3.DEBUG_MOTOR_TT_HALLIO = MotorHallIO[MotorTT];
    scm_canmatrix_tx.SCM_DEBUG4.DEBUG_MOTOR_ZY_HALLIO = MotorHallIO[MotorZY];

    //scm_canmatrix_tx.SCM_STATE.ZY_HEAT_STATE = AutoCalState;
}

/*******************************************************************************
 * the global functions
 ******************************************************************************/
void MotorCtrl_Init(void)
{
    for (uint8_t i = 0; i < 6; i++)
    {
        hw_MotorCtrl(i, Motor_ACT_NOACT);
    }
    MotorValueInit();
}


void MotorCtrl_Maintask(void)//10ms task
{
    
    MotorCtrl();
    
    for (Motor_ID_Type i = 0; i < MOTOR_NUM; i++)
    {
        hw_MotorCtrl( i, MotorStateReal[i]);
    }
    SetMotorMsg();
}

void setMotorState(Motor_ID_Type motorid,Motor_ACT_Type act)
{
    if (act < Motor_ACT_NUM && motorid < MOTOR_NUM)
    {
        MotorState[motorid] = act;
        
    }
}

void setMotorTarget(uint8_t motorid,uint16_t target)
{
    if (MotorHardStop1[motorid]!=0 && MotorHardStop2[motorid]!=0)
    {
        MotorTarget[motorid] = target;
    }
    else
    {
        MotorTarget[motorid] = 0;
    }
}

void StartAutoCal(void)
{
    AutoCalState = AUTOCAL_START;
    
}
void StopAutoCal(void)
{
    uint8_t i;
    if (AutoCalState != AUTOCAL_STOP)
    {
        AutoCalState = AUTOCAL_STOP;
        setMotorState(MotorHG,Motor_ACT_NOACT);
        setMotorState(MotorKB,Motor_ACT_NOACT);
        setMotorState(MotorTT,Motor_ACT_NOACT);
        setMotorState(MotorZY,Motor_ACT_NOACT);
    }
    for (i = 0; i < 4; i++)
    {
        if (MotorTarget[i] != 0)
        {
            MotorTarget[i] = 0;
            stopflag = 1;
        }
        
    }
    
}


void CurrentDetect(void)
{
    uint32_t oc_th,current;
    oc_th = getOverCurrentTh(0);
    current = GetAdcmv(ADCH_RLY1)/10;
    if (current > oc_th)
    {
        OC1Count++;
        if (OC1Count > OC_DELAY_TIME)
        {
            OC1Count = 0;
            OC1flag = 1;
        }
    }
    else
    {
        OC1Count = 0;
    }

    oc_th = getOverCurrentTh(1);
    current = GetAdcmv(ADCH_RLY3)/10;
    if (current > oc_th)
    {
        OC2Count++;
        if (OC2Count > OC_DELAY_TIME)
        {
            OC2Count = 0;
            OC2flag = 1;
        }
    }
    else
    {
        OC2Count = 0;
    }
    
}



static uint16_t getOverCurrentTh(uint8_t ch)
{
    uint16_t th = 50;
    switch (ch)
    {
    case 0:
        if (MotorStateReal[MotorHG]!=Motor_ACT_NOACT)
        {
            th+=100;
        }
        if (MotorStateReal[MotorKB]!=Motor_ACT_NOACT)
        {
            th+=100;
        }
        break;
    case 1:
        if (MotorStateReal[MotorTT]!=Motor_ACT_NOACT)
        {
            th+=100;
        }
        if (MotorStateReal[MotorZY]!=Motor_ACT_NOACT)
        {
            th+=100;
        }
        break;
    case 2:
        if (MotorStateReal[Motor5]!=Motor_ACT_NOACT)
        {
            th+=100;
        }
        if (MotorStateReal[Motor6]!=Motor_ACT_NOACT)
        {
            th+=100;
        }
        break;
    default:
        break;
    }
    return th;
}

void HallDetect(void)
{
    static uint8_t HallLastState[6],HallDelay[6];
    uint8_t i,hallstate;
    for (i = 0; i < 6; i++)
    {
        hallstate = GetHallIO(i);
        MotorHallIO[i] = hallstate;
        if (hallstate != HallLastState[i])
        {
            HallDelay[i]++;
            if (HallDelay[i] > HALLDELAYMAX)
            {
                if (MotorStateReal[i] == Motor_ACT_CW)
                {
                    MotorHallLoc[i]++;
                }
                else if (MotorStateReal[i] == Motor_ACT_CCW)
                {
                    MotorHallLoc[i]--;
                }
                HallLastState[i] = hallstate;
            }
            HallErrorCount[i] = 0;
            MotorErr[i] = 0;
            MotorDataChange();
        }
        else
        {
            HallDelay[i] = 0;
            if (MotorStateReal[i] != Motor_ACT_NOACT)
            {
                HallErrorCount[i]++;
                if (HallErrorCount[i] > 500)
                {
                    HallErrorCount[i] = 500;
                    MotorErr[i] = 1;
                }
                
            }
            
        }
    }
}


void MotorMemorySet(MOTOR_MEMORY_Type id)
{
    if (id >= 3)
    {
        return;
    }
    for (uint8_t i = 0; i < 6; i++)
    {
        if (MotorHardStop1[i]!=0 && MotorHardStop2[i]!=0)
        {
            MemoryLoc[id][i] = MotorHallLoc[i];
        }
    }
    MotorDataChange();
}

void MotorMemoryGet(MOTOR_MEMORY_Type id)
{
    if (id >= 3)
    {
        return;
    }
    for (uint8_t i = 0; i < 6; i++)
    {
        setMotorTarget(i,MemoryLoc[id][i]);
    }
}