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

86
BCZT/user/MotorCtrl.c
View File

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