rgv-kinco/applications/nav/lift.c

/**
 *********************************************************************************************************
 *                                                xmk guide
 *
 *                                 (c) Copyright 2016-2020, hualijidian.com
 *                                           All Rights Reserved
 *
 * @file     lift.c
 * @author   eric
 * @brief
 * @date     2018��2��1��
 * @version  V0.0.1
 *********************************************************************************************************
 */
#include "lift.h"
#include "guide.h"
#include "env.h"
#include "screen.h"
#include "systick.h"

#define DBG_SECTION_NAME  "lift"
#define DBG_LEVEL         DBG_INFO
#include <rtdbg.h>

uint8_t Lift_Init(void) {
    LOG_D("LiftInit");
    S.LiftStatus = LIFT_STATUS_INIT;
    S.DirStatus = DIR_STATUS_INIT;
//    Usart_Config(USART8, 57600, USART_Parity_No); //ǰ���ŷ�����
    return 1;
}

/**
 * @brief ���״̬
 * @param
 * @retval
 */
uint8_t Lift_Process(void) {
    if(S.LiftStatus == LIFT_STATUS_UP){
        return Lift_Up();
    }
    if(S.LiftStatus == LIFT_STATUS_DOWN){
        return Lift_Down();
    }
    return 0;
}

/**
 * @brief ֹͣ
 * @param
 * @retval
 * @note
 */
uint8_t Lift_Motor_Up(void) {
    MOTOR1_EN1 = 1;
    MOTOR1_BK1 = 0;
    MOTOR1_FR1 = 1;	//唐山是1
    LIFT1_Speed(1);
    Set.LiftRpm = 2000;
    LIFT1_UP;
    return 1;
}
uint8_t Lift_Motor_Down(void) {
    MOTOR1_EN1 = 1;
    MOTOR1_BK1 = 0;
    MOTOR1_FR1 = 0;	//唐山是0
    LIFT1_Speed(1);
	Set.LiftRpm = -2000;
	LIFT1_DOWN;
    return 1;
}
uint8_t Lift_Stop(void) {
    MOTOR1_EN1 = 0;
    MOTOR1_BK1 = 1;
    MOTOR1_FR1 = 0;
    LIFT1_Speed(10);
	Set.LiftRpm = 0;
	LIFT1_STOP;
    return 1;
}

/**
 * @brief ����
 * @param
 * @retval
 * @note
 */
uint8_t Lift_Up(void) {
    static uint32_t intervalTime100ms;
    if(intervalTime100ms == Timer100ms){
        return 0;
    }
    intervalTime100ms = Timer100ms;
    if(LIFT1_IS_IN_TOP){
    	Lift_Stop();
        Screen_Icon_Lift(Icon_Clear);
        S.LiftStatus = LIFT_STATUS_TOP;
        LOG_D("Lift in top ");
        return 1;
    }
    Lift_Motor_Up();
    Screen_Icon_Lift(Icon_Up);
    if(S.LiftStatus != LIFT_STATUS_UP){
        LOG_D("S.LiftStatus -> LIFT_STATUS_UP;");
    }
    S.LiftStatus = LIFT_STATUS_UP;
    return 0;
}
uint8_t Lift_ManualUp(void) {
    if(LIFT1_IS_IN_TOP){
    	Lift_Stop();
        Screen_Icon_Lift(Icon_Clear);
        S.LiftStatus = LIFT_STATUS_TOP;
        LOG_D("Lift in top");
        return 1;
    }
    Lift_Motor_Up();
    Screen_Icon_Lift(Icon_Up);
    S.LiftStatus = LIFT_STATUS_MANUAL_UP;
    LOG_D("Lift_ManualUp");
    return 0;
}
/**
 * @brief �½�
 * @param
 * @retval
 * @note
 */
