491 lines
11 KiB
C
491 lines
11 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;
|
|
|
|
|
|
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;
|
|
|
|
MotorHardStop1[i] = 0;
|
|
MotorHardStop2[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 setMotorAutoXQ(void)
|
|
{
|
|
|
|
MotorTarget[0] = MotorHardStop1[0] - 2500;
|
|
}
|
|
void setMotorAutoXH(void)
|
|
{
|
|
MotorTarget[0] = MotorHardStop1[0] - 10;
|
|
}
|
|
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
|
|
|
|
void StartAutoCal(void)
|
|
{
|
|
AutoCalState = AUTOCAL_START;
|
|
|
|
}
|
|
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 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;
|
|
}
|
|
|
|
}
|
|
|
|
|
|
void MotorCtrl(void)//10ms
|
|
{
|
|
uint8_t i;
|
|
//WriteMotorMemory(); //TODO
|
|
//AutoCalCtrl();
|
|
|
|
if (OC1flag == 1)
|
|
{
|
|
OC1flag = 0;
|
|
if (MotorState[0] == ACT_XQ)
|
|
{
|
|
MotorHardStop1[0] = MotorHallLoc[0];//后停止点
|
|
}
|
|
else if (MotorState[0] == ACT_XH)
|
|
{
|
|
MotorHardStop2[0] = MotorHallLoc[0];//前停止点
|
|
}
|
|
MotorState[MOTOR1] = ACT_NOACT;
|
|
}
|
|
|
|
|
|
|
|
MotorStateReal[0] = MotorState[0];
|
|
if (MotorState[0] == ACT_NOACT && MotorHardStop1[0] != 0 )//&& MotorHardStop2[0] != 0
|
|
{
|
|
if (MotorTarget[0]!=0 && MotorHardStop1[0] > MotorTarget[0] )//&& MotorTarget[0] > MotorHardStop2[0]
|
|
{
|
|
if (MotorTarget[0] > MotorHallLoc[0]+10)
|
|
{
|
|
MotorStateReal[0] = ACT_XQ;
|
|
}
|
|
else if (MotorTarget[0] < MotorHallLoc[0]-10)
|
|
{
|
|
MotorStateReal[0] = ACT_XH;
|
|
}
|
|
else
|
|
{
|
|
MotorTarget[0] = 0;
|
|
}
|
|
}
|
|
|
|
}
|
|
else if (MotorHardStop1[0] != 0 && MotorHardStop2[0] != 0)
|
|
{
|
|
if (MotorHallLoc[0] > (MotorHardStop1[0]-20) && MotorStateReal[0] == ACT_XQ)
|
|
{
|
|
MotorStateReal[0] = ACT_NOACT;
|
|
}
|
|
if (MotorHallLoc[0] < (MotorHardStop2[0] + 20) && MotorStateReal[0] == ACT_XH)
|
|
{
|
|
MotorStateReal[0] = ACT_NOACT;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
MotorTarget[0] = 0;
|
|
}
|
|
|
|
|
|
MOTOR1Ctrl(MotorStateReal[MOTOR1]);
|
|
}
|
|
|
|
|
|
|
|
#define OC_10A 100
|
|
#define OC_500mS 500
|
|
#define OC_50mS 50
|
|
|
|
void CurrentDetecte(void)
|
|
{
|
|
|
|
static uint16_t OC_Count1=0,OC_Count2=0,OC_Count3 = 0;
|
|
|
|
current1 = getAdval(ADCH_RLY1);
|
|
|
|
if (current1 > 100U && OC1flag == 0)
|
|
{
|
|
OC_Count1++;
|
|
if (OC_Count1 >= 100)
|
|
{
|
|
OC_Count1 = 0;
|
|
OC1flag = 1;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
OC_Count1 = 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 hallstate;
|
|
|
|
hallstate = GetIOState(SIGID_HALL1);
|
|
if (hallstate != HallLastState[0])
|
|
{
|
|
HallDelay[0]++;
|
|
if (HallDelay[0] > HALLDELAYMAX)
|
|
{
|
|
if (MotorStateReal[0] == ACT_XQ)
|
|
{
|
|
MotorHallLoc[0]++;
|
|
}
|
|
else if (MotorStateReal[0] == ACT_XH)
|
|
{
|
|
MotorHallLoc[0]--;
|
|
}
|
|
HallLastState[0] = hallstate;
|
|
}
|
|
HallErrorCount[0] = 0;
|
|
MotorErr[0] = 0;
|
|
EEL_SAVE_Counter = 1000;
|
|
EEL_SAVE_REQUIRE_FLAG = 1;
|
|
}
|
|
else
|
|
{
|
|
HallDelay[0] = 0;
|
|
if (MotorStateReal[0] != ACT_NOACT)
|
|
{
|
|
HallErrorCount[0]++;
|
|
if (HallErrorCount[0] > 500)
|
|
{
|
|
HallErrorCount[0] = 500;
|
|
MotorErr[0] = 1;
|
|
}
|
|
}
|
|
}
|
|
|
|
if (EEL_SAVE_Counter > 0)
|
|
{
|
|
EEL_SAVE_Counter--;
|
|
}
|
|
|
|
}
|
|
|
|
uint16_t getMotorLoc(void)
|
|
{
|
|
return MotorHallLoc[0];
|
|
}
|