mirror of https://github.com/rene-dev/stmbl.git
147 lines
3.8 KiB
C
147 lines
3.8 KiB
C
/*
|
|
* This file is part of the stmbl project.
|
|
*
|
|
* Copyright (C) 2013-2017 Rene Hopf <renehopf@mac.com>
|
|
* Copyright (C) 2013-2017 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 should 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(ypid);
|
|
|
|
HAL_PIN(pos_ext_cmd); // cmd in (rad)
|
|
HAL_PIN(pos_fb); // feedback in (rad)
|
|
HAL_PIN(pos_error); // error out (rad)
|
|
|
|
HAL_PIN(vel_ext_cmd); // cmd in (rad/s)
|
|
HAL_PIN(vel_fb); // feedback in (rad/s)
|
|
HAL_PIN(vel_cmd); // cmd out (rad/s)
|
|
HAL_PIN(vel_error); // error out (rad/s)
|
|
HAL_PIN(vel_min); // minimum velocity error 0.3
|
|
|
|
HAL_PIN(enable);
|
|
HAL_PIN(out);
|
|
|
|
HAL_PIN(pos_p); //10
|
|
|
|
HAL_PIN(vel_p); //0.5
|
|
HAL_PIN(vel_i); //0.005
|
|
HAL_PIN(vel_ff); //1.0
|
|
|
|
// system limits
|
|
HAL_PIN(max_vel);
|
|
HAL_PIN(max_acc);
|
|
HAL_PIN(max_out);
|
|
|
|
HAL_PIN(vel_sat);
|
|
HAL_PIN(out_sat);
|
|
HAL_PIN(saturated);
|
|
|
|
struct ypid_ctx_t {
|
|
float sat;
|
|
float vel_error_sum;
|
|
};
|
|
|
|
static void rt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) {
|
|
struct ypid_ctx_t *ctx = (struct ypid_ctx_t *)ctx_ptr;
|
|
struct ypid_pin_ctx_t *pins = (struct ypid_pin_ctx_t *)pin_ptr;
|
|
|
|
float vel_cmd;
|
|
float vel_fb = PIN(vel_fb);
|
|
|
|
float vel_error;
|
|
float vel_sat;
|
|
float out_sat;
|
|
|
|
float pos_error = minus(PIN(pos_ext_cmd), PIN(pos_fb));
|
|
|
|
float pos_p = MAX(PIN(pos_p), 0.0);
|
|
float vel_p = MAX(PIN(vel_p), 0.0);
|
|
float vel_i = MAX(PIN(vel_i), 0.0);
|
|
|
|
float velmin = MAX(-PIN(max_vel), vel_fb - PIN(max_acc) * period);
|
|
float velmax = MIN(PIN(max_vel), vel_fb + PIN(max_acc) * period);
|
|
|
|
float out;
|
|
|
|
if(PIN(enable) > 0.0) {
|
|
// pos -> vel
|
|
vel_cmd = pos_p * pos_error + PIN(vel_ff) * PIN(vel_ext_cmd);
|
|
vel_sat = SAT(vel_cmd, PIN(max_vel));
|
|
vel_cmd = CLAMP(vel_cmd, velmin, velmax); // min/max clamping
|
|
|
|
// vel -> out
|
|
vel_error = vel_cmd - vel_fb;
|
|
if(ABS(vel_error) < PIN(vel_min)) {
|
|
vel_error = 0;
|
|
}
|
|
|
|
ctx->vel_error_sum += vel_error;
|
|
out = LIMIT(vel_error * vel_p, PIN(max_out));
|
|
if(vel_i * vel_p > 0.0f) {
|
|
ctx->vel_error_sum = CLAMP(ctx->vel_error_sum, (-PIN(max_out) - out) / (vel_i * vel_p), (PIN(max_out) - out) / (vel_i * vel_p));
|
|
} else {
|
|
ctx->vel_error_sum = 0;
|
|
}
|
|
out += ctx->vel_error_sum * vel_i * vel_p;
|
|
out_sat = SAT(out, PIN(max_out) * 0.99);
|
|
out = LIMIT(out, PIN(max_out));
|
|
|
|
// sat
|
|
if(ABS(vel_sat) + ABS(out_sat) > 0.0) {
|
|
ctx->sat += period;
|
|
} else {
|
|
ctx->sat = 0.0;
|
|
}
|
|
} else {
|
|
ctx->vel_error_sum = 0.0;
|
|
vel_cmd = 0.0;
|
|
vel_error = 0.0;
|
|
vel_sat = 0.0;
|
|
out_sat = 0.0;
|
|
ctx->sat = 0.0;
|
|
out = 0.0;
|
|
}
|
|
|
|
PIN(pos_error) = pos_error;
|
|
PIN(vel_cmd) = vel_cmd;
|
|
PIN(vel_error) = vel_error;
|
|
PIN(out) = out;
|
|
|
|
PIN(vel_sat) = vel_sat;
|
|
PIN(out_sat) = out_sat;
|
|
PIN(saturated) = ctx->sat;
|
|
}
|
|
|
|
|
|
hal_comp_t ypid_comp_struct = {
|
|
.name = "ypid",
|
|
.nrt = 0,
|
|
.rt = rt_func,
|
|
.frt = 0,
|
|
.nrt_init = 0,
|
|
.rt_start = 0,
|
|
.frt_start = 0,
|
|
.rt_stop = 0,
|
|
.frt_stop = 0,
|
|
.ctx_size = sizeof(struct ypid_ctx_t),
|
|
.pin_count = sizeof(struct ypid_pin_ctx_t) / sizeof(struct hal_pin_inst_t),
|
|
};
|