#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]); } }