Skip to content

GitLab

Commit 990cb146 authored by Jeroen Vreeken's avatar Jeroen Vreeken
Browse files

Add id to setpoint generators 

Lots of test updates
parent 4d0ee609
Show whitespace changes
Inline Side-by-side
controller/block/block_limit.test.ctrl 0 → 100644
View file @ 990cb146
frequency 10
blocks {
{ "limit", "limit" }
{ "test_input_float", "in" }
{ "test_output_float", "out" }
}
links {
{ "in", "value", "limit", "in", true }
{ "limit", "out", "out", "value", true }
}
params {
{ "limit", "min", (float) -11.1 }
{ "limit", "max", (float) 22.2 }
{ "in", "value", 7, (float)
{ 0.0, -100.0, -11.1, -11.0, 22.0, 22.2, 100.0 }
}
{ "out", "value", 7,
(float) { 0.0, -11.1, -11.1, -11.0, 22.0, 22.2, 22.2 },
(float) { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }
}
}
controller/block/block_matrix_2x2.test.ctrl 0 → 100644
View file @ 990cb146
frequency 10
blocks {
{ "matrix_2x2", "matrix" }
{ "test_input_float", "in0" }
{ "test_input_float", "in1" }
{ "test_output_float", "out0" }
{ "test_output_float", "out1" }
}
links {
{ "in0", "value", "matrix", "in0", true }
{ "in1", "value", "matrix", "in1", true }
{ "matrix", "out0", "out0", "value", true }
{ "matrix", "out1", "out1", "value", true }
}
params {
{ "matrix", "constants",
(float) { 2.0, 4.0 },
(float) { 8.0, 16.0 }
}
{ "in0", "value", 4, (float)
{ 0.0, 1.0, 0.0, 1.0 }
}
{ "in1", "value", 4, (float)
{ 0.0, 0.0, 1.0, 1.0 }
}
{ "out0", "value", 4,
(float) { 0.0, 2.0, 4.0, 6.0 },
(float) { 0.0, 0.0, 0.0, 0.0 }
}
{ "out1", "value", 4,
(float) { 0.0, 8.0, 16.0, 24.0 },
(float) { 0.0, 0.0, 0.0, 0.0 }
}
}
controller/block/block_quantize.test.ctrl 0 → 100644
View file @ 990cb146
frequency 10
blocks {
{ "quantize", "quantize" }
{ "test_input_float", "in" }
{ "test_output_float", "out" }
}
links {
{ "in", "value", "quantize", "in", true }
{ "quantize", "out", "out", "value", true }
}
params {
{ "quantize", "quantum", (float) 0.1 }
{ "in", "value", 7, (float)
{ 0.0, -100.09, -11.13, -11.01, 22.01, 22.19, 100.001 }
}
{ "out", "value", 7,
(float) { 0.0, -100.1, -11.1, -11.0, 22.0, 22.2, 100.0 },
(float) { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }
}
}
controller/block/block_rangecheck.test.ctrl 0 → 100644
View file @ 990cb146
frequency 10
blocks {
{ "rangecheck", "rangecheck" }
{ "test_input_float", "in" }
{ "test_output_bool", "valid" }
{ "test_output_bool", "invalid" }
}
links {
{ "in", "value", "rangecheck", "in", true }
{ "rangecheck", "valid", "valid", "value", true }
{ "rangecheck", "invalid", "invalid", "value", true }
}
params {
{ "rangecheck", "min", (float) -11.1 }
{ "rangecheck", "max", (float) 22.2 }
{ "in", "value", 7, (float)
{ 0.0, -100.0, -11.1, -11.0, 22.0, 22.2, 100.0 }
}
{ "valid", "value", 7, (int)
{ true, false, true, true, true, true, false }
}
{ "invalid", "value", 7, (int)
{ false, true, false, false, false, false, true }
}
}
controller/block/block_servo_state.c
View file @ 990cb146
......@@ -99,6 +99,16 @@ static void servo_state_calculate(struct controller_block *servo_state)
return;
}
if (!safe && priv->state != SERVO_STATE_DISABLED) {
priv->enable = false;
priv->reset = true;
priv->state =
SERVO_STATE_DISABLED;
log_send(LOG_T_ERROR,
"%s: Unsafe, going to state DISABLED",
servo_state->name);
}
switch (priv->state) {
case SERVO_STATE_ENABLED:
priv->out_x = *priv->spg_x;
......@@ -134,18 +144,6 @@ static void servo_state_calculate(struct controller_block *servo_state)
float out_a = 0.0;
float t = controller_sample_period();
if (!safe) {
priv->enable = false;
priv->reset = true;
priv->state =
SERVO_STATE_DISABLED;
log_send(LOG_T_ERROR,
"%s: Unsafe, going to state DISABLED",
servo_state->name);
break;
}
if (out_v > 0.0) {
out_a = -priv->max_a;
out_v += out_a * t;
......
controller/block/block_servo_state.test.ctrl 0 → 100644
View file @ 990cb146
frequency 10
blocks {
{ "servo_state", "servo_state" }
{ "test_input_float", "spg_x" }
{ "test_input_float", "spg_v" }
{ "test_input_float", "spg_a" }
{ "test_input_bool", "safe" }
{ "test_input_bool", "override" }
{ "test_output_float", "out_x" }
{ "test_output_float", "out_v" }
{ "test_output_float", "out_a" }
{ "test_output_bool", "reset" }
{ "test_output_bool", "enable" }
{ "test_command", "command" }
}
links {
{ "spg_x", "value", "servo_state", "spg_x", true }
{ "spg_v", "value", "servo_state", "spg_v", true }
{ "spg_a", "value", "servo_state", "spg_a", true }
{ "safe", "value", "servo_state", "safe", true }
{ "override", "value", "servo_state", "override", true }
{ "servo_state", "out_x", "out_x", "value", true }
{ "servo_state", "out_v", "out_v", "value", true }
{ "servo_state", "out_a", "out_a", "value", true }
{ "servo_state", "reset", "reset", "value", true }
{ "servo_state", "enable", "enable", "value", true }
}
set SETPOINT 4
params {
{ "servo_state", "min_x", (float) -10.0 }
{ "servo_state", "max_x", (float) 20.0 }
{ "servo_state", "max_v", (float) 3.0 }
{ "servo_state", "max_a", (float) 1.5 }
{ "command", "command", "servo_state", $[SETPOINT], 42, true }
{ "command", "command", "servo_state", $[SETPOINT], 33, false }
{ "spg_x", "value", 4, (float) { 1.0, 2.0, 3.0, 4.0 } }
{ "spg_v", "value", 4, (float) { 0.3, -0.1, -3.0, 1.0 } }
{ "spg_a", "value", 4, (float) { 1.0, -0.2, 0.3, 0.4 } }
{ "safe", "value", 5, (int) { true, true, true, true, false, false } }
{ "override", "value", 5, (int) { false, false, false, true, false } }
{ "out_x", "value", 5,
(float) { 1.0, 2.0, 2.715, 4.0, 4.0 },
(float) { 0.0, 0.0, 0.0, 0.0, 0.0 }
}
{ "out_a", "value", 5,
(float) { 0.0, 0.0, 1.5, 0.4, 0.0 },
(float) { 0.0, 0.0, 0.0, 0.0, 0.0 }
}
{ "out_v", "value", 5,
(float) { 0.0, 0.0, -2.85, 1.0, 0.0 },
(float) { 0.0, 0.0, 0.0, 0.0, 0.0 }
}
{ "reset", "value", 5, (int) { true, true, true, false, true } }
{ "enable", "value", 5, (int) { false, false, true, true, false } }
}
controller/block/block_setpoint_generator_1d.c
View file @ 990cb146
......@@ -81,6 +81,8 @@ struct controller_block_private {
float cur_x;
float cur_v;
uint32_t id;
struct command_entry cur_command;
bool cur_done;
bool cur_start;
......@@ -105,6 +107,7 @@ static void setpoint_generator_1d_calculate(struct controller_block *spg)
priv->cur_command.type = COMMAND_PTYPE_SETPOINT;
priv->cmd_v = 0.0;
priv->cur_v = 0.0;
priv->id = COMMAND_ID_NONE;
return;
}
......@@ -115,12 +118,14 @@ static void setpoint_generator_1d_calculate(struct controller_block *spg)
if (r == 0) {
priv->cur_done = false;
priv->cur_start = false;
priv->id = priv->cur_command.id;
} else {
if (priv->cur_command.type ==
COMMAND_PTYPE_SETPOINT_TIME) {
priv->cmd_v = 0.0;
priv->cmd_x = priv->cur_command.value.f;
}
priv->id = COMMAND_ID_NONE;
}
}
if (!priv->cur_done) {
......@@ -308,6 +313,7 @@ static struct controller_block_outterm_list outterms[] = {
{ "x", CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, cur_x) },
{ "v", CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, cur_v) },
{ "setpoint", CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, cmd_x) },
{ "id", CONTROLLER_BLOCK_TERM_UINT32, offsetof(struct controller_block_private, id) },
{ NULL }
};
......@@ -345,6 +351,8 @@ struct controller_block * block_setpoint_generator_1d_create(char *name, va_list
spg->private->cur_x = 0.0;
spg->private->cur_v = 0.0;
spg->private->id = COMMAND_ID_NONE;
if (controller_block_interm_list_init(spg, interms))
goto err_private;
......
controller/block/block_setpoint_generator_1d.test.ctrl
View file @ 990cb146
......@@ -9,6 +9,7 @@ blocks {
{ "test_output_float", "setpoint" }
{ "test_output_float", "x" }
{ "test_output_float", "v" }
{ "test_output_uint32", "id" }
{ "test_command", "command" }
}
......@@ -20,6 +21,7 @@ links {
{ "spg", "x", "x", "value", true }
{ "spg", "v", "v", "value", true }
{ "spg", "setpoint", "setpoint", "value", true }
{ "spg", "id", "id", "value", true }
}
set SETPOINT 4
......@@ -61,4 +63,8 @@ params {
(float) { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }
}
{ "id", "value", 20,
(int) { (int)0xffffffff, 123, 123, 123, 123, 123, 124, 125, 126, 200,
(int)0xffffffff, 127, 201,(int)0xffffffff,(int)0xffffffff,(int)0xffffffff,(int)0xffffffff,(int)0xffffffff,(int)0xffffffff,(int)0xffffffff}
}
}
controller/block/block_setpoint_generator_3d.c
View file @ 990cb146
......@@ -129,6 +129,7 @@ struct controller_block_private {
float cur_a_out;
float cur_j_out;
uint32_t id;
struct command_entry cur_command;
bool cur_done;
......@@ -245,6 +246,7 @@ static void setpoint_generator_3d_calculate(struct controller_block *spg)
priv->start_a = 0.0;
priv->start_j = 0.0;
priv->start_t = 0;
priv->id = COMMAND_ID_NONE;
goto set_output;
}
......@@ -256,12 +258,14 @@ static void setpoint_generator_3d_calculate(struct controller_block *spg)
if (r == 0) {
priv->cur_done = false;
priv->cur_start = false;
priv->id = priv->cur_command.id;
} else {
if (priv->cur_command.type ==
COMMAND_PTYPE_SETPOINT_TIME) {
priv->cmd_v = 0.0;
priv->cmd_x = priv->cur_command.value.f;
}
priv->id = COMMAND_ID_NONE;
}
}
if (!priv->cur_done) {
......@@ -937,6 +941,7 @@ static struct controller_block_outterm_list outterms[] = {
{ "a", CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, cur_a_out) },
{ "j", CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, cur_j_out) },
{ "setpoint", CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, cmd_x_out) },
{ "id", CONTROLLER_BLOCK_TERM_UINT32, offsetof(struct controller_block_private, id) },
{ NULL }
};
......@@ -999,6 +1004,7 @@ struct controller_block * block_setpoint_generator_3d_create(char *name, va_list
spg->private->cur_a_out = 0.0;
spg->private->cur_j_out = 0.0;
spg->private->cmd_x_out = 0.0;
spg->private->id = COMMAND_ID_NONE;
if (controller_block_interm_list_init(spg, interms))
goto err_private;
......
controller/block/block_setpoint_generator_3d.test.ctrl
View file @ 990cb146
......@@ -11,6 +11,7 @@ blocks {
{ "test_output_float", "v" }
{ "test_output_float", "a" }
{ "test_output_float", "j" }
{ "test_output_uint32", "id" }
{ "test_command", "command" }
}
......@@ -24,6 +25,7 @@ links {
{ "spg", "a", "a", "value", true }
{ "spg", "j", "j", "value", true }
{ "spg", "setpoint", "setpoint", "value", true }
{ "spg", "id", "id", "value", true }
}
set SETPOINT 4
......@@ -81,4 +83,9 @@ params {
(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 }
}
{ "id", "value", 20,
(int) { (int)0xffffffff, 123, 123, 123, 123, 123, 124, 125, 126, 200,
(int)0xffffffff, 127, 201,(int)0xffffffff,(int)0xffffffff,(int)0xffffffff,(int)0xffffffff,(int)0xffffffff,(int)0xffffffff,(int)0xffffffff}
}
}
controller/block/build.mk
View file @ 990cb146
......@@ -80,7 +80,12 @@ SRCS += $(BLOCK_SRCS)
CTRL_TESTS += \
$(DIR)/block_gain.test.ctrl \
$(DIR)/block_limit.test.ctrl \
$(DIR)/block_matrix_2x2.test.ctrl \
$(DIR)/block_not.test.ctrl \
$(DIR)/block_rangecheck.test.ctrl \
$(DIR)/block_setpoint_generator_1d.test.ctrl \
$(DIR)/block_setpoint_generator_3d.test.ctrl
$(DIR)/block_setpoint_generator_3d.test.ctrl \
$(DIR)/block_servo_state.test.ctrl \
$(DIR)/block_quantize.test.ctrl
controller/test/block_test_command.c
View file @ 990cb146
......@@ -103,8 +103,13 @@ static void param_set(struct controller_block *block, int param, va_list val)
case COMMAND_VALUE_TYPE_FLOAT:
entry->value.f = va_arg(val, double);
break;
case COMMAND_VALUE_TYPE_BOOL:
entry->value.b = va_arg(val, int);
break;
default:
log_send(LOG_T_ERROR, "not yet supported");
log_send(LOG_T_ERROR, "%s not yet supported",
enum_command_value_type2str(command->value_type));
log_server_flush();
exit(3);
}
......
controller/test/block_test_input_bool.c
View file @ 990cb146
......@@ -53,7 +53,8 @@ static void calculate_test_input_bool(struct controller_block *block)
priv->values_cur++;
if (priv->values_cur == priv->values_nr) {
if (priv->values_cur == priv->values_nr ||
(priv->values_nr == 0 && priv->values_cur == 1)) {
test_block_done(block, true);
}
}
......
controller/test/block_test_input_float.c
View file @ 990cb146
......@@ -53,7 +53,8 @@ static void calculate_test_input_float(struct controller_block *block)
priv->values_cur++;
if (priv->values_cur == priv->values_nr) {
if (priv->values_cur == priv->values_nr ||
(priv->values_nr == 0 && priv->values_cur == 1)) {
test_block_done(block, true);
}
}
......
controller/test/block_test_input_uint32.c 0 → 100644
View file @ 990cb146
/*
Copyright Jeroen Vreeken (jeroen@vreeken.net), 2014
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 <test/test_block.h>
#include <log/log.h>
/*
outputs
nr name
-----------------
| |
| 0 value |----
| |
-----------------
*/
struct controller_block_private {
uint32_t value;
uint32_t *values;
int values_nr;
int values_cur;
};
static void calculate_test_input_uint32(struct controller_block *block)
{
struct controller_block_private *priv = block->private;
if (priv->values_cur < priv->values_nr) {
priv->value = priv->values[priv->values_cur];
}
priv->values_cur++;
if (priv->values_cur == priv->values_nr ||
(priv->values_nr == 0 && priv->values_cur == 1)) {
test_block_done(block, true);
}
}
static struct controller_block_param_list params[] = {
{ "value", false },
{ NULL },
};
static void param_set(struct controller_block *block, int param, va_list val)
{
struct controller_block_private *priv = block->private;
int nr = va_arg(val, int);
int *values = va_arg(val, int *);
int i;
/* we have only one parameter... */
priv->values = calloc(nr, sizeof(uint32_t));
for (i = 0; i < nr; i++)
priv->values[i] = values[i];
priv->values_nr = nr;
log_send(LOG_T_INFO, "%s: %d test values", block->name, nr);
}
static struct controller_block_outterm_list outterms[] = {
{
.name = "value",
.type = CONTROLLER_BLOCK_TERM_UINT32,
.priv_offset = offsetof(struct controller_block_private, value)
},
{ NULL }
};
struct controller_block * block_test_input_uint32_create(char *name)
{
struct controller_block *block;
if (!(block = controller_block_alloc("test_input_uint32", name,
sizeof(struct controller_block_private))))
return NULL;
if (controller_block_outterm_list_init(block, outterms))
goto err_output;
block->calculate = calculate_test_input_uint32;
if (controller_block_param_list_init(block, params))
goto err_param;
block->param_set = param_set;
controller_block_add(block);
test_block_add(block);
return block;
err_param:
err_output:
controller_block_free(block);
return NULL;
}
controller/test/block_test_output_bool.c
View file @ 990cb146
......@@ -63,7 +63,8 @@ static void calculate_test_output_bool(struct controller_block *block)
priv->values_cur++;
if (priv->values_cur == priv->values_nr) {
if (priv->values_cur == priv->values_nr ||
(priv->values_nr == 0 && priv->values_cur == 1)) {
bool good = priv->values_ok == priv->values_cur;
test_block_done(block, good);
......
controller/test/block_test_output_float.c
View file @ 990cb146
......@@ -69,7 +69,8 @@ static void calculate_test_output_float(struct controller_block *block)
priv->values_cur++;
if (priv->values_cur == priv->values_nr) {
if (priv->values_cur == priv->values_nr ||
(priv->values_nr == 0 && priv->values_cur == 1)) {
bool good = priv->values_ok == priv->values_cur;
test_block_done(block, good);
......
controller/test/block_test_output_uint32.c 0 → 100644
View file @ 990cb146
/*
Copyright Jeroen Vreeken (jeroen@vreeken.net), 2014
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 <inttypes.h>
#include <controller/controller_block.h>
#include <test/test_block.h>
#include <log/log.h>
/*
inputs
nr name
-----------------
| |
----| 0 value |
| |
-----------------
*/
struct controller_block_private {
uint32_t *value;
uint32_t *values;
int values_nr;
int values_cur;
int values_ok;
};