#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;
        OC1flag = 0;
        break;
    case ACT_XQ://先1再2
        switch (act_step)
        {
        case 0:
            MotorStateReal[MOTOR1] = ACT_XQ;
            MotorStateReal[MOTOR2] = ACT_NOACT;
            act_step = 1;
            break;
        case 1:
            if (OC1flag == 1)
            {
                OC1flag = 0;
                MotorStateReal[MOTOR1] = ACT_NOACT;
                MotorStateReal[MOTOR2] = ACT_XQ;
                act_step = 2;
            }
            break;
            
        case 2:
            if (OC1flag == 1)
            {
                OC1flag = 0;
                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;
            MotorStateReal[MOTOR1] = ACT_NOACT;
            act_step = 1;
            break;
        case 1:
            if (OC1flag == 1)
            {
                OC1flag = 0;
                MotorStateReal[MOTOR2] = ACT_NOACT;
                MotorStateReal[MOTOR1] = ACT_XH;
                act_step = 2;
            }
            break;
            
        case 2:
            if (OC1flag == 1)
            {
                OC1flag = 0;
                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]);
    //MOTOR1Ctrl(MotorState[MOTOR1]);
}

uint16_t getOC_th(void)
{
    if (MotorStateReal[MOTOR1] != ACT_NOACT)
    {
        return 100U;
    }
    else
    {
        return 70U;
    }
    //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;
    }

}