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) //<2F><><EFBFBD><EFBFBD>ֹͣ<CDA3>ź<EFBFBD>
|
|||
|
|
{
|
|||
|
|
MOTORSTATE=STATESTOP;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
TargS1=SetSpeed;
|
|||
|
|
if(TimeCountPID>=20)//<2F><><EFBFBD><EFBFBD>ʵʱ<CAB5>ٶȲ<D9B6>ʱ<EFBFBD><CAB1>PID<49><44><EFBFBD><EFBFBD>
|
|||
|
|
{
|
|||
|
|
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; //ͣ<><CDA3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>л<EFBFBD><D0BB><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
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) //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϣ<EFBFBD><CFA3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ָʾ<D6B8><CABE><EFBFBD><EFBFBD>˸<EFBFBD><CBB8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
{
|
|||
|
|
SampleSpeed();
|
|||
|
|
if(SetSpeed == 0) //<2F><><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٺ<EFBFBD><D9BA>ָ<EFBFBD>
|
|||
|
|
{
|
|||
|
|
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);
|
|||
|
|
}
|