huahua/BCZT_NEW
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

改为一个电机动作

Browse Source
This commit is contained in:
sunbeam 2024-08-15 19:34:01 +08:00
parent 9deb904522
commit 14c3b38c04
1 changed files with 61 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

84
BCZT/user/MotorCtrl.c
View File

@ -212,8 +212,8 @@ void AutoCalCtrl(void)
MotorArr2state = 1;
MotorArr3state = 1;
setMotorState(MOTOR1,ACT_XQ);
setMotorState(MOTOR3,ACT_XQ);
setMotorState(MOTOR5,ACT_XQ);
//setMotorState(MOTOR3,ACT_XQ);
//setMotorState(MOTOR5,ACT_XQ);
autocalcounter[0] = 0;
autocalcounter[1] = 0;
autocalcounter[2] = 0;
@ -284,7 +284,7 @@ void AutoCalCtrl(void)
}
if (autocalcounter[0] > 3000 || MotorErr[MOTOR1] != 0)
{
MotorArr1state = 4;
MotorArr1state++;
wait1 = 0;
setMotorState(MOTOR1,ACT_NOACT);
}
@ -342,7 +342,23 @@ void AutoCalCtrl(void)
//2
switch (MotorArr2state)
{
case 1://motor3 xq
case 1:
if (MotorArr1state == 0)
{
wait2++;
if (wait2 > 50)
{
wait2 = 0;
MotorArr2state++;
setMotorState(MOTOR3,ACT_XQ);
}
}
else
{
wait2 = 0;
}
break;
case 2://motor3 xq
autocalcounter[1]++;
if (MotorHardStop1[MOTOR3] != 0)
{
@ -352,12 +368,12 @@ void AutoCalCtrl(void)
}
if (autocalcounter[1] > 3000 || MotorErr[MOTOR3] != 0)
{
MotorArr2state = 4;
MotorArr2state = 5;
wait2 = 0;
setMotorState(MOTOR3,ACT_NOACT);
}
break;
case 2://wait
case 3://wait
wait2++;
if (wait2 > 50)
{
@ -366,7 +382,7 @@ void AutoCalCtrl(void)
autocalcounter[1] = 0;
}
break;
case 3://motor3 xh
case 4://motor3 xh
autocalcounter[1]++;
if (MotorHardStop2[MOTOR3] != 0)
{
@ -376,12 +392,12 @@ void AutoCalCtrl(void)
}
if (autocalcounter[1] > 3000 || MotorErr[MOTOR3] != 0)
{
MotorArr2state = 4;
MotorArr2state++;
wait2 = 0;
setMotorState(MOTOR3,ACT_NOACT);
}
break;
case 4://wait
case 5://wait
wait2++;
if (wait2 > 50)
{
@ -390,7 +406,7 @@ void AutoCalCtrl(void)
autocalcounter[1] = 0;
}
break;
case 5://motor4 xq
case 6://motor4 xq
autocalcounter[1]++;
if (MotorHardStop1[MOTOR4] != 0)
{
@ -404,7 +420,7 @@ void AutoCalCtrl(void)
setMotorState(MOTOR4,ACT_NOACT);
}
break;
case 6://wait
case 7://wait
wait2++;
if (wait2 > 50)
{
@ -413,7 +429,7 @@ void AutoCalCtrl(void)
autocalcounter[1] = 0;
}
break;
case 7:
case 8:
autocalcounter[1]++;
if (MotorHardStop2[MOTOR4] != 0)
{
@ -432,7 +448,23 @@ void AutoCalCtrl(void)
}
switch (MotorArr3state)
{
case 1://motor5 xq
case 1:
if (MotorArr2state == 0)
{
wait3++;
if (wait3 > 50)
{
wait3 = 0;
MotorArr3state++;
setMotorState(MOTOR5,ACT_XQ);
}
}
else
{
wait3 = 0;
}
break;
case 2://motor5 xq
autocalcounter[2]++;
if (MotorHardStop1[MOTOR5] != 0)
{
@ -442,12 +474,12 @@ void AutoCalCtrl(void)
}
if (autocalcounter[2] > 3000 || MotorErr[MOTOR5] != 0)
{
MotorArr3state = 4;
MotorArr3state = 5;
wait3 = 0;
setMotorState(MOTOR5,ACT_NOACT);
}
break;
case 2://wait
case 3://wait
wait3++;
if (wait3 > 50)
{
@ -456,7 +488,7 @@ void AutoCalCtrl(void)
autocalcounter[2] = 0;
}
break;
case 3://motor5 xh
case 4://motor5 xh
autocalcounter[2]++;
if (MotorHardStop2[MOTOR5] != 0)
{
@ -466,12 +498,12 @@ void AutoCalCtrl(void)
}
if (autocalcounter[2] > 3000 || MotorErr[MOTOR5] != 0)
{
MotorArr3state = 4;
MotorArr3state = 5;
wait3 = 0;
setMotorState(MOTOR5,ACT_NOACT);
}
break;
case 4://wait
case 5://wait
wait3++;
if (wait3 > 50)
{
@ -480,7 +512,7 @@ void AutoCalCtrl(void)
autocalcounter[2] = 0;
}
break;
case 5://motor6 xq
case 6://motor6 xq
autocalcounter[2]++;
if (MotorHardStop1[MOTOR6] != 0)
{
@ -494,7 +526,7 @@ void AutoCalCtrl(void)
setMotorState(MOTOR6,ACT_NOACT);
}
break;
case 6://wait
case 7://wait
wait3++;
if (wait3 > 50)
{
@ -503,7 +535,7 @@ void AutoCalCtrl(void)
autocalcounter[2] = 0;
}
break;
case 7:
case 8:
autocalcounter[2]++;
if (MotorHardStop2[MOTOR6] != 0)
{
@ -759,10 +791,14 @@ void MotorCtrl(void)//10ms
MotorState[MOTOR5] = ACT_NOACT;
MotorState[MOTOR6] = ACT_NOACT;
}
for (i = 0; i < 6; i++)
{
MotorStateReal[i] = MotorState[i];
}
for (i = 0; i < 6; i++)
{
if (MotorState[i] == ACT_NOACT && MotorHardStop1[i] != 0 && MotorHardStop2[i] != 0 )
{
if (MotorTarget[i]!=0 && MotorHardStop1[i] > MotorTarget[i] && MotorTarget[i] > MotorHardStop2[i])
@ -770,10 +806,12 @@ void MotorCtrl(void)//10ms
if (MotorTarget[i] > MotorHallLoc[i]+10)
{
MotorStateReal[i] = ACT_XQ;
break;
}
else if (MotorTarget[i] < MotorHallLoc[i]-10)
{
MotorStateReal[i] = ACT_XH;
break;
}
else
{
@ -792,9 +830,7 @@ void MotorCtrl(void)//10ms
{
MotorStateReal[i] = ACT_NOACT;
}
}
else
{
MotorTarget[i] = 0;