/******************************************************************************************* * @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 #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