109 lines
1.6 KiB
C
109 lines
1.6 KiB
C
|
|
#include "MotorCtrl.h"
|
|
|
|
#include "hwCtrl.h"
|
|
#include "PINdef.h"
|
|
#include "appTask.h"
|
|
//#include "pfdl.h"
|
|
|
|
|
|
volatile uint8_t MotorState[6],MotorStateReal[6];
|
|
uint16_t current1;
|
|
uint8_t OC1flag;
|
|
uint16_t OC_Count1=0;
|
|
uint8_t act_step[6];
|
|
|
|
void MotorValueInit(void)
|
|
{
|
|
uint8_t i = 0;
|
|
for (i = 0; i < 6; i++)
|
|
{
|
|
MotorState[i] = ACT_NOACT;
|
|
MotorStateReal[i] = ACT_NOACT;
|
|
}
|
|
}
|
|
|
|
void ToggleMotorState(void)
|
|
{
|
|
static uint8_t KeyMode1 = 0;
|
|
if(MotorState[0] != ACT_NOACT)
|
|
{
|
|
setMotorState(0,ACT_NOACT);
|
|
return;
|
|
}
|
|
if (KeyMode1 == 0)
|
|
{
|
|
setMotorState(0,ACT_XQ);
|
|
KeyMode1 = 1;
|
|
}
|
|
else
|
|
{
|
|
setMotorState(0,ACT_XH);
|
|
KeyMode1 = 0;
|
|
}
|
|
}
|
|
|
|
|
|
void setMotorState(uint8_t motorid,uint8_t act)
|
|
{
|
|
if (act <= ACT_XH && motorid < 6)
|
|
{
|
|
|
|
MotorState[motorid] = act;
|
|
act_step[motorid] = 0;
|
|
}
|
|
}
|
|
|
|
|
|
|
|
|
|
void MotorCtrl(void)//10ms
|
|
{
|
|
if (OC1flag == 1)
|
|
{
|
|
OC1flag = 0;
|
|
OC_Count1 = 0;
|
|
MotorState[MOTOR1] = ACT_NOACT;
|
|
}
|
|
|
|
MotorStateReal[MOTOR1] = MotorState[MOTOR1];
|
|
|
|
|
|
|
|
MOTOR1Ctrl(MotorStateReal[MOTOR1]);
|
|
//MOTOR2Ctrl(MotorStateReal[MOTOR2]);
|
|
//MOTOR1Ctrl(MotorState[MOTOR1]);
|
|
}
|
|
|
|
uint16_t getOC_th(void)
|
|
{
|
|
|
|
return 100U;
|
|
|
|
}
|
|
|
|
void CurrentDetecte(void)
|
|
{
|
|
|
|
|
|
uint16_t OC_th;
|
|
current1 = getAdval(ADCH_RLY1);
|
|
OC_th = getOC_th();
|
|
if (current1 > OC_th && OC1flag == 0)
|
|
{
|
|
OC_Count1++;
|
|
if (OC_Count1 >= 2000)
|
|
{
|
|
OC_Count1 = 0;
|
|
OC1flag = 1;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
OC_Count1 = 0;
|
|
}
|
|
|
|
}
|
|
|
|
|