rgv-kinco/applications/wcs/command.c

/*******************************************************************************************
 * @file command.c
 *
 * @brief 小车指令
 *
 *               (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.17-T14:17:29+0800
 *
*******************************************************************************************/
#include "rtthread.h"
#include "lwip/netdb.h"
#include "stdint.h"
#include "wcs.h"
#include "task_scheduler.h"
#include "board.h"
#include "env.h"
#include "cargo.h"
#include "guide.h"
#include "lift.h"
#include "music.h"
#include "dm_scaner.h"


#define DBG_TAG                "wcs.cmd"
#define DBG_LVL           DBG_LOG
#include <rtdbg.h>

#define CMD_USING_TASK
enum
{
    WCS_CMD_PICK = 0x01,	    /* 托盘取货 */
    WCS_CMD_RELEASE = 0x02,	    /* 托盘放货 */
    WCS_CMD_OPEN_CHARGE = 0x03,	    /* 开始充电 */
    WCS_CMD_CLOSE_CHARGE = 0x04,	/* 关闭充电 */
    WCS_CMD_STEER_RAMP = 0x05,	/* 换向到坡道 */
    WCS_CMD_STEER_TUNNEL = 0x06,/* 换向到巷道 */

    WCS_CMD_PALLET_CAL = 0x08,	/* 托盘校准 */
    WCS_CMD_PALLET_CAL_LIFT = 0x09,	/* 托盘校准+托盘顶升 */
//    WCS_CMD_FORWARD_DST = 0x11,	/* 按1方向距离行驶 */
//    WCS_CMD_BACKWARD_DST = 0x12,	/* 按2方向距离行驶 */
//    WCS_CMD_LEFT_DST = 0x13,	/* 按3方向距离行驶 */
//    WCS_CMD_RIGHT_DST = 0x14,	/* 按4方向距离行驶 */
//    WCS_CMD_SYNC_TIME = 0x15,	/* 时间同步 */

    WCS_CMD_FORWARD_AUTO = 0x31,/* 1方向长感应停 */
    WCS_CMD_BACKWARD_AUTO = 0x32,/* 2方向长感应停 */
    WCS_CMD_LEFT_AUTO = 0x33,	/* 3方向长感应停 */
    WCS_CMD_RIGHT_AUTO = 0x34,	/* 4方向长感应停 */

    WCS_CMD_FORWARD_SPD = 0x41,	/* 按1方向速度行驶 */
    WCS_CMD_BACKWARD_SPD = 0x42,/* 按2方向速度行驶 */
    WCS_CMD_LEFT_SPD = 0x43,	/* 按3方向速度行驶 */
    WCS_CMD_RIGHT_SPD = 0x44,	/* 按4方向速度行驶 */

    WCS_CMD_RELOCATE = 0x50,	/* 更改小车坐标 */
    WCS_CMD_CAL_LOCAT = 0x51,	/* 校准位置 */

    WCS_CMD_STOP = 0x81,	    /* 小车急停 */
    WCS_CMD_RECOVERY = 0x82,	/* 小车暂停恢复 */
//    WCS_CMD_SPD = 0x83,	        /* 更改小车行驶速度 */
//    WCS_CMD_SPD_AT_AREA = 0x84,	/* 更改小车特殊位置行驶速度 */
    WCS_CMD_OPEN_BEEP = 0x85,	/* 打开小车蜂鸣器 */
    WCS_CMD_CLOSE_BEEP = 0x86,	/* 关闭小车蜂鸣器 */
    WCS_CMD_PAUSE = 0x87,	    /* 小车行驶暂停 */
//    WCS_CMD_PAUSE_AT_SPD = 0x88,/* 小车按速度停止 */
    WCS_CMD_INIT = 0x8e,	    /* 初始化指令 */
    WCS_CMD_CANCEL = 0x8f,	    /* 任务撤销 */
    WCS_CMD_CHK_PALLET = 0x91,	/* 查询小车托盘有无 */
    WCS_CMD_LIFT_FLOOR = 0x92,	/* 下发提升机当前层 */
//    WCS_CMD_SHUTDOWN = 0x96,	/* 小车系统关机 */
    WCS_CMD_REBOOT = 0x97,	    /* 小车系统重启 */
//    WCS_CMD_UPDATE_MAP = 0x98,	/* 发送地图 */
};

typedef struct
{
    uint8_t cmd;
    uint8_t cmd_no;
    int status;
    int result;
    uint32_t timeout_tick;
}cmd_delay_work_t;

static cmd_delay_work_t cmd_delay_work = {0};
static void (*cmd_clear_func)(void) = NULL;	/* 清状态回调函数，wcs.c调用 */

__weak void Music_Enable(int enable)
{
}