uint8_t Lift_Down(void) {
    static volatile uint32_t intervalTime100ms;
    if(intervalTime100ms == Timer100ms){
        return 0;
    }
    intervalTime100ms = Timer100ms;
    if(LIFT1_IS_IN_BOTTOM){
    	Lift_Stop();
        Screen_Icon_Lift(Icon_Clear);
        S.LiftStatus = LIFT_STATUS_BOTTOM;
        LOG_D("Lift in bottom");
        return 1;
    }
    if(S.LiftStatus != LIFT_STATUS_DOWN){
        LOG_D("S.LiftStatus ->Lift Down");
        S.LiftStatus = LIFT_STATUS_DOWN;
    }
    Lift_Motor_Down();
    Screen_Icon_Lift(Icon_Down);
    return 0;
}
uint8_t Lift_ManualDown(void) {
    if(LIFT1_IS_IN_BOTTOM){
    	Lift_Stop();
        Screen_Icon_Lift(Icon_Clear);
        S.LiftStatus = LIFT_STATUS_BOTTOM;
        LOG_D("Lift in bottom");
        return 1;
    }
    S.LiftStatus = LIFT_STATUS_MANUAL_DOWN;
    Screen_Icon_Lift(Icon_Down);
    LOG_D("Lift_ManualDown");
    Lift_Motor_Down();
    return 0;
}
//*********************˿�ܶ���************************************************************************
uint8_t Lift_Process_Screw(void) {
    if(S.LiftStatus == LIFT_STATUS_UP){
        return Lift_Up_Screw();
    }
    if(S.LiftStatus == LIFT_STATUS_DOWN){
        return Lift_Down_Screw();
    }
    return 0;
}
uint8_t Lift_Stop_Screw(void) {
	int32_t StopPos;
	if (S.LiftStatus == LIFT_STATUS_MANUAL_UP){
		StopPos = S.LiftPosL + 250000;
		if (StopPos > Set.LiftUpPos){
			StopPos = Set.LiftUpPos;
		}
		Set.LiftPos = StopPos;
	}
	if (S.LiftStatus == LIFT_STATUS_MANUAL_DOWN){
		StopPos = S.LiftPosL - 250000;
		if (StopPos < Set.LiftDownPos){
			StopPos = Set.LiftDownPos;
		}
		Set.LiftPos = StopPos;
	}
	S.LiftStatus = LIFT_STATUS_MANUAL_STOP;
	return 0;
}
uint8_t Lift_Up_Screw(void) {
    static uint32_t intervalTime100ms;
    if(intervalTime100ms == Timer100ms){
        return 0;
    }
    intervalTime100ms = Timer100ms;
    if(S.LiftPosL == Set.LiftUpPos && S.LiftPosR == Set.LiftUpPos){
        Screen_Icon_Lift(Icon_Clear);
        S.LiftStatus = LIFT_STATUS_TOP;
        LOG_D("Lift in top ");
        return 1;
    }
    Set.LiftPos = Set.LiftUpPos;
    Screen_Icon_Lift(Icon_Up);
    if(S.LiftStatus != LIFT_STATUS_UP){
        LOG_D("S.LiftStatus -> LIFT_STATUS_UP;");
    }
    S.LiftStatus = LIFT_STATUS_UP;
    return 0;
}
uint8_t Lift_ManualUp_Screw(void) {
    if(S.LiftPosL == Set.LiftUpPos && S.LiftPosR == Set.LiftUpPos){
        Screen_Icon_Lift(Icon_Clear);
        S.LiftStatus = LIFT_STATUS_TOP;
        LOG_D("Lift in top");
        return 1;
    }
    Set.LiftPos = Set.LiftUpPos;
    Screen_Icon_Lift(Icon_Up);
    S.LiftStatus = LIFT_STATUS_MANUAL_UP;
    LOG_D("Lift_ManualUp");
    return 0;
}
/**
 * @brief �½�
 * @param
 * @retval
 * @note
 */
