rgv-kinco/applications/nav/cargo.c

/*
 * cargo.c
 *
 *  Created on: 2018��6��23��
 *      Author: Eric
 */
#include "cargo.h"
#include "env.h"
#include "lift.h"

#define DBG_SECTION_NAME  "cargo"
#define DBG_LEVEL         DBG_INFO
#include <rtdbg.h>
/**
 * @brief �ղ���
 * @param
 * @retval 1 ���
 */
uint8_t Cargo_FuncNop(void) {
//	LOG_I("Cargo_FuncNop");
    return 1;
}
uint8_t Cargo_CheckStatusNop(void) {
    LOG_E("Cargo_CheckStatusNop not set");
    return 1;
}
uint8_t Cargo_ToHomeNop(void) {
    LOG_E("Cargo_ToHomeNop not set");
    return 1;
}
uint8_t Cargo_PickupNop(void) {
    LOG_E("Cargo_PickupNop not set");
    return 1;
}
uint8_t Cargo_ReleaseNop(void) {
    LOG_E("Cargo_ReleaseNop not set");
    return 1;
}
/**
 * @brief ����ʱҪ���޻��ﲢ�ص�Ĭ��λ��
 * @param
 * @retval 1 ��ʼ����ɣ�0 ���ڳ�ʼ����2 ��ʼ��ʱ�л���
 */
Cargo_Function_t Cargo_ToHomePos = &Cargo_ToHomeNop;
Cargo_Function_t Cargo_Pickup = &Cargo_PickupNop;
Cargo_Function_t Cargo_Release = &Cargo_ReleaseNop;
Cargo_Function_t Cargo_CheckStatus = &Cargo_CheckStatusNop;
/**
 * @brief ������״̬
 * @param
 * @retval
 */
uint8_t Cargo_Process(void) {	//前后两个货物检测的开关
    Cargo_CheckStatus();
	if (CARGO_IS_IN_FORWARD_MID && CARGO_IS_IN_BACKWARD_MID && S.LiftStatus  == LIFT_STATUS_TOP) {
		S.CargoStatus = CARGO_STATUS_CENTER;
//		LOG_I("CARGO_STATUS_CENTER");
		return 0;
	}
	if ((CARGO_IS_IN_FORWARD_MID || CARGO_IS_IN_BACKWARD_MID) && S.LiftStatus == LIFT_STATUS_TOP) {
		S.CargoStatus = CARGO_STATUS_SIDE;
//		LOG_I("CARGO_STATUS_SIDE");
	}
	S.CargoStatus = CARGO_STATUS_NULL;
//	LOG_I("CARGO_STATUS_NULL");
	return 0;
}
/* ����------------------------------------------------------------------------*/
uint8_t Cargo_InitPosLift(void) {
    if(S.CargoStatus == CARGO_STATUS_NULL || (S.CargoStatus != CARGO_STATUS_NULL && S.LiftStatus != LIFT_STATUS_TOP)){
        LOG_I("Cargo_ToHomeLift ok");
        return Lift_Down();
    }
    if (S.CargoStatus != CARGO_STATUS_NULL && S.LiftStatus == LIFT_STATUS_TOP){
        LOG_E("STATUS_ERROR_CARGO_START_FULL");
        S.Status = STATUS_ERROR_CARGO_START_FULL;
        return 0;
    }
//		if (S.LiftStatus == LIFT_STATUS_BOTTOM) {
//			Lift_Stop();
//			return 1;
//		} else {
//			return Lift_Down();
//		}
    return 0;
}

uint8_t Cargo_PickupLift(void) {
	//�޻�����
//    if(S.LiftStatus == LIFT_STATUS_TOP){
//        return true;
//    }
	//�����
	if(S.TgtStation == Set.ChargeStation)
	{	
		return true;
	}	
    if(S.CargoStatus == CARGO_STATUS_CENTER && S.LiftStatus == LIFT_STATUS_TOP){
        return true;
    }
    if(S.CargoStatus != CARGO_STATUS_CENTER && S.LiftStatus == LIFT_STATUS_TOP){
    	S.Status = STATUS_ERROR_CARGO_PICKUP_FAIL;
    	return false;
    }
    Lift_Up();
    return false;
}

uint8_t Cargo_ReleaseLift(void) {
	//�޻�����
//    if(S.LiftStatus == LIFT_STATUS_BOTTOM){
//        return true;
//    }
	//�����
	if(S.TgtStation == Set.ChargeStation)
	{
		
		return true;
	}	
    if(S.CargoStatus == CARGO_STATUS_NULL && S.LiftStatus == LIFT_STATUS_BOTTOM){
        return true;
    }
    if(S.CargoStatus != CARGO_STATUS_NULL && S.LiftStatus == LIFT_STATUS_BOTTOM){
    	S.Status = STATUS_ERROR_CARGO_RELEASE_FAIL;
    	 return false;
    }
    Lift_Down();
    return false;
}

void Cargo_Init(void)
{
    Cargo_ToHomePos = &Cargo_InitPosLift;
    Cargo_Pickup = &Cargo_PickupLift;
    Cargo_Release = &Cargo_ReleaseLift;
    Cargo_CheckStatus = &Lift_Process;
    LOG_I("Cargo_Init");
    Cargo_Process();
    Cfg.CargoType = CFG_CARGO_TYPE_LIFT;
    LOG_I("Cargo type lift");
    Lift_Init();
}