/**
 * @funtion cmd_set_clear_indication
 * @brief 设置清状态回调函数
 * @Author Simon
 * @DateTime 2021.06.25-T17:20:35+0800
 *
 * @param   clear_func  description
 */
void cmd_set_clear_indication(void (*clear_func)(void))
{
    cmd_clear_func = clear_func;
}

/**
 * @funtion cmd_set_point
 * @brief 更改小车坐标
 * @Author Simon
 * @DateTime 2021.06.19-T15:29:34+0800
 *
 * @param   point  坐标点
 * @return  成功
 */
static int cmd_set_point(uint32_t point)
{
    wcs_point_t *ppoint;
    point = point >> 8;
    ppoint = (wcs_point_t *)&point;
    S.xValue = ppoint->x;
    S.yValue = ppoint->y;
    S.cValue = ppoint->z;
	if(ppoint->z)
	scan.Set_cValue = ppoint->z;
	else
	LOG_D("wcs Set err cValue");
    return ERR_C_SYSTEM_SUCCESS;
}

static int cmd_cancel_task(void)
{
    if(task_cancel(0) < 0)
        return ERR_C_RES_REVOCATION;
    return ERR_C_SYSTEM_SUCCESS;
}

static int cmd_initial(void)
{
    if(S.Status != STATUS_ESTOP)
        S.Status = STATUS_INIT;
    if(cmd_clear_func)
    {
        cmd_clear_func();
    }
    if(task_cancel(1) < 0)
        return ERR_C_RES_REVOCATION;
    GDClearPauseTask();
    Lift_ManualDown();
    cmd_delay_work.cmd = WCS_CMD_INIT;
    cmd_delay_work.status = 0;
    cmd_delay_work.result = ERR_C_SYSTEM_RECV_SUCCESS;
    cmd_delay_work.timeout_tick = rt_tick_get() + 5000;
    return ERR_C_SYSTEM_RECV_SUCCESS;
}

static void cmd_initial_delay_work(void)
{
    if(Lift_ManualDown())
    {
        cmd_delay_work.result = ERR_C_SYSTEM_SUCCESS;
    }
    else
    {
        if(rt_tick_get() > cmd_delay_work.timeout_tick)
        {
            cmd_delay_work.result = ERR_C_RES_TRAVEL_TIME_OUT;
            LOG_W("release timeout");
        }
    }
}

static int cmd_pickup(void)
{
    Lift_ManualUp();
    cmd_delay_work.cmd = WCS_CMD_PICK;
    cmd_delay_work.status = 0;
    cmd_delay_work.result = ERR_C_SYSTEM_RECV_SUCCESS;
    cmd_delay_work.timeout_tick = rt_tick_get() + 5000;
    return ERR_C_SYSTEM_RECV_SUCCESS;
}

static void cmd_pickup_delay_work(void)
{
    if(Lift_ManualUp())
    {
        cmd_delay_work.result = ERR_C_SYSTEM_SUCCESS;
    }
    else
    {
        if(rt_tick_get() > cmd_delay_work.timeout_tick)
        {
            cmd_delay_work.result = ERR_C_RES_TRAVEL_TIME_OUT;
            LOG_W("pickup timeout");
        }
    }
}

static int cmd_release(void)
{
    Lift_ManualDown();
    cmd_delay_work.cmd = WCS_CMD_RELEASE;
    cmd_delay_work.status = 0;
    cmd_delay_work.result = ERR_C_SYSTEM_RECV_SUCCESS;
    cmd_delay_work.timeout_tick = rt_tick_get() + 5000;
    return ERR_C_SYSTEM_RECV_SUCCESS;
}

static void cmd_release_delay_work(void)
{
    if(Lift_ManualDown())
    {
        cmd_delay_work.result = ERR_C_SYSTEM_SUCCESS;
    }
    else
    {
        if(rt_tick_get() > cmd_delay_work.timeout_tick)
        {
            cmd_delay_work.result = ERR_C_RES_TRAVEL_TIME_OUT;
            LOG_W("release timeout");
        }
    }
}

static int cmd_charge_open(void)
{
    BTY_AUTO_ON;
    return ERR_C_SYSTEM_SUCCESS;
}

static int cmd_charge_close(void)
{
    BTY_AUTO_OFF;
    return ERR_C_SYSTEM_SUCCESS;
}

static int cmd_ramp(void)
{
    Dir_Lift_LR();	//BUG2,左右换向
    cmd_delay_work.cmd = WCS_CMD_STEER_RAMP;
    cmd_delay_work.status = 0;
    cmd_delay_work.result = ERR_C_SYSTEM_RECV_SUCCESS;
    cmd_delay_work.timeout_tick = rt_tick_get() + 5000;
    return ERR_C_SYSTEM_RECV_SUCCESS;
}

