mirror of https://github.com/rene-dev/stmbl.git
154 lines
3.6 KiB
C
154 lines
3.6 KiB
C
#include "field_comp.h"
|
|
/*
|
|
* This file is part of the stmbl project.
|
|
*
|
|
* Copyright (C) 2013-2016 Rene Hopf <renehopf@mac.com>
|
|
* Copyright (C) 2013-2016 Nico Stute <crinq@crinq.de>
|
|
*
|
|
* This program is free software: you can redistribute it and/or modify
|
|
* it under the terms of the GNU General Public License as published by
|
|
* the Free Software Foundation, either version 3 of the License, or
|
|
* (at your option) any later version.
|
|
*
|
|
* This program is distributed in the hope that it will be useful,
|
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
* GNU General Public License for more details.
|
|
*
|
|
* You shou l have received a copy of the GNU General Public License
|
|
* along with this program. If not, see <http://www.gnu.org/licenses/>.
|
|
*/
|
|
|
|
#include "commands.h"
|
|
#include "hal.h"
|
|
#include "math.h"
|
|
#include "defines.h"
|
|
#include "angle.h"
|
|
|
|
|
|
HAL_COMP(field);
|
|
|
|
//in
|
|
HAL_PIN(vele_n);
|
|
HAL_PIN(velm_n);
|
|
HAL_PIN(i_n);
|
|
HAL_PIN(t_n);
|
|
HAL_PIN(vel_boost);
|
|
HAL_PIN(t_boost);
|
|
|
|
HAL_PIN(vel);
|
|
|
|
HAL_PIN(vel_mode);
|
|
HAL_PIN(in_mode);
|
|
|
|
// inout
|
|
HAL_PIN(t);
|
|
HAL_PIN(iq);
|
|
|
|
//out
|
|
HAL_PIN(id);
|
|
|
|
HAL_PIN(iq_max);
|
|
HAL_PIN(iq_min);
|
|
|
|
HAL_PIN(t_max);
|
|
HAL_PIN(t_min);
|
|
|
|
HAL_PIN(slip);
|
|
HAL_PIN(velm);
|
|
HAL_PIN(vele);
|
|
|
|
static void nrt_init(void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
|
|
// struct field_ctx_t *ctx = (struct field_ctx_t *)ctx_ptr;
|
|
struct field_pin_ctx_t *pins = (struct field_pin_ctx_t *)pin_ptr;
|
|
PIN(vele_n) = 60.0 * 2.0 * M_PI / 2.0;
|
|
PIN(velm_n) = 1745.0 / 60.0 * 2.0 * M_PI;
|
|
PIN(i_n) = 15.0;
|
|
PIN(t_n) = 23.0;
|
|
PIN(vel_boost) = 1.0;
|
|
PIN(t_boost) = 1.5;
|
|
}
|
|
|
|
static void rt_func(float period, void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
|
|
// struct field_ctx_t *ctx = (struct field_ctx_t *)ctx_ptr;
|
|
struct field_pin_ctx_t *pins = (struct field_pin_ctx_t *)pin_ptr;
|
|
|
|
float vel = MAX(ABS(PIN(vele)), 0.01);
|
|
float i_n = PIN(i_n);
|
|
float t_n = MAX(PIN(t_n), 0.01);
|
|
float vele_n = PIN(vele_n);
|
|
float velm_n = PIN(velm_n);
|
|
float t_boost = PIN(t_boost);
|
|
float vel_boost = PIN(vel_boost);
|
|
float slip_n = vele_n - velm_n;
|
|
float t = 0;
|
|
float iq = 0;
|
|
float slip = 0;
|
|
|
|
|
|
// float p_max = vel_n * t_n * vel_boost;
|
|
// float t_max = MIN(t_n * t_boost, p_max / vel);
|
|
|
|
float id_n = i_n / sqrtf(2.0);
|
|
float id = MIN(id_n, id_n * vele_n * vel_boost / vel);
|
|
|
|
float iq_max = 0;
|
|
float iq_min = 0;
|
|
|
|
if(PIN(vel) > 0.0){
|
|
iq_max = MIN(id_n * t_boost, id_n * vele_n * vel_boost / vel);
|
|
iq_min = -id_n * t_boost;
|
|
}
|
|
else{
|
|
iq_max = id_n * t_boost;
|
|
iq_min = -MIN(id_n * t_boost, id_n * vele_n * vel_boost / vel);
|
|
}
|
|
|
|
float t_max = iq_max * id / id_n / id_n * t_n;
|
|
float t_min = iq_min * id / id_n / id_n * t_n;
|
|
|
|
if(PIN(in_mode) > 0){ // ipnut = torque
|
|
t = PIN(t);
|
|
t = CLAMP(t, t_min, t_max);
|
|
iq = t / t_n / id * id_n * id_n;
|
|
PIN(iq) = iq;
|
|
}
|
|
else{ // input = iq
|
|
iq = PIN(iq);
|
|
iq = CLAMP(iq, iq_min, iq_max);
|
|
t = iq * id / id_n / id_n * t_n;
|
|
PIN(t) = t;
|
|
}
|
|
slip = slip_n / t_n * t;
|
|
|
|
if(PIN(vel_mode) > 0){ // input = velm
|
|
PIN(vele) = PIN(vel) + slip;
|
|
PIN(velm) = PIN(vel);
|
|
}
|
|
else{ // input = vele
|
|
PIN(vele) = PIN(vel);
|
|
PIN(velm) = PIN(vel) - slip;
|
|
}
|
|
|
|
PIN(slip) = slip;
|
|
PIN(id) = id;
|
|
PIN(iq_max) = iq_max;
|
|
PIN(iq_min) = iq_min;
|
|
PIN(t_max) = t_max;
|
|
PIN(t_min) = t_min;
|
|
}
|
|
|
|
hal_comp_t field_comp_struct = {
|
|
.name = "field",
|
|
.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 field_pin_ctx_t) / sizeof(struct hal_pin_inst_t),
|
|
};
|