rgv-kinco/applications/wcs/low_level.c

/*******************************************************************************************
 * @file low_level.c
 *
 * @brief 底层输入接口，仅供wcs.c使用. 移植需重新实现
 *
 *               (c) Copyright 2021, Shandong Huali electromechanical Co., Ltd..
 *             This is protected by international copyright laws. Knowledge of the
 *             source code may not be used to write a similar product. This file may
 *             only be used in accordance with a license and should not be redistributed
 *             in any way. We appreciate your understanding and fairness.
 *
 *
 * @author      Simon
 * @date        Created: 2021.06.19-T11:16:46+0800
 *
*******************************************************************************************/
#include "rtthread.h"
#include "env.h"
#include "wcs.h"
#include "task_scheduler.h"
#include "bms.h"
#include "dm_scaner.h"

#define DBG_TAG                "low"
#define DBG_LVL           DBG_INFO
#include <rtdbg.h>


int cmd_work_result(uint8_t *cmd_no);
/**
 * @funtion ll_get_point
 * @brief 获取当前坐标
 * @Author Simon
 * @DateTime 2021.06.19-T15:30:54+0800
 *
 * @param   point  输出坐标点
 */
void ll_get_point(wcs_point_t *point)
{
//    point->x = S.yValue;
//    point->y = S.xValue;
    point->x = S.xValue;
    point->y = S.yValue;
	if(S.cValue == 0)
	{
		if(S.TagNum != Set.TsjStation)	//非提升机码
		{
			if(scan.cValue)
			point->z = scan.cValue;
		}
		else
		{
			if(scan.Set_cValue)
			point->z = scan.Set_cValue;
		}
		LOG_E("push 0 cValue");
	}
	else	point->z = S.cValue;		
	  
}

/**
 * @funtion ll_get_seg
 * @brief 获取当前节点序号
 * @Author Simon
 * @DateTime 2021.06.19-T15:31:18+0800
 *
 * @return  表示小车已完成的节点号
 */
uint8_t ll_get_seg(void)
{
    return task_get_seg();
}

/**
 * @funtion ll_get_target
 * @brief 获取当前段终点坐标
 * @Author Simon
 * @DateTime 2021.06.19-T15:32:28+0800
 *
 * @return  当前段终点坐标
 */
uint16_t ll_get_target(void)
{
    uint16_t target_xy = 0;
    uint8_t point_x;
    uint32_t point = task_get_target();
    wcs_point_t *ppiont = (wcs_point_t *)&point;
    point_x = ppiont->x;
    ppiont->x = ppiont->y;
    ppiont->y = point_x;
    target_xy = ppiont->x;
    target_xy = (target_xy << 8) | ppiont->y;
    return target_xy;
}

/**
 * @funtion ll_get_car_status
 * @brief 获取小车状态
 * @Author Simon
 * @DateTime 2021.06.19-T15:32:55+0800
 *
 * @return  1：任务执行中；2：指令执行中；3：就绪；
 *          4：暂停，5：充电中；（6：故障；7：警告 后续版本取消，放在故障描述字段）；
 *          8：驱动器重启状态；9：小车急停重启状态 11：节点待命；0：手动模式
 */
uint8_t ll_get_car_status(void)
{
    uint8_t status;
    status = task_get_status();
    if(S.Status == STATUS_ESTOP)
    {
        return 9;
    }
    else if(S.Status >= 0x20 || S.Status == STATUS_DRIVER_ERROR
        || S.Status == STATUS_ERROR_CARGO_PICKUP_FAIL || S.Status == STATUS_ERROR_CARGO_RELEASE_FAIL || S.Status == STATUS_ERROR_CARGO_START_FULL)
    {
        return 6;
    }
    else if(S.Status == STATUS_REMOTE_MANUAL)
    {
        return 0;
    }
    else if(!LIFT_V10)	//BUG15
    {
        return 5;
    }
    else if(status == TASK_STATUS_EXE)
    {
        return 1;
    }
    if(status == TASK_STATUS_INIT)
    {
        return 1;
    }
    else if(status == TASK_STATUS_PAUSE)
    {
        return 4;
    }
    else if(cmd_work_result(NULL) >= 0)
    {
        return 2;
    }

    if(task_get_target())
    {
        return 11;
    }
    return 3;
}

/**
 * @funtion ll_get_dtc
 * @brief 获取错误
 * @Author Simon
 * @DateTime 2021.06.22-T17:41:35+0800
 *
 * @return  0：无警告；   1：行驶控制板失联；
 *          2：液压控制板失联； 3：行驶驱动器失联；
 *          4：液压驱动器失联； 5：行驶驱动器出现异常 ；
 *          6：液压驱动器出现异常 ；7：小车充电异常；
 *          8：驱动器重启状态；
 *          11：指令暂停 12：行驶系统障碍物暂停 13：行驶系统托盘障碍物暂停
 */
