mirror of
https://github.com/rene-dev/stmbl.git
synced 2024-12-20 15:42:18 +00:00
106 lines
2.9 KiB
C
106 lines
2.9 KiB
C
#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(volatile void *ctx_ptr, volatile 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, volatile void *ctx_ptr, volatile 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),
|
|
};
|