uint8_t Lift_Down_Screw(void) {
    static volatile uint32_t intervalTime100ms;
    if(intervalTime100ms == Timer100ms){
        return 0;
    }
    intervalTime100ms = Timer100ms;
    if(S.LiftPosL == Set.LiftDownPos && S.LiftPosR == Set.LiftDownPos){
        Screen_Icon_Lift(Icon_Clear);
        S.LiftStatus = LIFT_STATUS_BOTTOM;
        LOG_D("Lift in bottom");
        return 1;
    }
    if(S.LiftStatus != LIFT_STATUS_DOWN){
        LOG_D("S.LiftStatus ->Lift Down");
        S.LiftStatus = LIFT_STATUS_DOWN;
    }
    Set.LiftPos = Set.LiftDownPos;
    Screen_Icon_Lift(Icon_Down);
    return 0;
}
uint8_t Lift_ManualDown_Screw(void) {
    if(S.LiftPosL == Set.LiftDownPos && S.LiftPosR == Set.LiftDownPos){
        Screen_Icon_Lift(Icon_Clear);
        S.LiftStatus = LIFT_STATUS_BOTTOM;
        LOG_D("Lift in bottom");
        return 1;
    }
    S.LiftStatus = LIFT_STATUS_MANUAL_DOWN;
    Screen_Icon_Lift(Icon_Down);
    LOG_D("Lift_ManualDown");
    Set.LiftPos = Set.LiftDownPos;
    return 0;
}
//*********************CSCF������************************************************************************
static uint32_t DirValveOffTimer;
uint8_t Dir_Lift_Process(void) {
//	LOG_D("S.DirStatus:%d \n\r",S.DirStatus);
	switch(S.DirStatus){
	case DIR_STATUS_INIT:
		if (DIR_LIFT2_IS_IN_FB) //顶升的前后限位，换向。左右限位，轮子左右走
		{
			S.DirStatus = DIR_STATUS_FB;
			//LOG_D("DirStatus in FB");
		}
		break;
	case DIR_STATUS_FB:
		if (DIR_LIFT2_IS_IN_LR) {
			S.DirStatus = DIR_STATUS_LR;
			//LOG_D("DirStatus in LR");
		}
//		DirValveOff();
		break;
	case DIR_STATUS_LR:
		if (DIR_LIFT2_IS_IN_FB) {
			S.DirStatus = DIR_STATUS_FB;
			//LOG_D("DirStatus in FB");
		}
//		DirValveOff();
		break;
	}
	return 0;
}
uint8_t DirValveOff(void){
	DirValveOffTimer++;
	if(DirValveOffTimer > 3){
	}
	return 0;
}
uint8_t DirValveOn(void){
	DirValveOffTimer = 0;
	return 0;
}
uint8_t Dir_Lift_Up(void) {
    MOTOR1_EN1 = 1;
    MOTOR1_BK1 = 0;
    MOTOR1_FR1 = 1;	//唐山是1
    LIFT1_Speed(1);
	Set.LiftRpm = 2000;
	DIR_LIFT2_UP;
    return 1;
}
uint8_t Dir_Lift_Down(void) {
    MOTOR1_EN1 = 1;
    MOTOR1_BK1 = 0;
    MOTOR1_FR1 = 0;//唐山是0
    LIFT1_Speed(1);
	Set.LiftRpm = -2000;
	DIR_LIFT2_DOWN;
    return 1;
}
uint8_t Dir_Lift_Stop(void) {
    MOTOR1_EN1 = 0;
    MOTOR1_BK1 = 1;
    MOTOR1_FR1 = 0;
    LIFT1_Speed(10);
	Set.LiftRpm = 0;
	DIR_LIFT2_STOP;
    return 1;
}
uint32_t DirLiftFbTimer = 0;
void Dir_Lift_FB_Timer(void){
    if(DIR_LIFT2_IS_IN_FB){
        if(DirLiftFbTimer != DIR_LIFT_TIMER){
            DirLiftFbTimer++;
        }
    }else{
        DirLiftFbTimer = 0;
    }
}
uint8_t Dir_Lift_FB(void) {
    static uint32_t intervalTime100ms;
    if(intervalTime100ms == Timer100ms){
        return 0;
    }
    intervalTime100ms = Timer100ms;
    if(DIR_LIFT2_IS_IN_FB){
        if(DirLiftFbTimer == DIR_LIFT_TIMER){
            Dir_Lift_Stop();
            Screen_Icon_Lift(Icon_Clear);
            S.DirStatus = DIR_STATUS_FB;
            LOG_D("DirStatus in fb ");
            return 1;
        }
    }
    Dir_Lift_Down();
    Screen_Icon_Lift(Icon_Up);
    if(S.DirLiftStatus != DIR_STATUS_DOWN){
        LOG_D("S.DIR_STATUS_FB -> DIR_STATUS_FB;");
    }
    S.DirLiftStatus = DIR_STATUS_DOWN;
    return 0;
}
uint8_t Dir_Lift_ManualFB(void) {
	if(S.Status != STATUS_ESTOP)
	S.Status = STATUS_REMOTE_MANUAL;
    if(DIR_LIFT2_IS_IN_FB){
        if(DirLiftFbTimer == DIR_LIFT_TIMER){
            Dir_Lift_Stop();
            Screen_Icon_Lift(Icon_Clear);
            S.DirStatus = DIR_STATUS_FB;
            LOG_D("DirStatus in fb ");
            return 1;
        }
    }
    Dir_Lift_Down();
    Screen_Icon_Lift(Icon_Up);
    S.DirLiftStatus = DIR_STATUS_MANUAL_DOWN;
    LOG_D("Dir_Lift_ManualDown");
    return 0;
}
/**
 * @brief 下降
 * @param
 * @retval
 * @note
 */
