rgv-kinco/applications/task_scheduler.c

/*******************************************************************************************
 * @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 <rtdbg.h>

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);