static void cmd_ramp_delay_work(void)
{
    if(Dir_Lift_LR())//BUG2左右换向
    {
        cmd_delay_work.result = ERR_C_SYSTEM_SUCCESS;
    }
    else
    {
        if(rt_tick_get() > cmd_delay_work.timeout_tick)
        {
            cmd_delay_work.result = ERR_C_RES_TRAVEL_TIME_OUT;
            LOG_W("steer ramp timeout");
        }
    }
}

static int cmd_tunnel(void)
{
    Dir_Lift_FB();//BUG2
    cmd_delay_work.cmd = WCS_CMD_STEER_TUNNEL;
    cmd_delay_work.status = 0;
    cmd_delay_work.result = ERR_C_SYSTEM_RECV_SUCCESS;
    cmd_delay_work.timeout_tick = rt_tick_get() + 5000;
    return ERR_C_SYSTEM_RECV_SUCCESS;
}

static void cmd_tunnel_delay_work(void)
{
    if(Dir_Lift_FB())//BUG2
    {
        cmd_delay_work.result = ERR_C_SYSTEM_SUCCESS;
    }
    else
    {
        if(rt_tick_get() > cmd_delay_work.timeout_tick)
        {
            cmd_delay_work.result = ERR_C_RES_TRAVEL_TIME_OUT;
            LOG_W("steer tunnel timeout");
        }
    }
}

static void cmd_reboot(void)
{
    cmd_delay_work.cmd = WCS_CMD_REBOOT;
    cmd_delay_work.status = 0;
    cmd_delay_work.result = ERR_C_SYSTEM_RECV_SUCCESS;
    cmd_delay_work.timeout_tick = rt_tick_get() + 1000;
}

static void cmd_reboot_delay_work(void)
{
    if(rt_tick_get() > cmd_delay_work.timeout_tick)
    {
        LOG_I("reboot now...");
        rt_hw_cpu_reset();
    }
}

int cmd_parser(uint8_t cmd_no, uint8_t cmd, uint32_t *param)
{
    int rc = ERR_C_TRAVEL_ERR_HAVE_NOCMD;
    if(cmd_delay_work.cmd == cmd)
    {
        return ERR_C_TRAVEL_ERR_CMDREP;
    }
    cmd_delay_work.cmd_no = cmd_no;
    switch(cmd)
    {
        case WCS_CMD_RELOCATE:
            rc = cmd_set_point(*param);
            break;
        case WCS_CMD_STOP:
            GDEStop();
            rc = ERR_C_SYSTEM_SUCCESS;
            break;
        case WCS_CMD_PAUSE:
            GDPause();
            rc = ERR_C_SYSTEM_SUCCESS;
            break;
        case WCS_CMD_RECOVERY:
            GDResume();
            rc = ERR_C_SYSTEM_SUCCESS;
            break;
        case WCS_CMD_LIFT_FLOOR:
            S.TsjFloorStatus = ntohl(*param);
            rc = ERR_C_SYSTEM_SUCCESS;
            break;
        case WCS_CMD_CHK_PALLET:
            if(S.CargoStatus == CARGO_STATUS_CENTER || S.CargoStatus == CARGO_STATUS_SIDE)
            {
                *param = htonl(1);
            }
            else
            {
                *param = 0;
            }
            rc = ERR_C_SYSTEM_SUCCESS;
            break;
        case WCS_CMD_OPEN_BEEP:
            Music_Enable(1);
            rc = ERR_C_SYSTEM_SUCCESS;
            break;
        case WCS_CMD_CLOSE_BEEP:
            Music_Enable(0);
            rc = ERR_C_SYSTEM_SUCCESS;
            break;
        /* 任务执行中返回ERR_C_RES_TASK_DOING */
        case WCS_CMD_PICK:
        case WCS_CMD_RELEASE:
        case WCS_CMD_OPEN_CHARGE:
        case WCS_CMD_CLOSE_CHARGE:
        case WCS_CMD_STEER_RAMP:
        case WCS_CMD_STEER_TUNNEL:
        case WCS_CMD_FORWARD_SPD:
        case WCS_CMD_FORWARD_AUTO:
        case WCS_CMD_BACKWARD_SPD:
        case WCS_CMD_BACKWARD_AUTO:
        case WCS_CMD_LEFT_SPD:
        case WCS_CMD_LEFT_AUTO:
        case WCS_CMD_RIGHT_SPD:
        case WCS_CMD_RIGHT_AUTO:
        case WCS_CMD_REBOOT:
        case WCS_CMD_PALLET_CAL:
        case WCS_CMD_CAL_LOCAT:
        case WCS_CMD_CANCEL:
        case WCS_CMD_INIT:
            if(task_get_status() == TASK_STATUS_EXE || task_get_status() == TASK_STATUS_PAUSE)
            {
                rc = ERR_C_RES_TASK_DOING;
            }
            break;
        default:

            break;
    }
    if(cmd == WCS_CMD_INIT)
    {
        if(task_get_status() != TASK_STATUS_EXE)
        {
            rc = cmd_initial();
        }
        else
        {
            rc = ERR_C_RES_TASK_DOING;
        }
    }
    if(task_get_status() != TASK_STATUS_EXE && task_get_status() != TASK_STATUS_PAUSE)
    {
        switch(cmd)
        {
            case WCS_CMD_PICK:
                rc = cmd_pickup();
                break;
            case WCS_CMD_RELEASE:
                rc = cmd_release();
                break;
            case WCS_CMD_OPEN_CHARGE:
                rc = cmd_charge_open();
                break;
            case WCS_CMD_CLOSE_CHARGE:
                rc = cmd_charge_close();
                break;
            case WCS_CMD_STEER_RAMP:
                rc = cmd_ramp();
                break;
            case WCS_CMD_STEER_TUNNEL:
                rc = cmd_tunnel();
                break;
            case WCS_CMD_FORWARD_SPD:
            case WCS_CMD_FORWARD_AUTO:
                GD_ManualForward();
                rc = ERR_C_SYSTEM_SUCCESS;
                break;
            case WCS_CMD_BACKWARD_SPD:
            case WCS_CMD_BACKWARD_AUTO:
                GD_ManualBackward();
                rc = ERR_C_SYSTEM_SUCCESS;
                break;
            case WCS_CMD_LEFT_SPD:
            case WCS_CMD_LEFT_AUTO:
                GD_ManualDriftLeft();
                rc = ERR_C_SYSTEM_SUCCESS;
                break;
            case WCS_CMD_RIGHT_SPD:
            case WCS_CMD_RIGHT_AUTO:
                GD_ManualDriftRight();
                rc = ERR_C_SYSTEM_SUCCESS;
                break;
            case WCS_CMD_REBOOT:
                cmd_reboot();
                rc = ERR_C_SYSTEM_SUCCESS;
                break;
            case WCS_CMD_PALLET_CAL:
                rc = ERR_C_SYSTEM_SUCCESS;
                break;
            case WCS_CMD_CAL_LOCAT:
                rc = ERR_C_SYSTEM_SUCCESS;
                break;
            case WCS_CMD_CANCEL:
                rc = cmd_cancel_task();
                break;
            default:
                break;
        }
    }
    return rc;
}

