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

删除一个电机

Browse Source
This commit is contained in:
sunbeam 2024-10-24 08:29:57 +08:00
parent ed235dba4a
commit 7caeaa4dcd
5 changed files with 30 additions and 270 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

248
app/user/MotorCtrl.c
View File

@ -195,259 +195,19 @@ void FW_act(void)
{
setMotorTarget(MOTOR1,MotorHardStop1[MOTOR1]-100);
setMotorTarget(MOTOR2,MotorHardStop1[MOTOR2]-100);
setMotorTarget(MOTOR3,MotorHardStop2[MOTOR3]+100);
setMotorTarget(MOTOR4,MotorHardStop1[MOTOR4]-100);
setMotorTarget(MOTOR3,MotorHardStop1[MOTOR3]-100);
}
void TP_act(void)
{
setMotorTarget(MOTOR1,MotorHardStop2[MOTOR1]+500);//kb
setMotorTarget(MOTOR2,MotorHardStop2[MOTOR2]+100);//hg
setMotorTarget(MOTOR3,MotorHardStop1[MOTOR3]-100);//zd
setMotorTarget(MOTOR4,MotorHardStop2[MOTOR4]+100);//tt
setMotorTarget(MOTOR3,MotorHardStop2[MOTOR3]+100);//tt
}
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(MOTOR1,ACT_XQ);
setMotorState(MOTOR3,ACT_XQ);
//setMotorState(MOTOR5,ACT_XQ);
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 && MotorArr3state == 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[MOTOR1] != 0)
{
MotorArr1state++;
wait1 = 0;
setMotorState(MOTOR1,ACT_NOACT);
}
if (autocalcounter[0] > 3000 || MotorErr[MOTOR1] != 0)
{
MotorArr1state = 4;
wait1 = 0;
setMotorState(MOTOR1,ACT_NOACT);
}
break;
case 2://wait
wait1++;
if (wait1 > 50)
{
MotorArr1state++;
setMotorState(MOTOR1,ACT_XH);
autocalcounter[0] = 0;
}
break;
case 3://motor1 xh
autocalcounter[0]++;
if (MotorHardStop2[MOTOR1] != 0)
{
MotorArr1state++;
wait1 = 0;
setMotorState(MOTOR1,ACT_NOACT);
}
if (autocalcounter[0] > 3000 || MotorErr[MOTOR1] != 0)
{
MotorArr1state = 4;
wait1 = 0;
setMotorState(MOTOR1,ACT_NOACT);
}
break;
case 4://wait
wait1++;
if (wait1 > 50)
{
MotorArr1state++;
setMotorState(MOTOR2,ACT_XQ);
autocalcounter[0] = 0;
}
break;
case 5://motor2 xq
autocalcounter[0]++;
if (MotorHardStop1[MOTOR2] != 0)
{
MotorArr1state++;
wait1 = 0;
setMotorState(MOTOR2,ACT_NOACT);
}
if (autocalcounter[0] > 3000 || MotorErr[MOTOR2] != 0)
{
MotorArr1state = 0;
setMotorState(MOTOR2,ACT_NOACT);
}
break;
case 6://wait
wait1++;
if (wait1 > 50)
{
MotorArr1state++;
setMotorState(MOTOR2,ACT_XH);
autocalcounter[0] = 0;
}
break;
case 7:
autocalcounter[0]++;
if (MotorHardStop2[MOTOR2] != 0)
{
MotorArr1state=0;
wait1 = 0;
setMotorState(MOTOR2,ACT_NOACT);
}
if (autocalcounter[0] > 3000 || MotorErr[MOTOR2] != 0)
{
MotorArr1state = 0;
setMotorState(MOTOR2,ACT_NOACT);
}
break;
default:
break;
}
//2
switch (MotorArr2state)
{
case 1://motor3 xq
autocalcounter[1]++;
if (MotorHardStop1[MOTOR3] != 0)
{
MotorArr2state++;
wait2 = 0;
setMotorState(MOTOR3,ACT_NOACT);
}
if (autocalcounter[1] > 3000 || MotorErr[MOTOR3] != 0)
{
MotorArr2state = 4;
wait2 = 0;
setMotorState(MOTOR3,ACT_NOACT);
}
break;
case 2://wait
wait2++;
if (wait2 > 50)
{
MotorArr2state++;
setMotorState(MOTOR3,ACT_XH);
autocalcounter[1] = 0;
}
break;
case 3://motor3 xh
autocalcounter[1]++;
if (MotorHardStop2[MOTOR3] != 0)
{
MotorArr2state++;
wait2 = 0;
setMotorState(MOTOR3,ACT_NOACT);
}
if (autocalcounter[1] > 3000 || MotorErr[MOTOR3] != 0)
{
MotorArr2state = 4;
wait2 = 0;
setMotorState(MOTOR3,ACT_NOACT);
}
break;
case 4://wait
wait2++;
if (wait2 > 50)
{
MotorArr2state++;
setMotorState(MOTOR4,ACT_XQ);
autocalcounter[1] = 0;
}
break;
case 5://motor4 xq
autocalcounter[1]++;
if (MotorHardStop1[MOTOR4] != 0)
{
MotorArr2state++;
wait2 = 0;
setMotorState(MOTOR4,ACT_NOACT);
}
if (autocalcounter[1] > 3000 || MotorErr[MOTOR4] != 0)
{
MotorArr2state = 0;
setMotorState(MOTOR4,ACT_NOACT);
}
break;
case 6://wait
wait2++;
if (wait2 > 50)
{
MotorArr2state++;
setMotorState(MOTOR4,ACT_XH);
autocalcounter[1] = 0;
}
break;
case 7:
autocalcounter[1]++;
if (MotorHardStop2[MOTOR4] != 0)
{
MotorArr2state=0;
wait2 = 0;
setMotorState(MOTOR4,ACT_NOACT);
}
if (autocalcounter[1] > 3000 || MotorErr[MOTOR4] != 0)
{
MotorArr2state = 0;
setMotorState(MOTOR4,ACT_NOACT);
}
break;
default:
break;
}
}
uint8_t flagMotorMemoryKeyStart,flagMotorMemoryKeyLongPress,flagKEYM1press;
uint16_t countMotorMemoryKeyStart;
uint16_t countMotorMemoryKeyLongPress;
@ -581,7 +341,7 @@ void MotorCtrl(void)//10ms
{
uint8_t i;
WriteMotorMemory(); //TODO
AutoCalCtrl();
//AutoCalCtrl();
if (flagMotorMemoryKeyStart == 1)
{
countMotorMemoryKeyStart++;
@ -699,7 +459,7 @@ void MotorCtrl(void)//10ms
MOTOR1Ctrl(MotorStateReal[MOTOR1]);
MOTOR2Ctrl(MotorStateReal[MOTOR2]);
MOTOR3Ctrl(MotorStateReal[MOTOR3]);
MOTOR4Ctrl(MotorStateReal[MOTOR4]);
}

8
app/user/PINdef.h
View File

@ -80,10 +80,10 @@ typedef enum
#define OUT_RLY1N P14_bit.no0
#define OUT_RLY2P P7_bit.no0//HG
#define OUT_RLY2N P3_bit.no2
#define OUT_RLY3P P13_bit.no0//ZD
#define OUT_RLY3N P7_bit.no1
#define OUT_RLY4P P7_bit.no2//TT
#define OUT_RLY4N P7_bit.no3
#define OUT_RLY4P P13_bit.no0//TT
#define OUT_RLY4N P7_bit.no1
#define OUT_RLY3P P7_bit.no2//JS
#define OUT_RLY3N P7_bit.no3

38
app/user/appTask.c
View File

@ -112,21 +112,21 @@ void KeyPressLogic(uint8_t keyid)
case KEYID_HG_R:
setMotorState(MOTOR2,ACT_XH);
break;
case KEYID_ZD_UP:
// case KEYID_ZD_UP:
// setMotorState(MOTOR3,ACT_XQ);
// break;
// case KEYID_ZD_DOWN:
// setMotorState(MOTOR3,ACT_XH);
// break;
case KEYID_TT_UP:
setMotorState(MOTOR3,ACT_XQ);
break;
case KEYID_ZD_DOWN:
case KEYID_TT_DOWN:
setMotorState(MOTOR3,ACT_XH);
break;
case KEYID_TT_UP:
setMotorState(MOTOR4,ACT_XQ);
break;
case KEYID_TT_DOWN:
setMotorState(MOTOR4,ACT_XH);
break;
case KEYID_FW:
case KEYID_JS:
//buzzer_start();
TP_act();
//TP_act();
break;
case KEYID_TP:
//buzzer_start();
@ -162,19 +162,19 @@ void KeyReleaseLogic(uint8_t keyid)
case KEYID_HG_R:
setMotorState(MOTOR2,ACT_NOACT);
break;
case KEYID_ZD_UP:
setMotorState(MOTOR3,ACT_NOACT);
break;
case KEYID_ZD_DOWN:
setMotorState(MOTOR3,ACT_NOACT);
break;
// case KEYID_ZD_UP:
// setMotorState(MOTOR3,ACT_NOACT);
// break;
// case KEYID_ZD_DOWN:
// setMotorState(MOTOR3,ACT_NOACT);
// break;
case KEYID_TT_UP:
setMotorState(MOTOR4,ACT_NOACT);
setMotorState(MOTOR3,ACT_NOACT);
break;
case KEYID_TT_DOWN:
setMotorState(MOTOR4,ACT_NOACT);
setMotorState(MOTOR3,ACT_NOACT);
break;
case KEYID_FW:
case KEYID_JS:
break;
case KEYID_TP:

2
app/user/hwCtrl.c
View File

@ -117,7 +117,7 @@ uint8_t GetSigState(uint8_t keyno)
return (getIOstate(IOID_P86)==0)?1:0;
case SIGID_KEY_TT_DOWN:
return (getIOstate(IOID_P85)==0)?1:0;
case SIGID_KEY_FW:
case SIGID_KEY_JS:
return (getIOstate(IOID_P120)==0)?1:0;
case SIGID_KEY_TP:
return (getIOstate(IOID_P125)==0)?1:0;

4
app/user/hwCtrl.h
View File

@ -18,7 +18,7 @@ typedef enum
KEYID_ZD_DOWN,
KEYID_TT_UP,
KEYID_TT_DOWN,
KEYID_FW,
KEYID_JS,
KEYID_TP,
KEYID_JY_SET,
KEYID_JY_1,
@ -37,7 +37,7 @@ typedef enum
SIGID_KEY_ZD_DOWN,
SIGID_KEY_TT_UP,
SIGID_KEY_TT_DOWN,
SIGID_KEY_FW,
SIGID_KEY_JS,
SIGID_KEY_TP,
SIGID_KEY_JY_SET,
SIGID_KEY_JY_1,