mirror of https://github.com/rene-dev/stmbl.git
93 lines
2.1 KiB
C
93 lines
2.1 KiB
C
#include "mpid_comp.h"
|
|
#include "commands.h"
|
|
#include "hal.h"
|
|
#include "math.h"
|
|
#include "defines.h"
|
|
#include "angle.h"
|
|
|
|
HAL_COMP(mpid);
|
|
|
|
HAL_PIN(pos_ext_cmd);
|
|
HAL_PIN(pos_fb);
|
|
HAL_PIN(old_pos_ext_cmd);
|
|
HAL_PIN(old_pos_fb);
|
|
HAL_PIN(pos_error);
|
|
|
|
HAL_PIN(vel_ext_cmd);
|
|
HAL_PIN(vel_fb);
|
|
HAL_PIN(vel_error);
|
|
HAL_PIN(vel_cmd);
|
|
|
|
HAL_PIN(acc_cmd);
|
|
|
|
HAL_PIN(torque_ext_cmd);
|
|
HAL_PIN(torque_sum);
|
|
HAL_PIN(torque_cmd);
|
|
|
|
HAL_PIN(min_pos_error);
|
|
HAL_PIN(max_vel);
|
|
HAL_PIN(max_acc);
|
|
HAL_PIN(max_torque);
|
|
|
|
HAL_PIN(pos_p);
|
|
HAL_PIN(vel_i);
|
|
HAL_PIN(vel_p);
|
|
HAL_PIN(j);
|
|
HAL_PIN(scale);
|
|
|
|
HAL_PIN(en);
|
|
|
|
static void rt_func(float period, void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
|
|
// struct mpid_ctx_t *ctx = (struct mpid_ctx_t *)ctx_ptr;
|
|
struct mpid_pin_ctx_t *pins = (struct mpid_pin_ctx_t *)pin_ptr;
|
|
|
|
if(PIN(en) > 0.0){
|
|
PIN(pos_error) += minus(PIN(pos_ext_cmd), PIN(old_pos_ext_cmd)) * PIN(scale);
|
|
PIN(pos_error) -= minus(PIN(pos_fb), PIN(old_pos_fb));
|
|
|
|
PIN(vel_cmd) = LIMIT(PIN(pos_error) * PIN(pos_p), PIN(max_vel));
|
|
|
|
if(ABS(PIN(pos_error)) < PIN(min_pos_error)){
|
|
PIN(vel_cmd) = 0.0;
|
|
}
|
|
|
|
PIN(vel_cmd) += PIN(vel_ext_cmd);
|
|
|
|
PIN(vel_error) = PIN(vel_cmd) - PIN(vel_fb);
|
|
|
|
PIN(acc_cmd) = LIMIT(PIN(vel_error) * PIN(vel_p), PIN(max_acc));
|
|
|
|
PIN(torque_cmd) = LIMIT(PIN(acc_cmd) * PIN(j), PIN(max_torque));
|
|
|
|
PIN(torque_sum) += PIN(vel_error) * PIN(vel_i) * period;
|
|
PIN(torque_sum) = CLAMP(PIN(torque_sum), -PIN(max_torque) - PIN(torque_cmd), PIN(max_torque) - PIN(torque_cmd));
|
|
|
|
PIN(torque_cmd) += PIN(torque_ext_cmd);
|
|
PIN(torque_cmd) += PIN(torque_sum);
|
|
}
|
|
else{
|
|
PIN(pos_error) = 0.0;
|
|
PIN(vel_cmd) = 0.0;
|
|
PIN(vel_error) = 0.0;
|
|
PIN(acc_cmd) = 0.0;
|
|
PIN(torque_sum) = 0.0;
|
|
PIN(torque_cmd) = 0.0;
|
|
}
|
|
|
|
PIN(old_pos_fb) = PIN(pos_fb);
|
|
PIN(old_pos_ext_cmd) = PIN(pos_ext_cmd);
|
|
}
|
|
|
|
hal_comp_t mpid_comp_struct = {
|
|
.name = "mpid",
|
|
.nrt = 0,
|
|
.rt = rt_func,
|
|
.frt = 0,
|
|
.nrt_init = 0,
|
|
.rt_start = 0,
|
|
.frt_start = 0,
|
|
.rt_stop = 0,
|
|
.frt_stop = 0,
|
|
.ctx_size = 0,
|
|
.pin_count = sizeof(struct mpid_pin_ctx_t) / sizeof(struct hal_pin_inst_t),
|
|
}; |