173 lines
3.4 KiB
C
173 lines
3.4 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 current1;
|
||
uint8_t OC1flag;
|
||
uint8_t act_step;
|
||
|
||
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)
|
||
{
|
||
if (act <= ACT_XH && motorid < 6)
|
||
{
|
||
MotorState[motorid] = act;
|
||
act_step = 0;
|
||
}
|
||
}
|
||
|
||
|
||
|
||
/*
|
||
20240106
|
||
正转时：第一个电机M1正转6秒后，M2电机正转，M1最大运行时间15秒
|
||
反转时：第一个电机M2反转8秒后，M1电机反转，M2最大运行时间17秒。
|
||
20240111
|
||
正转时：第一个电机M1正转2秒后，M2电机正转，M1最大运行时间15秒
|
||
反转时：第一个电机M2反转5秒后，M1电机反转，M2最大运行时间17秒。
|
||
*/
|
||
|
||
#define MOTOR_DELAY_TIME_8S (500)
|
||
#define MOTOR_DELAY_TIME_6S (200)
|
||
#define MOTOR_DELAY_TIME_15S (1500)
|
||
#define MOTOR_DELAY_TIME_17S (1700)
|
||
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://先1再2
|
||
switch (act_step)
|
||
{
|
||
case 0:
|
||
MotorStateReal[MOTOR1] = ACT_XQ;
|
||
act_step = 1;
|
||
break;
|
||
case 1:
|
||
if (OC1flag == 1)
|
||
{
|
||
MotorStateReal[MOTOR1] = ACT_NOACT;
|
||
MotorStateReal[MOTOR2] = ACT_XQ;
|
||
act_step = 2;
|
||
}
|
||
break;
|
||
|
||
case 2:
|
||
if (OC1flag == 1)
|
||
{
|
||
MotorStateReal[MOTOR1] = ACT_NOACT;
|
||
MotorStateReal[MOTOR2] = ACT_NOACT;
|
||
act_step = 3;
|
||
}
|
||
break;
|
||
|
||
case 3:
|
||
break;
|
||
|
||
default:
|
||
break;
|
||
}
|
||
break;
|
||
|
||
case ACT_XH://先2再1
|
||
switch (act_step)
|
||
{
|
||
case 0:
|
||
MotorStateReal[MOTOR2] = ACT_XH;
|
||
act_step = 1;
|
||
break;
|
||
case 1:
|
||
if (OC1flag == 1)
|
||
{
|
||
MotorStateReal[MOTOR2] = ACT_NOACT;
|
||
MotorStateReal[MOTOR1] = ACT_XH;
|
||
act_step = 2;
|
||
}
|
||
break;
|
||
|
||
case 2:
|
||
if (OC1flag == 1)
|
||
{
|
||
MotorStateReal[MOTOR1] = ACT_NOACT;
|
||
MotorStateReal[MOTOR2] = ACT_NOACT;
|
||
act_step = 3;
|
||
}
|
||
break;
|
||
|
||
case 3:
|
||
break;
|
||
|
||
default:
|
||
break;
|
||
}
|
||
break;
|
||
|
||
|
||
default:
|
||
break;
|
||
}
|
||
|
||
MOTOR1Ctrl(MotorStateReal[MOTOR1]);
|
||
MOTOR2Ctrl(MotorStateReal[MOTOR2]);
|
||
}
|
||
|
||
uint16_t getOC_th(void)
|
||
{
|
||
if (MotorStateReal[MOTOR1] != ACT_NOACT)
|
||
{
|
||
return 70U;
|
||
}
|
||
else
|
||
{
|
||
return 60U;
|
||
}
|
||
//return 50U;
|
||
|
||
}
|
||
|
||
void CurrentDetecte(void)
|
||
{
|
||
|
||
static uint16_t OC_Count1=0;
|
||
uint16_t OC_th;
|
||
current1 = getAdval(ADCH_RLY1);
|
||
OC_th = getOC_th();
|
||
if (current1 > OC_th && OC1flag == 0)
|
||
{
|
||
OC_Count1++;
|
||
if (OC_Count1 >= 200)
|
||
{
|
||
OC_Count1 = 0;
|
||
OC1flag = 1;
|
||
}
|
||
}
|
||
else
|
||
{
|
||
OC_Count1 = 0;
|
||
}
|
||
|
||
}
|
||
|
||
|