Commit 348c480c authored by Jeroen Vreeken's avatar Jeroen Vreeken
Browse files

Add i2t block.

Determines current limit based on the i2t algorithm.
parent 05017b43
/*
Copyright Jeroen Vreeken (jeroen@vreeken.net) 2015
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 <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <controller/controller_block.h>
#include <controller/controller_time.h>
#include <log/log.h>
/*
inputs outputs
nr name nr name
----------------------
| |
---| 0 I 0 Ilim |----
| |
----------------------
*/
struct controller_block_private {
float *I;
float Ilim;
float i2t;
float peak2t;
float continuous2;
float peaklim;
float continuous;
float peak;
float duration;
};
static void i2t_calculate(struct controller_block *block)
{
struct controller_block_private *priv = block->private;
float i2 = *priv->I;
float i2t = priv->i2t;
float continuous2 = priv->continuous2;
float peak2t = priv->peak2t;
i2 *= i2;
i2t += (i2 - continuous2);
if (i2t < 0.0)
i2t = 0.0;
if (i2t >= peak2t)
priv->Ilim = priv->continuous;
else
priv->Ilim = priv->peaklim;
priv->i2t = i2t;
}
static void set_peak2t(struct controller_block *i2t)
{
struct controller_block_private *priv = i2t->private;
double tick = controller_time_period_get(i2t->time);
priv->peak2t = (priv->peak * priv->peak - priv->continuous2) * priv->duration / tick;
priv->peaklim = priv->peak / tick;
}
static int param_set_peak(struct controller_block *i2t, char *param, int argc,
va_list val)
{
struct controller_block_private *priv = i2t->private;
priv->peak = va_arg(val, double);
set_peak2t(i2t);
return 0;
}
static int param_set_duration(struct controller_block *i2t, char *param, int argc,
va_list val)
{
struct controller_block_private *priv = i2t->private;
priv->duration = va_arg(val, double);
set_peak2t(i2t);
return 0;
}
static int param_set_continuous(struct controller_block *i2t, char *param, int argc,
va_list val)
{
struct controller_block_private *priv = i2t->private;
priv->continuous = va_arg(val, double);
priv->continuous2 = priv->continuous * priv->continuous;
set_peak2t(i2t);
return 0;
}
static struct controller_block_param_list params[] = {
{ "continuous", false, param_set_continuous, .args = { "double", NULL } },
{ "peak", false, param_set_peak, .args = { "double", NULL } },
{ "duration", false, param_set_duration, .args = { "double", NULL } },
{ NULL },
};
static struct controller_block_interm_list interms[] = {
{ "I", CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, I) },
{ NULL }
};
static struct controller_block_outterm_list outterms[] = {
{ "Ilim", CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, Ilim) },
{ NULL }
};
static struct controller_block * block_i2t_create(char *name, int argc, va_list val)
{
struct controller_block *i2t;
if (!(i2t = controller_block_alloc("i2t", name, sizeof(struct controller_block_private))))
return NULL;
if (controller_block_interm_list_init(i2t, interms))
goto err_block;
if (controller_block_outterm_list_init(i2t, outterms))
goto err_block;
i2t->calculate = i2t_calculate;
if (controller_block_param_list_add(i2t, params))
goto err_block;
if (controller_block_add(i2t))
goto err_block;
return i2t;
err_block:
controller_block_free(i2t);
return NULL;
}
BLOCK_CREATE(i2t) = {
.create = block_i2t_create,
.args = { NULL },
};
trigger {
{ "immediate" }
}
blocks (10.0, 0.0) {
{ "i2t", "i2t_10" }
}
blocks (1.0, 0.0) {
{ "i2t", "i2t_1" }
{ "test_input_float", "test_1_I" }
{ "test_output_float", "test_1_Ilim" }
{ "test_input_float", "test_10_I" }
{ "test_output_float", "test_10_Ilim" }
}
links {
{ "test_1_I", "value", "i2t_1", "I", true }
{ "i2t_1", "Ilim", "test_1_Ilim", "value", true }
{ "test_10_I", "value", "i2t_10", "I", true }
{ "i2t_10", "Ilim", "test_10_Ilim", "value", true }
}
params {
{ "i2t_1", "continuous", (float) 10.0 }
{ "i2t_1", "peak", (float) 30.0 }
{ "i2t_1", "duration", (float) 10.0 }
{ "i2t_10", "continuous", (float) 10.0 }
{ "i2t_10", "peak", (float) 30.0 }
{ "i2t_10", "duration", (float) 10.0 }
{ "test_1_I", "value", 30, (float) {
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
30.0, 30.0, 30.0, 30.0, 30.0, 30.0, 30.0, 30.0, 30.0, 30.0,
10.0, 10.0, 10.0, 10.0, 10.0, 8.0, 8.0, 0.0, 0.0, 0.0 }
}
{ "test_1_Ilim", "value", 30,
(float) {
30.0, 30.0, 30.0, 30.0, 30.0, 30.0, 30.0, 30.0, 30.0, 30.0,
30.0, 30.0, 30.0, 30.0, 30.0, 30.0, 30.0, 30.0, 30.0, 10.0,
10.0, 10.0, 10.0, 10.0, 10.0, 30.0, 30.0, 30.0, 30.0, 30.0
},
(float) {
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0
}
}
{ "test_10_I", "value", 30, (float) {
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
30.0, 30.0, 30.0, 30.0, 30.0, 30.0, 30.0, 30.0, 30.0, 30.0,
10.0, 10.0, 10.0, 10.0, 10.0, 8.0, 8.0, 0.0, 0.0, 0.0 }
}
{ "test_10_Ilim", "value", 30,
(float) {
300.0, 300.0, 300.0, 300.0, 300.0, 300.0, 300.0, 300.0, 300.0, 300.0,
300.0, 300.0, 300.0, 300.0, 300.0, 300.0, 300.0, 300.0, 300.0, 300.0,
10.0, 10.0, 10.0, 10.0, 10.0, 300.0, 300.0, 300.0, 300.0, 300.0
},
(float) {
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0
}
}
}
......@@ -18,6 +18,7 @@ BLOCKS := \
friction \
gain \
gain_var \
i2t \
inverse_proportional \
limit \
limit_dyn \
......@@ -100,6 +101,7 @@ CTRL_TESTS += \
$(DIR)/block_command_bool.test.output \
$(DIR)/block_command_float.test.output \
$(DIR)/block_gain.test.output \
$(DIR)/block_i2t.test.output \
$(DIR)/block_limit.test.output \
$(DIR)/block_limit_switch.test.output \
$(DIR)/block_limit_var.test.output \
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment