/******************************************************************************************* * @file task_scheduler.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-T18:25:48+0800 * *******************************************************************************************/ #include "rtthread.h" #include "task_scheduler.h" #include "string.h" #include "env.h" #define DBG_TAG "task" #define DBG_LVL DBG_LOG #include enum { TASK_TRAJECTORY_EOK = 0, TASK_TRAJECTORY_EREPEAT = 0x02, /* 接收任务后者指令重复 */ TASK_TRAJECTORY_EFULL = 0x47, /* 任务接续时，小车当前已有两条任务，不接收新的任务 */ TASK_TRAJECTORY_EOVERFLOW = 0x68, /* 接收到WCS的任务节点个数超过RES自身设定的节点个数 */ TASK_TRAJECTORY_EXY = 0x61, /* 校验行驶指令，相邻的两坐标巷道坡道均不一致时错误编码 */ TASK_TRAJECTORY_ELINK = 0x50, /* 任务接续失败，路径起点坐标与上一任务终点坐标不一致 */ TASK_TRAJECTORY_EZ = 0x60, /* 校验行驶指令，相邻的两个坐标位置坐标层数不一致时错误编码 */ TASK_TRAJECTORY_EPASS = 0x62, /* 校验行驶指令，坐标节点位置信息不允许通过时错误编码 */ TASK_TRAJECTORY_ECUR = 0x64, /* 校验行驶指令，当前指令起点坐标与当前小车起点坐标不一致时错误编码 */ }; enum { TASK_SET_EOK = 0, TASK_SET_EDIFF = 0x48, /* 接收到的任务序号与RES内部缓存的任务不匹配 */ TASK_SET_EEXIST = 0x49, /* 接收到的任务还没有执行，又接收到新的任务 */ TASK_SET_EBUSY = 0x6a, /* 小车当前已有任务在执行 */ TASK_SET_ESEG = 0x71, /* 接收到的目的段序号小于上次的目的段序号或大于总的目的段序号） */ TASK_SET_EFINISH = 0x70, /* 当前任务已完成 */ TASK_SET_EEMPTY = 0x72, /* 当前小车没有任务 */ }; typedef struct { uint8_t id; uint8_t cnt; point_t point[TASK_MAX_POINT]; }task_trajectory_t; typedef struct { uint8_t status; uint8_t id; /* trajectory id */ uint8_t cur_point_idx; /* form 1-255 */ uint8_t target_point_idx; /* form 1-255 */ }task_t; static task_trajectory_t task_trajectory_list[TASK_MAX_TRAJECTORY] = {0}; static task_t task_sch = {0}; /** * @funtion task_current_trajectory * @brief 获取当前任务路径 * @Author Simon * @DateTime 2021.06.22-T17:56:13+0800 * * @return >=0-任务路径序号, <0-无 */ static int task_current_trajectory(void) { if(!task_trajectory_list[0].id && !task_trajectory_list[1].id) { return -1; } else if(!task_trajectory_list[0].id || !task_trajectory_list[1].id) { if(!task_trajectory_list[0].id) { return 1; } else { return 0; } } if(task_trajectory_list[0].id > task_trajectory_list[1].id) { if(task_trajectory_list[0].id != 255 && task_trajectory_list[1].id != 1) { return 1; } else { return 0; } } else { if(task_trajectory_list[1].id != 255 && task_trajectory_list[0].id != 1) { return 0; } else { return 1; } } } /** * @funtion task_add_trajectory * @brief 添加路径 * @Author Simon * @DateTime 2021.06.22-T17:57:21+0800 * * @param id 任务序号 * @param cnt 点数 * @param trajectory 路径点表 * @return * TASK_TRAJECTORY_EOK = 0, * TASK_TRAJECTORY_EREPEAT = 0x02, 接收任务后者指令重复 * TASK_TRAJECTORY_EFULL = 0x47, 任务接续时，小车当前已有两条任务，不接收新的任务 * TASK_TRAJECTORY_EOVERFLOW = 0x68, 接收到WCS的任务节点个数超过RES自身设定的节点个数 * TASK_TRAJECTORY_EXY = 0x61, 校验行驶指令，相邻的两坐标巷道坡道均不一致时错误编码 * TASK_TRAJECTORY_ELINK = 0x50, 任务接续失败，路径起点坐标与上一任务终点坐标不一致 * TASK_TRAJECTORY_EZ = 0x60, 校验行驶指令，相邻的两个坐标位置坐标层数不一致时错误编码 * TASK_TRAJECTORY_EPASS = 0x62, 校验行驶指令，坐标节点位置信息不允许通过时错误编码 * TASK_TRAJECTORY_ECUR = 0x64, 校验行驶指令，当前指令起点坐标与当前小车起点坐标不一致时错误编码 */ int task_add_trajectory(uint8_t id, uint8_t cnt, point_t *trajectory) { uint8_t i; uint8_t push_idx; uint8_t point_idx = 0; if(cnt > TASK_MAX_POINT) { return TASK_TRAJECTORY_EOVERFLOW; } /* 任务满判断 */ for(push_idx = 0; push_idx < TASK_MAX_TRAJECTORY; push_idx++) { if(!task_trajectory_list[push_idx].cnt) { break; } } if(push_idx >= TASK_MAX_TRAJECTORY) { LOG_W("trajectory full"); return TASK_TRAJECTORY_EFULL; } /* 起始点接续判断 */ if(!task_trajectory_list[0].id && !task_trajectory_list[1].id) { if(trajectory[0].x != S.xValue || trajectory[0].y != S.yValue) { LOG_W("trajectory start point is not at current position"); return TASK_TRAJECTORY_ECUR; } } if(task_trajectory_list[0].id > task_trajectory_list[1].id) { if(task_trajectory_list[0].id != 255 && task_trajectory_list[1].id != 1) { if(memcmp(&task_trajectory_list[0].point[task_trajectory_list[0].cnt - 1], &trajectory[0], sizeof(point_t))) { LOG_W("points link error"); return TASK_TRAJECTORY_ELINK; } } } if(task_trajectory_list[1].id > task_trajectory_list[0].id) { if(task_trajectory_list[1].id != 255 && task_trajectory_list[0].id != 1) { if(memcmp(&task_trajectory_list[1].point[task_trajectory_list[1].cnt - 1], &trajectory[0], sizeof(point_t))) { LOG_W("points link error"); return TASK_TRAJECTORY_ELINK; } } } /* 起点判断 */ /* 改进起点判断*/ if(S.xValue != trajectory[0].x || S.yValue != trajectory[0].y) { LOG_W("start points are no same"); return TASK_TRAJECTORY_ECUR; } /* 路径直线判断 */ for(i = 1; i < cnt; i++) { if(trajectory[i].z == trajectory[i - 1].z) { if(trajectory[i].x != trajectory[i -1].x && trajectory[i].y != trajectory[i - 1].y) { LOG_W("points are not not in line"); return TASK_TRAJECTORY_EXY; } } else { LOG_W("points are not in same floor"); return TASK_TRAJECTORY_EZ; } } /* 是否当前层 */ if(trajectory[0].z != S.cValue) { LOG_W("points are not in current floor"); return TASK_TRAJECTORY_EPASS; } /* 路径重复判断 */ for(i = 0; i < TASK_MAX_TRAJECTORY; i++) { if(id == task_trajectory_list[i].id) { LOG_W("trajectory repeat"); return TASK_TRAJECTORY_EREPEAT; } } for(i = 0; i < TASK_MAX_TRAJECTORY; i++) { if(task_trajectory_list[i].cnt == cnt) { if(memcmp(trajectory, task_trajectory_list[i].point, cnt * sizeof(point_t))) { LOG_W("trajectory repeat"); return TASK_TRAJECTORY_EREPEAT; } } } /* 插入路径，无动作：运动作为一个命令，有动作：运动作为一个命令，动作作为一个命令 */ for(i = 0; i < cnt; i++) { task_trajectory_list[push_idx].point[point_idx] = trajectory[i]; if(trajectory[i].action) { task_trajectory_list[push_idx].point[point_idx].action = 0; point_idx++; task_trajectory_list[push_idx].point[point_idx] = trajectory[i]; } point_idx++; if(point_idx > TASK_MAX_POINT) { task_trajectory_list[push_idx].id = 0; task_trajectory_list[push_idx].cnt = 0; LOG_W("points overflow"); return TASK_TRAJECTORY_EOVERFLOW; } } task_trajectory_list[push_idx].id = id; task_trajectory_list[push_idx].cnt = point_idx; LOG_I("add trajectory id[%u], points[%u->%u], target[%u, %u, %u]", id, cnt, point_idx, task_trajectory_list[push_idx].point[point_idx - 1].x, task_trajectory_list[push_idx].point[point_idx - 1].y, task_trajectory_list[push_idx].point[point_idx - 1].z); return TASK_TRAJECTORY_EOK; } /** * @funtion task_set * @brief 设置任务 * @Author Simon * @DateTime 2021.06.22-T17:59:45+0800 * * @param id 任务序号 * @param target_id 执行节点 * @return * TASK_SET_EOK = 0, * TASK_SET_EDIFF = 0x48, 接收到的任务序号与RES内部缓存的任务不匹配 * TASK_SET_EEXIST = 0x49, 接收到的任务还没有执行，又接收到新的任务 * TASK_SET_EBUSY = 0x6a, 小车当前已有任务在执行 * TASK_SET_ESEG = 0x71, 接收到的目的段序号小于上次的目的段序号或大于总的目的段序号） * TASK_SET_EFINISH = 0x70, 当前任务已完成 * TASK_SET_EEMPTY = 0x72, 当前小车没有任务 */ int task_set(uint8_t id, uint8_t target_id) { int cur_traj; if(id != task_trajectory_list[0].id && id != task_trajectory_list[1].id) { return TASK_SET_EDIFF; } cur_traj = task_current_trajectory(); if(cur_traj < 0) { return TASK_SET_EEMPTY; } if(id != task_trajectory_list[cur_traj].id) { return TASK_SET_EDIFF; } if(task_sch.status == TASK_STATUS_INIT) { return TASK_SET_EEXIST; } if(task_sch.status == TASK_STATUS_EXE || task_sch.status == TASK_STATUS_PAUSE) { return TASK_SET_EBUSY; } // if(id == task_sch.id && task_sch.status == TASK_STATUS_DONE && target_id == task_sch.target_point_idx) // { // return TASK_SET_EFINISH; // } /* 接收到的目的段序号小于上次的目的段序号或大于总的目的段序号 */ if(id == task_sch.id) { if(target_id < task_sch.target_point_idx) { return TASK_SET_ESEG; } } if(target_id > task_trajectory_list[cur_traj].cnt) { return TASK_SET_ESEG; } task_sch.target_point_idx = target_id; task_sch.cur_point_idx = 1; task_sch.id = id; task_sch.status = TASK_STATUS_INIT; return TASK_SET_EOK; } /** * @funtion task_get * @brief 获取目标 * @Author Simon * @DateTime 2021.06.22-T18:01:34+0800 * * @param cur_point 当前坐标 * @param trajectory 输出路径 * @return 路径坐标点数，0-无任务 */ int task_get(point_t cur_point, point_t *trajectory) { uint8_t i; uint8_t idx; if(task_sch.status == TASK_STATUS_NONE || task_sch.status == TASK_STATUS_DONE) { return 0; } for(idx = 0; idx < TASK_MAX_TRAJECTORY; idx++) { if(task_trajectory_list[idx].id == task_sch.id) { break; } } if(idx >= TASK_MAX_TRAJECTORY) { task_sch.status = TASK_STATUS_NONE; return 0; } for(i = 0; i < task_sch.target_point_idx; i++) { if(cur_point.x == task_trajectory_list[idx].point[i].x && cur_point.y == task_trajectory_list[idx].point[i].y) { break; } } if(i >= task_sch.target_point_idx) { return 0; } memcpy(trajectory, &task_trajectory_list[idx].point[i], (task_sch.target_point_idx - i) * sizeof(point_t)); return task_sch.target_point_idx - i; } /** * @funtion task_set_status * @brief 设置任务状态 * @Author Simon * @DateTime 2021.06.22-T18:02:59+0800 * * @param status 0：执行中，1：完成，2：暂停 * @param cur_point 当前坐标点 */ void task_set_status(uint8_t status, point_t cur_point) { int cur_traj; uint8_t i; cur_traj = task_current_trajectory(); if(cur_traj < 0) { return; } if(task_trajectory_list[cur_traj].id != task_sch.id) { return; } switch(status) { case 0: task_sch.status = TASK_STATUS_EXE; break; case 1: task_sch.status = TASK_STATUS_DONE; break; case 2: task_sch.status = TASK_STATUS_PAUSE; break; default: break; } if(task_sch.status == TASK_STATUS_DONE && task_trajectory_list[cur_traj].point[task_trajectory_list[cur_traj].cnt - 1].x == cur_point.x && task_trajectory_list[cur_traj].point[task_trajectory_list[cur_traj].cnt - 1].y == cur_point.y) { task_trajectory_list[cur_traj].cnt = 0; task_trajectory_list[cur_traj].id = 0; } else { for(i = 0; i < task_trajectory_list[cur_traj].cnt; i++) { if(task_trajectory_list[cur_traj].point[i].x == cur_point.x && task_trajectory_list[cur_traj].point[i].y == cur_point.y) { task_sch.cur_point_idx = i + 1; break; } } if(task_sch.cur_point_idx >= task_trajectory_list[cur_traj].cnt) { task_trajectory_list[cur_traj].cnt = 0; task_trajectory_list[cur_traj].id = 0; } } } /** * @funtion task_get_status * @brief 获取任务状态 * @Author Simon * @DateTime 2021.06.22-T18:03:41+0800 * * @return * TASK_STATUS_NONE, 无任务 * TASK_STATUS_INIT, 任务待命 * TASK_STATUS_EXE, 任务执行中 * TASK_STATUS_PAUSE, 任务暂停 * TASK_STATUS_DONE, 任务完成 */ uint8_t task_get_status(void) { return task_sch.status; } /** * @funtion task_get_seg * @brief 获取当前节点号 * @Author Simon * @DateTime 2021.06.22-T18:04:44+0800 * * @return 当前节点号 */ uint8_t task_get_seg(void) { if(!task_sch.id) { return 0; } return task_sch.cur_point_idx; } /** * @funtion task_get_target * @brief 获取任务终点坐标 * @Author Simon * @DateTime 2021.06.22-T18:05:15+0800 * * @return 终点坐标 */ uint32_t task_get_target(void) { int idx = task_current_trajectory(); if(idx < 0) return 0; return *(uint32_t *)&task_trajectory_list[idx].point[task_trajectory_list[idx].cnt - 1]; } /** * @funtion task_cancel * @brief 撤消任务 * @Author Simon * @DateTime 2021.06.22-T18:05:53+0800 * * @param force 1强制撤消, 0非强制 * @return 0-成功，-1-失败 */ int task_cancel(int force) { if(task_sch.status != TASK_STATUS_EXE) { if(task_sch.status != TASK_STATUS_PAUSE || force) { task_sch.id = 0; task_sch.status = TASK_STATUS_NONE; task_trajectory_list[0].id = 0; task_trajectory_list[0].cnt = 0; task_trajectory_list[1].id = 0; task_trajectory_list[1].cnt = 0; return 0; } } return -1; } int task_init(void) { task_sch.status = TASK_STATUS_NONE; return 0; } INIT_APP_EXPORT(task_init);