uint32_t DirLiftLrTimer = 0;
void Dir_Lift_LR_Timer(void){
    if(DIR_LIFT2_IS_IN_LR){
        if(DirLiftLrTimer != DIR_LIFT_TIMER){
            DirLiftLrTimer++;
        }
    }else{
        DirLiftLrTimer = 0;
    }
}
/**
 * @brief �½�
 * @param
 * @retval
 * @note
 */
uint8_t Dir_Lift_LR(void) {
    static volatile uint32_t intervalTime100ms;
    if(intervalTime100ms == Timer100ms){
        return 0;
    }
    intervalTime100ms = Timer100ms;
    if(DIR_LIFT2_IS_IN_LR){
        if(DirLiftLrTimer == DIR_LIFT_TIMER){
            Dir_Lift_Stop();
            Screen_Icon_Lift(Icon_Clear);
            S.DirStatus = DIR_STATUS_LR;
//            LOG_W("Dir in lr");
            return 1;
        }
    }
    if(S.DirLiftStatus != DIR_STATUS_UP){
        LOG_D("S.DirStatus ->Dir_Lift Up");
        S.DirLiftStatus = DIR_STATUS_UP;
    }
    Dir_Lift_Up();
    Screen_Icon_Lift(Icon_Down);
    return 0;
}
uint8_t Dir_Lift_ManualLR(void) {
	if(S.Status != STATUS_ESTOP)
	S.Status = STATUS_REMOTE_MANUAL;
    if(DIR_LIFT2_IS_IN_LR){
        if(DirLiftLrTimer == DIR_LIFT_TIMER){
            Dir_Lift_Stop();
            Screen_Icon_Lift(Icon_Clear);
            S.DirStatus = DIR_STATUS_LR;
            LOG_D("DirStatus in lr");
            return 1;
        }
    }
    S.DirLiftStatus = DIR_STATUS_UP;
    Screen_Icon_Lift(Icon_Down);
    LOG_D("Lift_ManualDown");
    Dir_Lift_Up();
    return 0;
}
uint8_t Decompression(uint8_t btn){
	if(btn == 1){
		DEC_PRESS_1;
	}
	if(btn == 2){
		DEC_PRESS_2;
	}
	if(btn == 0){
		DEC_PRESS_STOP;
	}
	return 0;
}