647 lines
14 KiB
C
647 lines
14 KiB
C
|
|
#include "MotorCtrl.h"
|
|
|
|
#include "hwCtrl.h"
|
|
#include "PINdef.h"
|
|
#include "appTask.h"
|
|
#include "pfdl.h"
|
|
#include "buzzer.h"
|
|
|
|
|
|
uint8_t MotorState[6],MotorStateReal[6];
|
|
uint16_t MotorHallLoc[6],MotorHardStop1[6],MotorHardStop2[6];
|
|
uint8_t MotorLearnState[6];
|
|
uint8_t MotorErr[6],HallNotChange[6];
|
|
uint16_t current1,current2,current3;
|
|
uint16_t MotorTarget[6] = {0};
|
|
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;
|
|
uint8_t i;
|
|
if (act <= ACT_XH && motorid < 6)
|
|
{
|
|
MotorState[motorid] = act;
|
|
if (act!=ACT_NOACT)
|
|
{
|
|
for (i = 0; i < 3; i++)
|
|
{
|
|
MotorTarget[i] = 0;
|
|
}
|
|
|
|
}
|
|
}
|
|
}
|
|
uint8_t getMotorState(uint8_t motorid)
|
|
{
|
|
return MotorState[motorid];
|
|
}
|
|
void setMotorTarget(uint8_t motorid,uint16_t target)
|
|
{
|
|
|
|
MotorTarget[motorid] = target;
|
|
}
|
|
|
|
uint16_t getMotorTarget(uint8_t motorid)
|
|
{
|
|
return MotorTarget[motorid];
|
|
}
|
|
|
|
|
|
|
|
#define AUTOCAL_STOP 0
|
|
#define AUTOCAL_START 1
|
|
#define AUTOCAL_ACT1 2
|
|
#define AUTOCAL_ACT2 3
|
|
#define AUTOCAL_END 4
|
|
|
|
void StartAutoCal(void)
|
|
{
|
|
//AutoCalState = AUTOCAL_START;
|
|
uint8_t i;
|
|
for (i = 0; i < 4; i++)
|
|
{
|
|
MotorHardStop1[i] = 0;
|
|
MotorHardStop2[i] = 0;
|
|
|
|
}
|
|
|
|
|
|
}
|
|
void StopAutoCal(void)
|
|
{
|
|
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);
|
|
}
|
|
}
|
|
|
|
void FW_act(void)
|
|
{
|
|
setMotorTarget(MOTOR1,MotorHardStop1[MOTOR1]-100);
|
|
setMotorTarget(MOTOR2,MotorHardStop1[MOTOR2]-100);
|
|
setMotorTarget(MOTOR3,MotorHardStop1[MOTOR3]-100);
|
|
}
|
|
|
|
void TP_act(void)
|
|
{
|
|
setMotorTarget(MOTOR1,MotorHardStop2[MOTOR1]+500);//kb
|
|
setMotorTarget(MOTOR2,MotorHardStop2[MOTOR2]+100);//hg
|
|
setMotorTarget(MOTOR3,MotorHardStop2[MOTOR3]+100);//tt
|
|
}
|
|
|
|
|
|
|
|
|
|
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)
|
|
{
|
|
|
|
}
|
|
void MotorMemoryKeyM1Release(void)
|
|
{
|
|
uint8_t i;
|
|
if (flagMotorMemoryKeyLongPress == 2)
|
|
{
|
|
flagKEYM1press++;
|
|
if (flagKEYM1press >= 3)
|
|
{
|
|
StartAutoCal();
|
|
}
|
|
|
|
}
|
|
if (flagMotorMemoryKeyStart == 1)
|
|
{
|
|
flagMotorMemoryKeyStart = 0;
|
|
for (i = 0; i < 3; i++)
|
|
{
|
|
buzzer_start();
|
|
if (MotorHardStop1[i]!=0 && MotorHardStop2[i]!=0)
|
|
{
|
|
MemoryLoc[0][i] = MotorHallLoc[i];
|
|
}
|
|
EEL_SAVE_Counter = 1000;
|
|
EEL_SAVE_REQUIRE_FLAG = 1;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
for (i = 0; i < 3; i++)
|
|
{
|
|
setMotorTarget(i,MemoryLoc[0][i]);
|
|
}
|
|
}
|
|
}
|
|
|
|
void MotorMemoryKeyM2Press(void)
|
|
{
|
|
|
|
}
|
|
void MotorMemoryKeyM2Release(void)
|
|
{
|
|
uint8_t i;
|
|
if (flagMotorMemoryKeyStart == 1)
|
|
{
|
|
flagMotorMemoryKeyStart = 0;
|
|
buzzer_start();
|
|
for (i = 0; i < 3; i++)
|
|
{
|
|
if (MotorHardStop1[i]!=0 && MotorHardStop2[i]!=0)
|
|
{
|
|
MemoryLoc[1][i] = MotorHallLoc[i];
|
|
}
|
|
}
|
|
EEL_SAVE_Counter = 1000;
|
|
EEL_SAVE_REQUIRE_FLAG = 1;
|
|
}
|
|
else
|
|
{
|
|
for (i = 0; i < 3; i++)
|
|
{
|
|
setMotorTarget(i,MemoryLoc[1][i]);
|
|
}
|
|
}
|
|
}
|
|
|
|
void MotorMemoryKeyM3Press(void)
|
|
{
|
|
|
|
}
|
|
void MotorMemoryKeyM3Release(void)
|
|
{
|
|
uint8_t i;
|
|
if (flagMotorMemoryKeyStart == 1)
|
|
{
|
|
flagMotorMemoryKeyStart = 0;
|
|
buzzer_start();
|
|
for (i = 0; i < 3; i++)
|
|
{
|
|
if (MotorHardStop1[i]!=0 && MotorHardStop2[i]!=0)
|
|
{
|
|
MemoryLoc[2][i] = MotorHallLoc[i];
|
|
}
|
|
}
|
|
EEL_SAVE_Counter = 1000;
|
|
EEL_SAVE_REQUIRE_FLAG = 1;
|
|
}
|
|
else
|
|
{
|
|
for (i = 0; i < 3; i++)
|
|
{
|
|
setMotorTarget(i,MemoryLoc[2][i]);
|
|
}
|
|
}
|
|
|
|
}
|
|
|
|
/// @brief
|
|
/// @param
|
|
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 && MotorState[1] == ACT_NOACT && MotorErr[0] == 0)
|
|
{
|
|
MotorHardStop1[0] = MotorHallLoc[0];
|
|
}
|
|
else if (MotorState[0] == ACT_XH && MotorState[1] == ACT_NOACT && MotorErr[0] == 0)
|
|
{
|
|
MotorHardStop2[0] = MotorHallLoc[0];
|
|
}
|
|
else if (MotorState[1] == ACT_XQ && MotorState[0] == ACT_NOACT && MotorErr[1] == 0)
|
|
{
|
|
MotorHardStop1[1] = MotorHallLoc[1];
|
|
}
|
|
else if (MotorState[1] == ACT_XH && MotorState[0] == ACT_NOACT && MotorErr[1] == 0)
|
|
{
|
|
MotorHardStop2[1] = MotorHallLoc[1];
|
|
}
|
|
|
|
if (MotorState[MOTOR1] != ACT_NOACT && MotorState[MOTOR2] != ACT_NOACT)
|
|
{
|
|
if (HallNotChange[MOTOR1] == 1)
|
|
{
|
|
MotorState[MOTOR1] = ACT_NOACT;
|
|
}
|
|
if (HallNotChange[MOTOR2] == 1)
|
|
{
|
|
MotorState[MOTOR2] = ACT_NOACT;
|
|
}
|
|
|
|
|
|
}
|
|
else
|
|
{
|
|
MotorState[MOTOR1] = ACT_NOACT;
|
|
MotorState[MOTOR2] = ACT_NOACT;
|
|
}
|
|
|
|
/*
|
|
MotorState[MOTOR1] = ACT_NOACT;
|
|
MotorState[MOTOR2] = ACT_NOACT;
|
|
*/
|
|
}
|
|
if (OC2flag == 1)
|
|
{
|
|
OC2flag = 0;
|
|
if (MotorState[2] == ACT_XQ && MotorState[3] == ACT_NOACT && MotorErr[2] == 0)
|
|
{
|
|
MotorHardStop1[2] = MotorHallLoc[2];
|
|
}
|
|
else if (MotorState[2] == ACT_XH && MotorState[3] == ACT_NOACT && MotorErr[2] == 0)
|
|
{
|
|
MotorHardStop2[2] = MotorHallLoc[2];
|
|
}
|
|
else if (MotorState[3] == ACT_XQ && MotorState[2] == ACT_NOACT && MotorErr[3] == 0)
|
|
{
|
|
MotorHardStop1[3] = MotorHallLoc[3];
|
|
}
|
|
else if (MotorState[3] == ACT_XH && MotorState[2] == ACT_NOACT && MotorErr[3] == 0)
|
|
{
|
|
MotorHardStop2[3] = MotorHallLoc[3];
|
|
}
|
|
MotorState[MOTOR3] = ACT_NOACT;
|
|
//MotorState[MOTOR4] = ACT_NOACT;
|
|
}
|
|
|
|
|
|
for (i = 0; i < 3; 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]);
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
#define OC_10A 100
|
|
#define OC_500mS 500
|
|
#define OC_50mS 50
|
|
|
|
|
|
uint16_t getOCval(void)
|
|
{
|
|
uint16_t OCval = 56U;
|
|
if (MotorStateReal[MOTOR1] != ACT_NOACT)
|
|
{
|
|
OCval += 168U;
|
|
}
|
|
if (MotorStateReal[MOTOR2] != ACT_NOACT)
|
|
{
|
|
OCval += 168U;
|
|
}
|
|
return OCval;
|
|
}
|
|
void CurrentDetecte(void)
|
|
{
|
|
|
|
static uint16_t OC_Count1=0,OC_Count2=0,OC_Count3 = 0;
|
|
|
|
current1 = getAdval(ADCH_RLY2);
|
|
current2 = getAdval(ADCH_RLY1);
|
|
|
|
|
|
if (current1 > getOCval() && OC1flag == 0)//实测1A->0.14V->28AD
|
|
{
|
|
OC_Count1++;
|
|
if (OC_Count1 >= 300)
|
|
{
|
|
OC_Count1 = 0;
|
|
OC1flag = 1;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
OC_Count1 = 0;
|
|
}
|
|
|
|
if (current2 > 262U && OC2flag == 0)//实测1A->0.16V->32AD
|
|
{
|
|
OC_Count2++;
|
|
if (OC_Count2 >= 300)
|
|
{
|
|
OC_Count2 = 0;
|
|
OC2flag = 1;
|
|
}
|
|
|
|
}
|
|
else
|
|
{
|
|
OC_Count2 = 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 < 4; i++)
|
|
{
|
|
hallstate = GetSigState(SIGID_HALL_KB + i);
|
|
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;
|
|
}
|
|
|
|
}
|
|
else
|
|
{
|
|
HallErrorCount[i] = 0;
|
|
}
|
|
|
|
}
|
|
if (HallErrorCount[i] > 100)
|
|
{
|
|
HallNotChange[i] = 1;
|
|
}
|
|
else
|
|
{
|
|
HallNotChange[i] = 0;
|
|
}
|
|
}
|
|
|
|
if (EEL_SAVE_Counter > 0)
|
|
{
|
|
EEL_SAVE_Counter--;
|
|
}
|
|
|
|
}
|
|
|
|
|
|
void setJSMomory(void)
|
|
{
|
|
buzzer_start();
|
|
MemoryLoc[0][3] = MotorHallLoc[MOTOR1];
|
|
MemoryLoc[1][3] = MotorHallLoc[MOTOR2];
|
|
MemoryLoc[2][3] = MotorHallLoc[MOTOR3];
|
|
EEL_SAVE_Counter = 1000;
|
|
EEL_SAVE_REQUIRE_FLAG = 1;
|
|
}
|
|
|
|
void gotoJSLoc(void)
|
|
{
|
|
setMotorTarget(MOTOR1,MemoryLoc[0][3]);
|
|
setMotorTarget(MOTOR2,MemoryLoc[1][3]);
|
|
setMotorTarget(MOTOR3,MemoryLoc[2][3]);
|
|
}
|
|
|