rgv-kinco/applications/nav/obs.c

/*
 * obs.c
 *
 *  Created on: 2019��6��20��
 *      Author: Eric
 */

#include "obs.h"
#include "stdint.h"
#include "env.h"

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

uint8_t _getObsStatus_BF(void);
CheckObs_Fn_t ObsGetStatus;
uint8_t TrayObsNearF = 0;
uint8_t TrayObsNearB = 0;
uint8_t ObsInit(void) {
    ObsGetStatus = _getObsStatus_BF;
    return True;
}
uint8_t _getObsStatus_BF(void)
{
    //LogNoRepeatVar
    //;
    if(S.Status == STATUS_REMOTE_MANUAL){
        return OBS_STATUS_NULL;
    }
    switch(S.Direction){
    case DIRECTION_FORWARD:
        //避障停止
        if((S.LiftStatus == LIFT_STATUS_TOP && CARGO_IS_IN_FORWARD_STOP) || !I.OBS4In1){
            LOG_I("OBS_FORWARD");
            return OBS_STATUS_NEAR;
        }
        //托盘减速
        if(S.LiftStatus == LIFT_STATUS_TOP && (!I.OBS3In2 || !I.OBS4In2) && S.CargoStatus == CARGO_STATUS_CENTER){
            TrayObsNearF = OBS_STATUS_FAR;
        }
        //避障减速
        if(TrayObsNearF == OBS_STATUS_FAR || !I.OBS3In1){
            LOG_D("OBS_FORWARD_LowSpeed");
            return OBS_STATUS_FAR;
        }
        break;
    case DIRECTION_BACKWARD:
        //避障停止
        if((S.LiftStatus == LIFT_STATUS_TOP && CARGO_IS_IN_BACKWARD_STOP) || !I.OBS2In1){
            LOG_I("OBS_BACKWARD");
//            return OBS_STATUS_NEAR;
			//专为唐山修改,2排11列与3排11列不起作用
			if(S.Point != Set.TsjPoint && S.Point != (Set.TsjPoint - 1000))	//非提升机码
			{
				return OBS_STATUS_NEAR;
			} 
        }
        //托盘减速
        if(S.LiftStatus == LIFT_STATUS_TOP && (!I.OBS1In2 || !I.OBS2In2) && S.CargoStatus == CARGO_STATUS_CENTER){
            TrayObsNearB = OBS_STATUS_FAR;
        }
        //避障减速
        if(TrayObsNearB == OBS_STATUS_FAR || !I.OBS1In1){
            LOG_D("OBS_BACKWARD_LowSpeed");
            return OBS_STATUS_FAR;
        }
        break;
    case DIRECTION_LEFT:
        //避障停止
        if(!I.OBS4In3){
            LOG_I("OBS_LEFT");
            return OBS_STATUS_NEAR;
        }
        //避障减速
        if(!I.OBS2In3){
            LOG_D("OBS_LEFT_LowSpeed");
            return OBS_STATUS_FAR;
        }
        break;
    case DIRECTION_RIGHT:
        // 避障停止
        if(!I.OBS3In3){
            LOG_I("OBS_RIGHT");
            return OBS_STATUS_NEAR;
        }
        //避障减速
        if(!I.OBS1In3){
            LOG_D("OBS_RIGHT_LowSpeed");
            return OBS_STATUS_FAR;
        }
        break;
    default:
        TrayObsNearF = OBS_STATUS_NEAR;
        TrayObsNearB = OBS_STATUS_NEAR;
    }
    return OBS_STATUS_NULL;
}