uint8_t ll_get_dtc(void)
{
    if(S.Status == STATUS_DRIVER_ERROR)
    {
        return 5;
    }
    else if(S.Status == STATUS_STOP_OBS || S.Status == STATUS_STOP_BUMPER)
    {
        return 12;
    }
    else if(S.Status == STATUS_ERROR_CARGO_PICKUP_FAIL || S.Status == STATUS_ERROR_CARGO_RELEASE_FAIL || S.Status == STATUS_ERROR_CARGO_START_FULL)
    {
		LOG_I("S.Status:%d",S.Status);
        return 13;
    }
    return 0;
}

/**
 * @funtion ll_get_pallet
 * @brief 获取托板状态
 * @Author Simon
 * @DateTime 2021.06.19-T15:33:31+0800
 *
 * @return  0：托板降状态；1：托板升状态；2：异常状态；
 */
uint8_t ll_get_pallet(void)
{
    if(S.LiftStatus == LIFT_STATUS_BOTTOM
        || S.LiftStatus == LIFT_STATUS_DOWN
        || S.LiftStatus == LIFT_STATUS_MANUAL_DOWN
        || S.LiftStatus == LIFT_STATUS_INIT)
    {
        return 0;
    }
    else if(S.LiftStatus == LIFT_STATUS_TOP
        || S.LiftStatus == LIFT_STATUS_UP
        || S.LiftStatus == LIFT_STATUS_MANUAL_UP)
    {
        return 1;
    }
    else
    {
        return 2;
    }
}

/**
 * @funtion ll_get_battery
 * @brief 获取电量
 * @Author Simon
 * @DateTime 2021.06.19-T15:33:57+0800
 *
 * @return  0：表示获取电池电量失败；1-100：表示电量百分比
 */
uint8_t ll_get_battery(void)
{
	
	static uint8_t BatteryStatus = BATTERY_STATUS_FULL;
	if(get_bat_enable())
	{
		BatteryStatus = get_bat_rsoc();
	}
	else
	{
		if(S.BatteryVolt >= Cfg.VoltFull)	
		{
			BatteryStatus = BATTERY_STATUS_FULL;
		}
		else 
		if(S.BatteryVolt >= Cfg.VoltHigh && S.BatteryVolt < Cfg.VoltFull-30)	//bug8
		{
			BatteryStatus = BATTERY_STATUS_HIGH;
		}
		else 
		if(S.BatteryVolt >= Cfg.VoltMid && S.BatteryVolt < Cfg.VoltHigh-30)	//bug8
		{
			BatteryStatus = BATTERY_STATUS_MID;
		}
		else 
		if(S.BatteryVolt >= Cfg.VoltLow && S.BatteryVolt < Cfg.VoltMid-30)	//bug8
		{
			BatteryStatus = BATTERY_STATUS_LOW;
		}
		else 
		if(S.BatteryVolt >= Cfg.VoltWarn && S.BatteryVolt < Cfg.VoltLow-30)	//bug8
		{
			BatteryStatus = BATTERY_STATUS_WARN;
		}
		else
		if(S.BatteryVolt < Cfg.VoltWarn-30)	//bug8
		{
			BatteryStatus = BATTERY_STATUS_STOP;
		}
	}
	return S.BatteryStatus = BatteryStatus;
}

/**
 * @funtion ll_get_reserve
 * @brief 获取换向状态
 * @Author Simon
 * @DateTime 2021.06.20-T17:11:21+0800
 *
 * @return  0x24-坡道 0x13-巷道
 */
uint8_t ll_get_reserve(void)
{
    if(S.DirStatus == DIR_STATUS_FB)
    {
        return 0x13;
    }
    else
    {
        return 0x24;
    }
}

/**
 * @funtion ll_get_dir
 * @brief 获取行驶方向
 * @Author Simon
 * @DateTime 2021.06.20-T17:11:21+0800
 *
 * @return  0x24-坡道 0x13-巷道
 */
uint8_t ll_get_dir(void)
{
    if(S.Action == ACT_STOP || S.Action == ACT_MANUAL_STOP
        || S.Action == ACT_NULL || S.Action == ACT_RESET)
    {
        return 0;
    }
    else if(S.Direction == DIRECTION_NULL)
    {
        return 0;
    }
    else if(S.Direction == DIRECTION_FORWARD)
    {
        return 1;
    }
    else if(S.Direction == DIRECTION_BACKWARD)
    {
        return 2;
    }
    else if(S.Direction == DIRECTION_LEFT)
    {
        return 3;
    }
    else if(S.Direction == DIRECTION_RIGHT)
    {
        return 4;
    }
    else
    {
        return 0;
    }
}