980 lines
22 KiB
C
980 lines
22 KiB
C
|
|
#include "MotorCtrl.h"
|
|
|
|
#include "hwCtrl.h"
|
|
#include "PINdef.h"
|
|
#include "appTask.h"
|
|
#include "pfdl.h"
|
|
|
|
|
|
uint8_t MotorState[6],MotorStateReal[6];
|
|
uint16_t MotorHallLoc[6],MotorHardStop1[6],MotorHardStop2[6];
|
|
uint8_t MotorLearnState[6];
|
|
uint8_t MotorErr[6];
|
|
uint16_t current1,current2,current3;
|
|
|
|
static uint8_t AutoCalState;
|
|
|
|
uint8_t EEL_SAVE_REQUIRE_FLAG;
|
|
uint16_t EEL_SAVE_Counter;
|
|
#define OVMOTORKB 1
|
|
#define OVMOTORZY 2
|
|
|
|
uint8_t OC1flag,OC2flag,OC3flag;
|
|
|
|
typedef struct
|
|
{
|
|
uint16_t MotorStopLoc1;
|
|
uint16_t MotorStopLoc2;
|
|
uint16_t MotorNowLoc;
|
|
}MOTOR_DATA;
|
|
|
|
MOTOR_DATA MotorData[6];
|
|
uint16_t MemoryLoc[3][6];
|
|
|
|
typedef struct
|
|
{
|
|
uint16_t start_flag;
|
|
MOTOR_DATA MotorData[6];
|
|
uint16_t MemoryLoc[3][6];
|
|
uint16_t checksum;
|
|
uint16_t stop_flag;
|
|
}MEMORY_DATA;
|
|
MEMORY_DATA MemoryData;
|
|
|
|
uint16_t GetCheckSum(uint16_t * addr,uint8_t len)
|
|
{
|
|
uint16_t sum=0;
|
|
while (len --)
|
|
{
|
|
sum += *addr;
|
|
}
|
|
sum ^= 0xffff;
|
|
|
|
return sum;
|
|
}
|
|
#define START_FLAG 0x55aa
|
|
#define STOP_FLAG 0xaa55
|
|
void WriteMotorMemory(void)
|
|
{
|
|
uint8_t i;
|
|
if (EEL_SAVE_REQUIRE_FLAG == 0 || EEL_SAVE_Counter > 0)
|
|
{
|
|
return;
|
|
}
|
|
|
|
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,36);
|
|
if(WriteDataflash(&MemoryData,0x000F1000,sizeof(MemoryData)) == 0x11)
|
|
{
|
|
EEL_SAVE_REQUIRE_FLAG=0;
|
|
}
|
|
}
|
|
|
|
|
|
void ReadMotorMemory(void)
|
|
{
|
|
uint16_t checksum;
|
|
uint8_t i;
|
|
ReadFlashData(&MemoryData,0x000F1000,sizeof(MemoryData));
|
|
checksum = GetCheckSum(&MemoryData.MotorData[0].MotorStopLoc1,36);
|
|
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;
|
|
}
|
|
|
|
}
|
|
}
|
|
|
|
|
|
void MotorValueInit(void)
|
|
{
|
|
uint8_t i;
|
|
for (i = 0; i < 6; i++)
|
|
{
|
|
MotorHallLoc[i] = 0x8000;
|
|
MotorErr[i] = 0;
|
|
}
|
|
ReadMotorMemory();
|
|
}
|
|
|
|
void setMotorState(uint8_t motorid,uint8_t act)
|
|
{
|
|
//motorid-=1;
|
|
if (act <= ACT_XH && motorid < 6)
|
|
{
|
|
MotorState[motorid] = act;
|
|
}
|
|
}
|
|
uint16_t MotorTarget[6] = {0};
|
|
void setMotorTarget(uint8_t motorid,uint16_t target)
|
|
{
|
|
|
|
MotorTarget[motorid] = target;
|
|
}
|
|
|
|
|
|
|
|
|
|
#define AUTOCAL_STOP 0
|
|
#define AUTOCAL_START 1
|
|
#define AUTOCAL_ACT1 2
|
|
#define AUTOCAL_ACT2 3
|
|
#define AUTOCAL_END 4
|
|
|
|
uint8_t stopflag = 0;
|
|
void StartAutoCal(void)
|
|
{
|
|
AutoCalState = AUTOCAL_START;
|
|
|
|
}
|
|
void StopAutoCal(void)
|
|
{
|
|
uint8_t i;
|
|
if (AutoCalState != AUTOCAL_STOP)
|
|
{
|
|
AutoCalState = AUTOCAL_STOP;
|
|
setMotorState(MOTOR1,ACT_NOACT);
|
|
setMotorState(MOTOR2,ACT_NOACT);
|
|
setMotorState(MOTOR3,ACT_NOACT);
|
|
setMotorState(MOTOR4,ACT_NOACT);
|
|
setMotorState(MOTOR5,ACT_NOACT);
|
|
setMotorState(MOTOR6,ACT_NOACT);
|
|
}
|
|
for (i = 0; i < 6; i++)
|
|
{
|
|
if (MotorTarget[i] != 0)
|
|
{
|
|
MotorTarget[i] = 0;
|
|
stopflag = 1;
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
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 < 6; 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;
|
|
}
|
|
switch (MotorArr3state)
|
|
{
|
|
case 1://motor5 xq
|
|
autocalcounter[2]++;
|
|
if (MotorHardStop1[MOTOR5] != 0)
|
|
{
|
|
MotorArr3state++;
|
|
wait3 = 0;
|
|
setMotorState(MOTOR5,ACT_NOACT);
|
|
}
|
|
if (autocalcounter[2] > 3000 || MotorErr[MOTOR5] != 0)
|
|
{
|
|
MotorArr3state = 4;
|
|
wait3 = 0;
|
|
setMotorState(MOTOR5,ACT_NOACT);
|
|
}
|
|
break;
|
|
case 2://wait
|
|
wait3++;
|
|
if (wait3 > 50)
|
|
{
|
|
MotorArr3state++;
|
|
setMotorState(MOTOR5,ACT_XH);
|
|
autocalcounter[2] = 0;
|
|
}
|
|
break;
|
|
case 3://motor5 xh
|
|
autocalcounter[2]++;
|
|
if (MotorHardStop2[MOTOR5] != 0)
|
|
{
|
|
MotorArr3state++;
|
|
wait3 = 0;
|
|
setMotorState(MOTOR5,ACT_NOACT);
|
|
}
|
|
if (autocalcounter[2] > 3000 || MotorErr[MOTOR5] != 0)
|
|
{
|
|
MotorArr3state = 4;
|
|
wait3 = 0;
|
|
setMotorState(MOTOR5,ACT_NOACT);
|
|
}
|
|
break;
|
|
case 4://wait
|
|
wait3++;
|
|
if (wait3 > 50)
|
|
{
|
|
MotorArr3state++;
|
|
setMotorState(MOTOR6,ACT_XQ);
|
|
autocalcounter[2] = 0;
|
|
}
|
|
break;
|
|
case 5://motor6 xq
|
|
autocalcounter[2]++;
|
|
if (MotorHardStop1[MOTOR6] != 0)
|
|
{
|
|
MotorArr3state++;
|
|
wait3 = 0;
|
|
setMotorState(MOTOR6,ACT_NOACT);
|
|
}
|
|
if (autocalcounter[2] > 3000 || MotorErr[MOTOR6] != 0)
|
|
{
|
|
MotorArr3state = 0;
|
|
setMotorState(MOTOR6,ACT_NOACT);
|
|
}
|
|
break;
|
|
case 6://wait
|
|
wait3++;
|
|
if (wait3 > 50)
|
|
{
|
|
MotorArr3state++;
|
|
setMotorState(MOTOR6,ACT_XH);
|
|
autocalcounter[2] = 0;
|
|
}
|
|
break;
|
|
case 7:
|
|
autocalcounter[2]++;
|
|
if (MotorHardStop2[MOTOR6] != 0)
|
|
{
|
|
MotorArr3state=0;
|
|
wait3 = 0;
|
|
setMotorState(MOTOR6,ACT_NOACT);
|
|
}
|
|
if (autocalcounter[2] > 3000 || MotorErr[MOTOR6] != 0)
|
|
{
|
|
MotorArr3state = 0;
|
|
setMotorState(MOTOR6,ACT_NOACT);
|
|
}
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
}
|
|
uint8_t flagMotorMemoryKeyStart,flagMotorMemoryKeyLongPress,flagKEYM1press;
|
|
uint16_t countMotorMemoryKeyStart;
|
|
uint16_t countMotorMemoryKeyLongPress;
|
|
void MotorMemoryKeyMMPress(void)
|
|
{
|
|
|
|
flagMotorMemoryKeyLongPress = 1;
|
|
|
|
countMotorMemoryKeyLongPress = 0;
|
|
}
|
|
|
|
void MotorMemoryKeyMMRelease(void)
|
|
{
|
|
if (countMotorMemoryKeyLongPress >= 300)
|
|
{
|
|
flagMotorMemoryKeyLongPress = 2;
|
|
countMotorMemoryKeyLongPress = 500;
|
|
flagKEYM1press = 0;
|
|
}
|
|
else
|
|
{
|
|
flagMotorMemoryKeyStart = 1;
|
|
countMotorMemoryKeyStart = 0;
|
|
flagMotorMemoryKeyLongPress = 0;
|
|
countMotorMemoryKeyLongPress = 0;
|
|
|
|
}
|
|
}
|
|
|
|
void MotorMemoryKeyM1Press(void)
|
|
{
|
|
stopflag = 0;
|
|
}
|
|
void MotorMemoryKeyM1Release(void)
|
|
{
|
|
uint8_t i;
|
|
if (flagMotorMemoryKeyLongPress == 2)
|
|
{
|
|
flagKEYM1press++;
|
|
if (flagKEYM1press >= 3)
|
|
{
|
|
StartAutoCal();
|
|
}
|
|
|
|
}
|
|
if (flagMotorMemoryKeyStart == 1)
|
|
{
|
|
flagMotorMemoryKeyStart = 0;
|
|
for (i = 0; i < 6; i++)
|
|
{
|
|
|
|
if (MotorHardStop1[i]!=0 && MotorHardStop2[i]!=0)
|
|
{
|
|
MemoryLoc[0][i] = MotorHallLoc[i];
|
|
}
|
|
EEL_SAVE_Counter = 1000;
|
|
EEL_SAVE_REQUIRE_FLAG = 1;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if (stopflag == 0)
|
|
{
|
|
for (i = 0; i < 6; i++)
|
|
{
|
|
setMotorTarget(i,MemoryLoc[0][i]);
|
|
}
|
|
}
|
|
stopflag = 0;
|
|
}
|
|
}
|
|
|
|
void MotorMemoryKeyM2Press(void)
|
|
{
|
|
stopflag = 0;
|
|
}
|
|
void MotorMemoryKeyM2Release(void)
|
|
{
|
|
uint8_t i;
|
|
if (flagMotorMemoryKeyStart == 1)
|
|
{
|
|
flagMotorMemoryKeyStart = 0;
|
|
for (i = 0; i < 6; i++)
|
|
{
|
|
if (MotorHardStop1[i]!=0 && MotorHardStop2[i]!=0)
|
|
{
|
|
MemoryLoc[1][i] = MotorHallLoc[i];
|
|
}
|
|
}
|
|
EEL_SAVE_Counter = 1000;
|
|
EEL_SAVE_REQUIRE_FLAG = 1;
|
|
}
|
|
else
|
|
{
|
|
if (stopflag == 0)
|
|
for (i = 0; i < 6; i++)
|
|
{
|
|
setMotorTarget(i,MemoryLoc[1][i]);
|
|
}
|
|
stopflag = 0;
|
|
}
|
|
}
|
|
|
|
void MotorMemoryKeyM3Press(void)
|
|
{
|
|
stopflag = 0;
|
|
}
|
|
void MotorMemoryKeyM3Release(void)
|
|
{
|
|
uint8_t i;
|
|
if (flagMotorMemoryKeyStart == 1)
|
|
{
|
|
flagMotorMemoryKeyStart = 0;
|
|
for (i = 0; i < 6; i++)
|
|
{
|
|
if (MotorHardStop1[i]!=0 && MotorHardStop2[i]!=0)
|
|
{
|
|
MemoryLoc[2][i] = MotorHallLoc[i];
|
|
}
|
|
}
|
|
EEL_SAVE_Counter = 1000;
|
|
EEL_SAVE_REQUIRE_FLAG = 1;
|
|
}
|
|
else
|
|
{
|
|
if (stopflag == 0)
|
|
for (i = 0; i < 6; i++)
|
|
{
|
|
setMotorTarget(i,MemoryLoc[2][i]);
|
|
}
|
|
stopflag = 0;
|
|
}
|
|
|
|
}
|
|
|
|
void MotorCtrl(void)//10ms
|
|
{
|
|
uint8_t i;
|
|
WriteMotorMemory(); //TODO
|
|
AutoCalCtrl();
|
|
if (flagMotorMemoryKeyStart == 1)
|
|
{
|
|
countMotorMemoryKeyStart++;
|
|
if (countMotorMemoryKeyStart > 500)//5S
|
|
{
|
|
flagMotorMemoryKeyStart = 0;
|
|
countMotorMemoryKeyStart = 0;
|
|
}
|
|
}
|
|
if (flagMotorMemoryKeyLongPress == 1 && countMotorMemoryKeyLongPress < 1000)
|
|
{
|
|
countMotorMemoryKeyLongPress++;
|
|
}
|
|
if (flagMotorMemoryKeyLongPress == 2 && countMotorMemoryKeyLongPress > 0)
|
|
{
|
|
countMotorMemoryKeyLongPress--;
|
|
if (countMotorMemoryKeyLongPress == 0)
|
|
{
|
|
flagMotorMemoryKeyLongPress = 0;
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
if (OC1flag == 1)
|
|
{
|
|
OC1flag = 0;
|
|
if (MotorState[0] == ACT_XQ)
|
|
{
|
|
MotorHardStop1[0] = MotorHallLoc[0];
|
|
}
|
|
else if (MotorState[0] == ACT_XH)
|
|
{
|
|
MotorHardStop2[0] = MotorHallLoc[0];
|
|
}
|
|
else if (MotorState[1] == ACT_XQ)
|
|
{
|
|
MotorHardStop1[1] = MotorHallLoc[1];
|
|
}
|
|
else if (MotorState[1] == ACT_XH)
|
|
{
|
|
MotorHardStop2[1] = MotorHallLoc[1];
|
|
}
|
|
setMotorTarget(MOTOR1,0);
|
|
setMotorTarget(MOTOR2,0);
|
|
MotorState[MOTOR1] = ACT_NOACT;
|
|
MotorState[MOTOR2] = ACT_NOACT;
|
|
}
|
|
if (OC2flag == 1)
|
|
{
|
|
OC2flag = 0;
|
|
if (MotorState[2] == ACT_XQ)
|
|
{
|
|
MotorHardStop1[2] = MotorHallLoc[2];
|
|
}
|
|
else if (MotorState[2] == ACT_XH)
|
|
{
|
|
MotorHardStop2[2] = MotorHallLoc[2];
|
|
}
|
|
else if (MotorState[3] == ACT_XQ)
|
|
{
|
|
MotorHardStop1[3] = MotorHallLoc[3];
|
|
}
|
|
else if (MotorState[3] == ACT_XH)
|
|
{
|
|
MotorHardStop2[3] = MotorHallLoc[3];
|
|
}
|
|
setMotorTarget(MOTOR3,0);
|
|
setMotorTarget(MOTOR4,0);
|
|
MotorState[MOTOR3] = ACT_NOACT;
|
|
MotorState[MOTOR4] = ACT_NOACT;
|
|
}
|
|
if (OC3flag == 1)
|
|
{
|
|
OC3flag = 0;
|
|
if (MotorState[4] == ACT_XQ)
|
|
{
|
|
MotorHardStop1[4] = MotorHallLoc[4];
|
|
}
|
|
else if (MotorState[4] == ACT_XH)
|
|
{
|
|
MotorHardStop2[4] = MotorHallLoc[4];
|
|
}
|
|
else if (MotorState[5] == ACT_XQ)
|
|
{
|
|
MotorHardStop1[5] = MotorHallLoc[5];
|
|
}
|
|
else if (MotorState[5] == ACT_XH)
|
|
{
|
|
MotorHardStop2[5] = MotorHallLoc[5];
|
|
}
|
|
setMotorTarget(MOTOR5,0);
|
|
setMotorTarget(MOTOR6,0);
|
|
MotorState[MOTOR5] = ACT_NOACT;
|
|
MotorState[MOTOR6] = ACT_NOACT;
|
|
}
|
|
|
|
for (i = 0; i < 6; i++)
|
|
{
|
|
MotorStateReal[i] = MotorState[i];
|
|
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] > MotorHallLoc[i]+10)
|
|
{
|
|
MotorStateReal[i] = ACT_XQ;
|
|
}
|
|
else if (MotorTarget[i] < MotorHallLoc[i]-10)
|
|
{
|
|
MotorStateReal[i] = ACT_XH;
|
|
}
|
|
else
|
|
{
|
|
MotorTarget[i] = 0;
|
|
}
|
|
}
|
|
|
|
}
|
|
else if (MotorHardStop1[i] != 0 && MotorHardStop2[i] != 0)
|
|
{
|
|
if (MotorHallLoc[i] > (MotorHardStop1[i]-20) && MotorStateReal[i] == ACT_XQ)
|
|
{
|
|
MotorStateReal[i] = ACT_NOACT;
|
|
}
|
|
if (MotorHallLoc[i] < (MotorHardStop2[i] + 20) && MotorStateReal[i] == ACT_XH)
|
|
{
|
|
MotorStateReal[i] = ACT_NOACT;
|
|
}
|
|
|
|
}
|
|
|
|
else
|
|
{
|
|
MotorTarget[i] = 0;
|
|
}
|
|
}
|
|
|
|
MOTOR1Ctrl(MotorStateReal[MOTOR1]);
|
|
MOTOR2Ctrl(MotorStateReal[MOTOR2]);
|
|
MOTOR3Ctrl(MotorStateReal[MOTOR3]);
|
|
MOTOR4Ctrl(MotorStateReal[MOTOR4]);
|
|
MOTOR5Ctrl(MotorStateReal[MOTOR5]);
|
|
MOTOR6Ctrl(MotorStateReal[MOTOR6]);
|
|
}
|
|
|
|
|
|
|
|
#define OC_10A 100
|
|
#define OC_30A 300
|
|
#define OC_500mS 500
|
|
#define OC_50mS 50
|
|
|
|
|
|
uint16_t getOverCurrentTh(uint8_t ch)
|
|
{
|
|
uint16_t th = 10;
|
|
switch (ch)
|
|
{
|
|
case 0:
|
|
if (MotorStateReal[MOTOR1]!=ACT_NOACT)
|
|
{
|
|
th+=50;
|
|
}
|
|
if (MotorStateReal[MOTOR2]!=ACT_NOACT)
|
|
{
|
|
th+=50;
|
|
}
|
|
break;
|
|
case 1:
|
|
if (MotorStateReal[MOTOR3]!=ACT_NOACT)
|
|
{
|
|
th+=50;
|
|
}
|
|
if (MotorStateReal[MOTOR4]!=ACT_NOACT)
|
|
{
|
|
th+=50;
|
|
}
|
|
break;
|
|
case 2:
|
|
if (MotorStateReal[MOTOR5]!=ACT_NOACT)
|
|
{
|
|
th+=50;
|
|
}
|
|
if (MotorStateReal[MOTOR6]!=ACT_NOACT)
|
|
{
|
|
th+=20;
|
|
}
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
return th;
|
|
}
|
|
|
|
#define OC_TIME_200MS 200
|
|
void CurrentDetecte(void)
|
|
{
|
|
|
|
static uint16_t OC_Count1=0,OC_Count2=0,OC_Count3 = 0;
|
|
uint16_t OverCurrTh;
|
|
current1 = getAdval(ADCH_RLY1);
|
|
current2 = getAdval(ADCH_RLY2);
|
|
current3 = getAdval(ADCH_RLY3);
|
|
|
|
if (current1 > 60U && OC1flag == 0)
|
|
{
|
|
if (current1 > OC_30A)
|
|
{
|
|
OC_Count1 += 10;
|
|
}
|
|
OC_Count1++;
|
|
if (OC_Count1 >= OC_TIME_200MS)
|
|
{
|
|
OC_Count1 = 0;
|
|
OC1flag = 1;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
OC_Count1 = 0;
|
|
}
|
|
|
|
if (current2 > 60U && OC2flag == 0)
|
|
{
|
|
if (current2 > OC_30A)
|
|
{
|
|
OC_Count2 += 10;
|
|
}
|
|
OC_Count2++;
|
|
if (OC_Count2 >= OC_TIME_200MS)
|
|
{
|
|
OC_Count2 = 0;
|
|
OC2flag = 1;
|
|
}
|
|
|
|
}
|
|
else
|
|
{
|
|
OC_Count2 = 0;
|
|
}
|
|
OverCurrTh = getOverCurrentTh(2);
|
|
if (current3 > OverCurrTh && OC3flag == 0)
|
|
{
|
|
if (current3 > OC_30A)
|
|
{
|
|
OC_Count3 += 10;
|
|
}
|
|
OC_Count3++;
|
|
if (OC_Count3 >= OC_TIME_200MS)
|
|
{
|
|
OC_Count3 = 0;
|
|
OC3flag = 1;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
OC_Count3 = 0;
|
|
}
|
|
|
|
}
|
|
|
|
void OverCurrentPro(uint8_t ovmotor)
|
|
{
|
|
|
|
}
|
|
|
|
#define HALLDELAYMAX 2
|
|
|
|
uint16_t HallErrorCount[6];
|
|
void HallDetecte(void)
|
|
{
|
|
static uint8_t HallLastState[6],HallDelay[6];
|
|
uint8_t i,hallstate;
|
|
for (i = 0; i < 6; i++)
|
|
{
|
|
hallstate = GetIOState(i+1);
|
|
if (hallstate != HallLastState[i])
|
|
{
|
|
HallDelay[i]++;
|
|
if (HallDelay[i] > HALLDELAYMAX)
|
|
{
|
|
if (MotorStateReal[i] == ACT_XQ)
|
|
{
|
|
MotorHallLoc[i]++;
|
|
}
|
|
else if (MotorStateReal[i] == ACT_XH)
|
|
{
|
|
MotorHallLoc[i]--;
|
|
}
|
|
HallLastState[i] = hallstate;
|
|
}
|
|
HallErrorCount[i] = 0;
|
|
MotorErr[i] = 0;
|
|
EEL_SAVE_Counter = 1000;
|
|
EEL_SAVE_REQUIRE_FLAG = 1;
|
|
}
|
|
else
|
|
{
|
|
HallDelay[i] = 0;
|
|
if (MotorStateReal[i] != ACT_NOACT)
|
|
{
|
|
HallErrorCount[i]++;
|
|
if (HallErrorCount[i] > 500)
|
|
{
|
|
HallErrorCount[i] = 500;
|
|
MotorErr[i] = 1;
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
}
|
|
|
|
if (EEL_SAVE_Counter > 0)
|
|
{
|
|
EEL_SAVE_Counter--;
|
|
}
|
|
|
|
}
|