91 lines
1.6 KiB
C
91 lines
1.6 KiB
C
|
|
||
|
|
#include "MotorCtrl.h"
|
||
|
|
|
||
|
|
#include "hwCtrl.h"
|
||
|
|
#include "PINdef.h"
|
||
|
|
#include "appTask.h"
|
||
|
|
//#include "pfdl.h"
|
||
|
|
|
||
|
|
|
||
|
|
uint8_t MotorState[6],MotorStateReal[6];
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
void MotorValueInit(void)
|
||
|
|
{
|
||
|
|
uint8_t i = 0;
|
||
|
|
for (i = 0; i < 6; i++)
|
||
|
|
{
|
||
|
|
MotorState[i] = ACT_NOACT;
|
||
|
|
MotorStateReal[i] = ACT_NOACT;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
void setMotorState(uint8_t motorid,uint8_t act)
|
||
|
|
{
|
||
|
|
//motorid-=1;
|
||
|
|
if (act <= ACT_XH && motorid < 6)
|
||
|
|
{
|
||
|
|
MotorState[motorid] = act;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
#define MOTOR_DELAY_TIME_8S (800)
|
||
|
|
#define MOTOR_DELAY_TIME_6S (600)
|
||
|
|
void MotorCtrl(void)//10ms
|
||
|
|
{
|
||
|
|
static uint16_t MotorRunCount;
|
||
|
|
switch (MotorState[0])
|
||
|
|
{
|
||
|
|
case ACT_NOACT:
|
||
|
|
MotorRunCount = 0;
|
||
|
|
MotorStateReal[MOTOR1] = ACT_NOACT;
|
||
|
|
MotorStateReal[MOTOR2] = ACT_NOACT;
|
||
|
|
break;
|
||
|
|
case ACT_XQ:
|
||
|
|
MotorRunCount++;
|
||
|
|
MotorStateReal[MOTOR1] = ACT_XQ;
|
||
|
|
if (MotorRunCount > MOTOR_DELAY_TIME_6S)
|
||
|
|
{
|
||
|
|
MotorRunCount = MOTOR_DELAY_TIME_6S;
|
||
|
|
MotorStateReal[MOTOR2] = ACT_XQ;
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
MotorStateReal[MOTOR2] = ACT_NOACT;
|
||
|
|
|
||
|
|
}
|
||
|
|
break;
|
||
|
|
|
||
|
|
case ACT_XH:
|
||
|
|
MotorRunCount++;
|
||
|
|
MotorStateReal[MOTOR2] = ACT_XH;
|
||
|
|
if (MotorRunCount > MOTOR_DELAY_TIME_8S)
|
||
|
|
{
|
||
|
|
MotorRunCount = MOTOR_DELAY_TIME_8S;
|
||
|
|
MotorStateReal[MOTOR1] = ACT_XH;
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
MotorStateReal[MOTOR1] = ACT_NOACT;
|
||
|
|
}
|
||
|
|
break;
|
||
|
|
|
||
|
|
|
||
|
|
default:
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
|
||
|
|
MOTOR1Ctrl(MotorStateReal[MOTOR1]);
|
||
|
|
MOTOR2Ctrl(MotorStateReal[MOTOR2]);
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
|