/**
 * @funtion cmd_work_result
 * @brief description
 * @Author Simon
 * @DateTime 2021.06.25-T9:33:43+0800
 *
 * @param   cmd_no  指令序号输出
 * @return  >=0 指令完成结果, -1 无指令
 */
int cmd_work_result(uint8_t *cmd_no)
{
    int rc = -1;
    if(cmd_delay_work.cmd)
    {
        if(cmd_no)
        {
            *cmd_no = cmd_delay_work.cmd_no;
        }
        rc = cmd_delay_work.result;
        if(rc == ERR_C_SYSTEM_SUCCESS && cmd_no)
        {
            memset(&cmd_delay_work, 0, sizeof(cmd_delay_work_t));
        }
        /**/
//        if(rc == ERR_C_SYSTEM_RECV_SUCCESS)
//        {
//            rc = ERR_C_SYSTEM_SUCCESS;
//        }
    }
    return rc;
}

/**
 * @funtion cmd_delay_task
 * @brief 创建任务运行或放主循环运行
 * @Author Simon
 * @DateTime 2021.06.19-T18:16:51+0800
 *
 */
void cmd_delay_task(void)
{
    switch(cmd_delay_work.cmd)
    {
        case WCS_CMD_PICK:
            cmd_pickup_delay_work();
            break;
        case WCS_CMD_RELEASE:
            cmd_release_delay_work();
            break;
        case WCS_CMD_STEER_RAMP:
            cmd_ramp_delay_work();
            break;
        case WCS_CMD_STEER_TUNNEL:
            cmd_tunnel_delay_work();
            break;
        case WCS_CMD_REBOOT:
            cmd_reboot_delay_work();
            break;
        case WCS_CMD_INIT:
            cmd_initial_delay_work();
            break;
        default:
            break;
    }
}

#ifdef CMD_USING_TASK
static void cmd_task(void *param)
{
    while(1)
    {
        cmd_delay_task();
        rt_thread_delay(100);
    }
}

static int cmd_init(void)
{
    static rt_thread_t tid;
    tid = rt_thread_create("wcs_cmd", cmd_task, NULL, 512, 15, 10);
    if(tid)
    {
        rt_thread_startup(tid);
    }
    return RT_EOK;
}
INIT_APP_EXPORT(cmd_init);
#endif