/
pmsm_limits.c
106 lines (88 loc) · 2.87 KB
/
pmsm_limits.c
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
#include "pmsm_limits_comp.h"
#include "commands.h"
#include "hal.h"
#include "math.h"
#include "defines.h"
#include "angle.h"
HAL_COMP(pmsm_limits);
// motor values
HAL_PIN(psi);
HAL_PIN(r);
HAL_PIN(ld);
HAL_PIN(lq);
HAL_PIN(polecount);
// sys limit
HAL_PIN(ac_volt);
// next min max out -> pid, curpid
HAL_PIN(next_max_cur);
HAL_PIN(next_max_torque);
HAL_PIN(next_min_cur);
HAL_PIN(next_min_torque);
// min max out @ current vel
HAL_PIN(max_cur);
HAL_PIN(max_torque);
HAL_PIN(min_cur);
HAL_PIN(min_torque);
// abs max out
HAL_PIN(abs_max_cur);
HAL_PIN(abs_max_torque);
HAL_PIN(abs_max_vel);
// pmsm feedback
HAL_PIN(iq);
HAL_PIN(indq);
static void nrt_init(void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
// struct pmsm_limits_ctx_t * ctx = (struct pmsm_limits_ctx_t *)ctx_ptr;
struct pmsm_limits_pin_ctx_t *pins = (struct pmsm_limits_pin_ctx_t *)pin_ptr;
PIN(psi) = 0.01;
PIN(r) = 1.0;
PIN(ld) = 0.001;
PIN(lq) = 0.001;
PIN(polecount) = 1.0;
}
static void rt_func(float period, void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
// struct pmsm_limits_ctx_t * ctx = (struct pmsm_limits_ctx_t *)ctx_ptr;
struct pmsm_limits_pin_ctx_t *pins = (struct pmsm_limits_pin_ctx_t *)pin_ptr;
float p = (int)MAX(PIN(polecount), 1.0);
//float ld = MAX(PIN(ld), 0.0001);
float lq = MAX(PIN(lq), 0.0001);
float psi = MAX(PIN(psi), 0.01);
float r = MAX(PIN(r), 0.001);
float iq = PIN(iq); // TODO id
float indq = PIN(indq); // TODO vel * (psi + id ld)
float volt = PIN(ac_volt);
float abs_max_cur = volt / r;
float abs_max_vel = volt / psi / p;
float abs_max_torque = 3.0 / 2.0 * p * psi * abs_max_cur;
float next_max_cur = iq + (volt - r * iq - indq) / lq * period * 2.0 / 3.0;
float next_min_cur = iq + (-volt - r * iq - indq) / lq * period * 2.0 / 3.0;
float next_max_torque = 3.0 / 2.0 * p * (psi * next_max_cur);
float next_min_torque = 3.0 / 2.0 * p * (psi * next_min_cur);
float max_cur = (volt - indq) / r;
float min_cur = (-volt - indq) / r;
float max_torque = 3.0 / 2.0 * p * (psi * max_cur);
float min_torque = 3.0 / 2.0 * p * (psi * min_cur);
PIN(abs_max_cur) = abs_max_cur;
PIN(abs_max_vel) = abs_max_vel;
PIN(abs_max_torque) = abs_max_torque;
PIN(next_max_cur) = next_max_cur;
PIN(next_min_cur) = next_min_cur;
PIN(next_max_torque) = next_max_torque;
PIN(next_min_torque) = next_min_torque;
PIN(max_cur) = max_cur;
PIN(min_cur) = min_cur;
PIN(max_torque) = max_torque;
PIN(min_torque) = min_torque;
}
hal_comp_t pmsm_limits_comp_struct = {
.name = "pmsm_limits",
.nrt = 0,
.rt = rt_func,
.frt = 0,
.nrt_init = nrt_init,
.rt_start = 0,
.frt_start = 0,
.rt_stop = 0,
.frt_stop = 0,
.ctx_size = 0,
.pin_count = sizeof(struct pmsm_limits_pin_ctx_t) / sizeof(struct hal_pin_inst_t),
};