diff --git a/BCZT/user/MotorCtrl.c b/BCZT/user/MotorCtrl.c index de793d9..6e5c86c 100644 --- a/BCZT/user/MotorCtrl.c +++ b/BCZT/user/MotorCtrl.c @@ -141,7 +141,7 @@ void setMotorState(uint8_t motorid,uint8_t act) uint16_t MotorTarget[6] = {0}; void setMotorTarget(uint8_t motorid,uint16_t target) { - if (MotorHardStop1[motorid]!=0 && MotorHardStop2[motorid]!=0) + if (MotorHardStop1[motorid]!=0 && MotorHardStop2[motorid]!=0 && MotorErr[motorid] != 0) { MotorTarget[motorid] = target; } @@ -765,6 +765,11 @@ void MotorCtrl(void)//10ms MotorStateReal[i] = MotorState[i]; if (MotorState[i] == ACT_NOACT && MotorHardStop1[i] != 0 && MotorHardStop2[i] != 0 ) { + if (MotorErr[i] != 0) + { + MotorTarget[i] = 0; + } + if (MotorTarget[i]!=0 && MotorHardStop1[i] > MotorTarget[i] && MotorTarget[i] > MotorHardStop2[i]) { if (MotorTarget[i] > MotorHallLoc[i]+10) @@ -970,6 +975,7 @@ void HallDetecte(void) { HallErrorCount[i] = 500; MotorErr[i] = 1; + MotorTarget[i] = 0; } }