1 /* 2 * Copyright (c) 2023 HPMicro 3 * 4 * SPDX-License-Identifier: BSD-3-Clause 5 * 6 */ 7 #ifndef HPM_MCL_PATH_PLAN_H 8 #define HPM_MCL_PATH_PLAN_H 9 10 #include "hpm_mcl_common.h" 11 12 /** 13 * @brief Trapezoidal Curve Configuration 14 * 15 */ 16 typedef struct { 17 float acc; /**< acceleration rad/s^2 */ 18 float dec; /**< deceleration rad/s^2 */ 19 float speed; /**< running speed rsd/s */ 20 float time; /**< total time */ 21 float acc_time; /**< acceleration time, s */ 22 float dec_time; /**< deceleration time, s */ 23 } path_plan_t_cure_cfg_t;; 24 25 /** 26 * @brief Configuration of path planning 27 * 28 */ 29 typedef struct { 30 path_plan_t_cure_cfg_t t_cure; 31 float loop_ts; /**< Time of cycle call in s */ 32 } mcl_path_plan_cfg_t; 33 34 /** 35 * @brief Running data for path planning 36 * 37 */ 38 typedef struct { 39 mcl_path_plan_cfg_t *cfg; 40 int32_t *pole_num; 41 struct { 42 path_plan_t_cure_cfg_t next; 43 path_plan_t_cure_cfg_t current; 44 bool next_empty; 45 bool current_empty; 46 uint32_t ts; 47 uint32_t t0; 48 uint32_t t1; 49 uint32_t t2; 50 float a0; 51 float a1; 52 float a2; 53 } t_cure; 54 struct { 55 float next; 56 float last; 57 float current; 58 } theta; 59 } mcl_path_plan_t; 60 61 #ifdef __cplusplus 62 extern "C" { 63 #endif 64 65 /** 66 * @brief Initialising the data structure for path planning 67 * 68 * @param path @ref mcl_path_plan_t 69 * @param cfg @ref mcl_path_plan_cfg_t 70 * @param mcl @ref mcl_cfg_t 71 * @return hpm_mcl_stat_t 72 */ 73 hpm_mcl_stat_t hpm_mcl_path_init(mcl_path_plan_t *path, mcl_path_plan_cfg_t *cfg, mcl_cfg_t *mcl); 74 75 /** 76 * @brief Updating data for trapezoidal curves 77 * 78 * @param path @ref mcl_path_plan_t 79 * @param cfg @ref path_plan_t_cure_cfg_t 80 * @return hpm_mcl_stat_t 81 */ 82 hpm_mcl_stat_t hpm_mcl_path_update_t_cure(mcl_path_plan_t *path, path_plan_t_cure_cfg_t *cfg); 83 84 /** 85 * @brief Get the angle value after path planning 86 * 87 * @param path @ref mcl_path_plan_t 88 * @return theta, rad 89 */ 90 float hpm_mcl_path_get_current_theta(mcl_path_plan_t *path); 91 92 /** 93 * @brief Get the next angle value after path planning 94 * 95 * @param path @ref mcl_path_plan_t 96 * @return theta, rad 97 */ 98 float hpm_mcl_path_get_next_theta(mcl_path_plan_t *path); 99 100 /** 101 * @brief Get the last angle value after path planning 102 * 103 * @param path @ref mcl_path_plan_t 104 * @return theta, rad 105 */ 106 float hpm_mcl_path_get_last_theta(mcl_path_plan_t *path); 107 108 /** 109 * @brief Generate trapezoidal curves, which need to be called periodically, at the same time as the loop_ts configuration 110 * 111 * @param path @ref mcl_path_plan_t 112 * @return hpm_mcl_stat_t 113 */ 114 hpm_mcl_stat_t hpm_mcl_path_t_cure_generate(mcl_path_plan_t *path); 115 116 /** 117 * @brief Get whether the curve is complete or not 118 * 119 * @param path @ref mcl_path_plan_t 120 * @return true The generation of the current curve has been completed 121 * @return false Current curve generation in progress 122 */ 123 bool hpm_mcl_path_t_cure_complete(mcl_path_plan_t *path); 124 125 #ifdef __cplusplus 126 } 127 #endif 128 129 #endif 130