1 /*
2 * Copyright (c) 2023 HPMicro
3 *
4 * SPDX-License-Identifier: BSD-3-Clause
5 *
6 */
7
8 #include "hpm_mcl_path_plan.h"
9
hpm_mcl_path_init(mcl_path_plan_t * path,mcl_path_plan_cfg_t * cfg,mcl_cfg_t * mcl)10 hpm_mcl_stat_t hpm_mcl_path_init(mcl_path_plan_t *path, mcl_path_plan_cfg_t *cfg, mcl_cfg_t *mcl)
11 {
12 MCL_ASSERT(path != NULL, mcl_invalid_pointer);
13 MCL_ASSERT(cfg != NULL, mcl_invalid_pointer);
14 MCL_ASSERT(mcl != NULL, mcl_invalid_pointer);
15 path->pole_num = &mcl->physical.motor.pole_num;
16 path->cfg = cfg;
17 path->t_cure.next_empty = true;
18 path->t_cure.current = cfg->t_cure;
19 path->t_cure.ts = 0;
20 path->t_cure.current_empty = true;
21
22 return mcl_success;
23 }
24
hpm_mcl_path_update_t_cure(mcl_path_plan_t * path,path_plan_t_cure_cfg_t * cfg)25 hpm_mcl_stat_t hpm_mcl_path_update_t_cure(mcl_path_plan_t *path, path_plan_t_cure_cfg_t *cfg)
26 {
27 MCL_ASSERT_OPT(path != NULL, mcl_invalid_pointer);
28
29 if (path->t_cure.current_empty) {
30 path->t_cure.current_empty = false;
31 path->t_cure.current = *cfg;
32 } else if (path->t_cure.next_empty) {
33 path->t_cure.next_empty = false;
34 path->t_cure.next = *cfg;
35 } else {
36 return mcl_fail;
37 }
38
39 return mcl_success;
40 }
41
hpm_mcl_path_get_current_theta(mcl_path_plan_t * path)42 float hpm_mcl_path_get_current_theta(mcl_path_plan_t *path)
43 {
44 return path->theta.current;
45 }
46
hpm_mcl_path_get_next_theta(mcl_path_plan_t * path)47 float hpm_mcl_path_get_next_theta(mcl_path_plan_t *path)
48 {
49 return path->theta.next;
50 }
51
hpm_mcl_path_get_last_theta(mcl_path_plan_t * path)52 float hpm_mcl_path_get_last_theta(mcl_path_plan_t *path)
53 {
54 return path->theta.last;
55 }
56
hpm_mcl_path_t_cure_complete(mcl_path_plan_t * path)57 bool hpm_mcl_path_t_cure_complete(mcl_path_plan_t *path)
58 {
59 if ((path->t_cure.current_empty == true) &&
60 (path->t_cure.next_empty == true)) {
61 return true;
62 } else {
63 return false;
64 }
65 }
66
hpm_mcl_path_t_cure_generate(mcl_path_plan_t * path)67 hpm_mcl_stat_t hpm_mcl_path_t_cure_generate(mcl_path_plan_t *path)
68 {
69 float t0, t1, t2;
70 int32_t num0, num1, num2;
71 float theta;
72
73 MCL_ASSERT_OPT(path != NULL, mcl_invalid_pointer);
74 MCL_ASSERT_OPT(path->cfg->loop_ts != 0, mcl_invalid_argument);
75 if (path->t_cure.current_empty) {
76 path->t_cure.ts = 0;
77 return mcl_success;
78 }
79 if (path->t_cure.ts == 0) {
80 if (path->t_cure.current.time != 0) {
81 t0 = path->t_cure.current.acc_time;
82 t2 = path->t_cure.current.dec_time;
83 t1 = path->t_cure.current.time - t0 - t2;
84 num0 = t0 / path->cfg->loop_ts;
85 num1 = t1 / path->cfg->loop_ts;
86 num2 = t2 / path->cfg->loop_ts;
87
88 path->t_cure.t0 = num0;
89 path->t_cure.t1 = num0 + num1;
90 path->t_cure.t2 = num0 + num1 + num2;
91 path->t_cure.a0 = path->t_cure.current.acc * path->cfg->loop_ts * 0.5f;
92 path->t_cure.a1 = ((path->t_cure.current.speed * t1) / num1);
93 path->t_cure.a2 = path->t_cure.current.dec * path->cfg->loop_ts * 0.5f;
94 } else {
95 path->t_cure.t0 = 0;
96 path->t_cure.t1 = 0;
97 path->t_cure.t2 = 0;
98 path->t_cure.a0 = 0;
99 path->t_cure.a1 = 0;
100 path->t_cure.a2 = 0;
101 }
102 }
103 path->theta.last = path->theta.current;
104 path->theta.current = path->theta.next;
105 path->t_cure.ts += 1;
106
107 if (path->t_cure.ts > path->t_cure.t2) {
108 theta = path->theta.current;
109 if (path->t_cure.next_empty == false) {
110 path->t_cure.current = path->t_cure.next;
111 path->t_cure.next_empty = true;
112 path->t_cure.ts = 0;
113 } else {
114 path->t_cure.current_empty = true;
115 path->t_cure.current.time = 0;
116 path->t_cure.ts = 0;
117 }
118 } else if (path->t_cure.ts > path->t_cure.t1) {
119 theta = path->t_cure.a2 * (path->cfg->loop_ts *(path->t_cure.t2 - path->t_cure.ts) * 2 - path->cfg->loop_ts);
120 theta *= *path->pole_num;
121 theta += path->theta.next;
122 } else if (path->t_cure.ts > path->t_cure.t0) {
123 theta = path->t_cure.a1;
124 theta *= *path->pole_num;
125 theta += path->theta.next;
126 } else {
127 theta = path->t_cure.a0 * (path->cfg->loop_ts * path->t_cure.ts * 2 - path->cfg->loop_ts);
128 theta *= *path->pole_num;
129 theta += path->theta.next;
130 }
131 path->theta.next = MCL_ANGLE_MOD_X(0, MCL_PI * 2, theta);
132
133 return mcl_success;
134 }
135