134 lines
2.3 KiB
C
134 lines
2.3 KiB
C
#include "control.h"
|
|
|
|
extern unsigned char save_flag;
|
|
extern void Flash_Save(void);
|
|
|
|
void MotorStartCheck(void)
|
|
{
|
|
if(MOTORSTATE==STATEERROR)return;
|
|
|
|
if(SetSpeed>0&&startflag==1)
|
|
{
|
|
MOTORSTATE=STATESTARTPID;
|
|
Dir=dirflag;
|
|
HALLcount=0;
|
|
HALLcount1=0;
|
|
HALLcountTemp=0;
|
|
}
|
|
}
|
|
|
|
void MotorStartPID(void)
|
|
{
|
|
if(MOTORSTATE==STATEERROR)return;
|
|
|
|
PID_init();
|
|
PIDcompute(MINSPEED,0);
|
|
HALL_MOTOR_START();
|
|
|
|
TargS1=SetSpeed;
|
|
MOTORSTATE=STATERUNPID;
|
|
}
|
|
|
|
void MotorRunPID(void)
|
|
{
|
|
|
|
if(MOTORSTATE==STATEERROR)return;
|
|
if(SetSpeed==0||startflag==0) //给出停止信号
|
|
{
|
|
MOTORSTATE=STATESTOP;
|
|
}
|
|
|
|
TargS1=SetSpeed;
|
|
if(TimeCountPID>=20)//计算实时速度并时行PID运算
|
|
{
|
|
TimeCountPID=0;
|
|
RealS1=HALLcountTemp*500/MPolePairs;//HALLcount*100*60/6/POLEPAIRS;
|
|
PIDcompute(TargS1,RealS1);
|
|
}
|
|
}
|
|
|
|
void MotorStop(void)
|
|
{
|
|
if(MOTORSTATE==STATEERROR)return;
|
|
Motor_Start_F=0;
|
|
MOTOR_STOP0();
|
|
|
|
if(RealS==0)
|
|
{
|
|
MOTORSTATE=STATESTARTCHECK; //停下来后才切换流程
|
|
}
|
|
}
|
|
|
|
void MotorError(void)
|
|
{
|
|
MOTOR_STOP0();
|
|
Motor_Start_F=0;
|
|
MOTORSTATE=STATEERROROVER;
|
|
printf("MotorError\n");
|
|
}
|
|
|
|
void MotorBreak(void)
|
|
{
|
|
unsigned int dd;
|
|
if(GPIO_ReadPin(BKIN_GPIO_PORT,BKIN_GPIO_PIN)==GPIO_Pin_RESET)
|
|
{
|
|
for(dd=0;dd<500;dd++)
|
|
{;}
|
|
if(GPIO_ReadPin(BKIN_GPIO_PORT,BKIN_GPIO_PIN)==GPIO_Pin_RESET)
|
|
{
|
|
MOTOR_STOP0();
|
|
Motor_Start_F=0;
|
|
while(GPIO_ReadPin(BKIN_GPIO_PORT,BKIN_GPIO_PIN)==GPIO_Pin_RESET);
|
|
}
|
|
}
|
|
}
|
|
|
|
void MotorErrorOver(void)
|
|
{
|
|
unsigned char times=0;
|
|
|
|
times=0;
|
|
|
|
LEDOFF();TimeCountTemp=0;
|
|
while(TimeCountTemp<200);
|
|
while(1) //发生故障，则故障指示灯闪烁。需重启
|
|
{
|
|
SampleSpeed();
|
|
if(SetSpeed == 0) //故障时，不加速后恢复
|
|
{
|
|
ErrorCode = 0;
|
|
WaitStart();
|
|
GPIO_WritePin(VOUT_EN_GPIO_PORT,VOUT_EN_GPIO_PIN,GPIO_Pin_SET);
|
|
break;
|
|
}
|
|
|
|
}
|
|
}
|
|
|
|
void WaitStart(void)
|
|
{
|
|
if(SetSpeed==0) MOTORSTATE=STATESTARTCHECK;
|
|
}
|
|
|
|
void EnDirCheck(void)
|
|
{
|
|
|
|
|
|
if((GPIO_ReadPin(EN_GPIO_PORT,EN_GPIO_PIN)==GPIO_Pin_SET)\
|
|
&&(GPIO_ReadPin(DR_GPIO_PORT,DR_GPIO_PIN)==GPIO_Pin_SET) )
|
|
{
|
|
LevelSet = 2;
|
|
}
|
|
else if((GPIO_ReadPin(EN_GPIO_PORT,EN_GPIO_PIN)==GPIO_Pin_RESET)\
|
|
&&(GPIO_ReadPin(DR_GPIO_PORT,DR_GPIO_PIN)==GPIO_Pin_SET) )
|
|
{
|
|
LevelSet = 3;
|
|
}
|
|
if((GPIO_ReadPin(EN_GPIO_PORT,EN_GPIO_PIN)==GPIO_Pin_SET)\
|
|
&&(GPIO_ReadPin(DR_GPIO_PORT,DR_GPIO_PIN)==GPIO_Pin_RESET) )
|
|
{
|
|
LevelSet = 1;
|
|
}
|
|
printf("Level=%d\n",LevelSet);
|
|
}
|