Skip to content

GitLab

Commit c08e7427 authored by Jeroen Vreeken's avatar Jeroen Vreeken
Browse files

Add track setpoint command. 

Introduce track command in 3rd order spg.
With this command the spg is set to track the 'track_x' input but will
still honour all limits (v, a, j, x)
parent 6e1eee3e
Hide whitespace changes
Inline Side-by-side
common/command/command.c
View file @ c08e7427
......@@ -521,6 +521,8 @@ char *enum_command_ptype2str(enum command_ptype ptype)
return "SPEED";
case COMMAND_PTYPE_SETPOINT_TIME:
return "SETPOINT_TIME";
case COMMAND_PTYPE_SETPOINT_TRACK:
return "SETPOINT_TRACK";
case COMMAND_PTYPE_MAX:
return "MAX";
default:
......
common/command/command.h
View file @ c08e7427
......@@ -35,6 +35,7 @@ enum command_ptype {
COMMAND_PTYPE_SETPOINT = 4, /* Simple direct setpoint */
COMMAND_PTYPE_SPEED = 5, /* Simple direct 1st derivative setpoint */
COMMAND_PTYPE_SETPOINT_TIME = 6, /* Setpoint to be reached @ time */
COMMAND_PTYPE_SETPOINT_TRACK = 7, /* Use setpoint from track input */
COMMAND_PTYPE_MAX, /* Always the last entry! */
};
......
controller/block/block_setpoint_generator_3d.c
View file @ c08e7427
......@@ -36,11 +36,11 @@
--------------------------
| |
----| 0 reset_x 0 x |----
----| 0 reset_x 0 x |----
| |
----| 1 reset 1 v |----
----| 1 reset 1 v |----
| |
| 2 a |----
----| 2 track_x 2 a |----
| |
| 3 j |----
| |
......@@ -79,6 +79,7 @@ struct controller_block_private {
float *reset_x;
bool *reset;
float *track_x;
/* beware: 'samples' is the time unit, not 'seconds',
all parameters and commands are scaled on block entry/exit */
......@@ -280,6 +281,10 @@ static void setpoint_generator_3d_calculate(struct controller_block *spg)
priv->cmd_v = 0.0;
priv->cur_done = true;
break;
case COMMAND_PTYPE_SETPOINT_TRACK:
priv->cmd_v = 0.0;
priv->cur_done = true;
break;
case COMMAND_PTYPE_SPEED:
priv->cmd_v = priv->cur_command.value.f * controller_time_period_get(spg->time);
priv->cur_done = true;
......@@ -325,6 +330,9 @@ static void setpoint_generator_3d_calculate(struct controller_block *spg)
ignore_x = true;
priv->cmd_x = cur_x + priv->cmd_v;
}
if (priv->cur_command.type == COMMAND_PTYPE_SETPOINT_TRACK) {
priv->cmd_x = *priv->track_x;
}
if (priv->cmd_x > priv->max_x)
priv->cmd_x = priv->max_x;
......@@ -800,6 +808,8 @@ static int block_setpoint_generator_command_filter(struct controller_command *co
break;
}
case COMMAND_PTYPE_SETPOINT_TRACK:
break;
default:
r = -1;
break;
......@@ -934,6 +944,7 @@ static struct controller_block_param_list params[] = {
static struct controller_block_interm_list interms[] = {
{ "reset_x", CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, reset_x) },
{ "reset", CONTROLLER_BLOCK_TERM_BOOL, offsetof(struct controller_block_private, reset) },
{ "track_x", CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, track_x) },
{ NULL }
};
......@@ -1023,6 +1034,7 @@ static struct controller_block * block_setpoint_generator_3d_create(char *name,
spg->private->command->command_types[0] = COMMAND_PTYPE_SETPOINT;
spg->private->command->command_types[1] = COMMAND_PTYPE_SPEED;
spg->private->command->command_types[2] = COMMAND_PTYPE_SETPOINT_TIME;
spg->private->command->command_types[3] = COMMAND_PTYPE_SETPOINT_TRACK;
spg->private->command->filter =
block_setpoint_generator_command_filter;
......
controller/block/block_setpoint_generator_3d.test.ctrl
View file @ c08e7427
......@@ -8,6 +8,7 @@ blocks (10.0, 0.0) {
{ "test_input_bool", "reset" }
{ "test_input_float", "reset_x" }
{ "test_input_float", "track_x" }
{ "test_output_float", "setpoint" }
{ "test_output_float", "x" }
......@@ -22,6 +23,7 @@ blocks (10.0, 0.0) {
links {
{ "reset", "value", "spg", "reset", true }
{ "reset_x", "value", "spg", "reset_x", true }
{ "track_x", "value", "spg", "track_x", true }
{ "spg", "x", "x", "value", true }
{ "spg", "v", "v", "value", true }
......@@ -31,9 +33,11 @@ links {
{ "spg", "id", "id", "value", true }
}
# derived from command/command.h
set SETPOINT 4
set SPEED 5
set SETPOINT_TIME 6
set SETPOINT_TRACK 7
params {
{ "spg", "min_x", (float) -100.0 }
......@@ -49,6 +53,15 @@ params {
{ "command", "command", "spg", $[SPEED], 200, 11.0 }
{ "command", "command", "spg", $[SETPOINT_TIME], 127, -95.0, 1, 100000000 }
{ "command", "command", "spg", $[SPEED], 201, -33.0 }
{ "command", "command", "spg", $[SPEED], 201, -33.0 }
{ "command", "command", "spg", $[SPEED], 201, -33.0 }
{ "command", "command", "spg", $[SPEED], 201, -33.0 }
{ "command", "command", "spg", $[SPEED], 201, -33.0 }
{ "command", "command", "spg", $[SPEED], 201, -33.0 }
{ "command", "command", "spg", $[SPEED], 201, -33.0 }
{ "command", "command", "spg", $[SPEED], 201, -33.0 }
{ "command", "command", "spg", $[SPEED], 201, -33.0 }
{ "command", "command", "spg", $[SETPOINT_TRACK], 301, 999.0 }
{ "reset", "value", 12,
(int) { true, false, false, false, false, false, false, false, false, false,
......@@ -56,10 +69,22 @@ params {
{ "reset_x", "value", 11,
(float) { 50.0, 33.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
-94.0 } }
{ "track_x", "value", 22,
(float) { 42.0, 42.0, 42.0, 42.0, 42.0, 42.0, 42.0, 42.0, 42.0, 42.0,
42.0, 42.0, 42.0, 42.0, 42.0, 42.0, 42.0, 42.0, 42.0, 42.0,
40.0, 1337.0
}
}
{ "setpoint", "value", 10,
(float) { 50.0, 56.0, 62.0, 68.0, 74.0, 80.0, -100.0, 200.0, 61.0, 51.1395 },
(float) { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-7 }
{ "setpoint", "value", 22,
(float) { 50.0, 56.0, 62.0, 68.0, 74.0, 80.0, -100.0, 200.0, 61.0, 51.1395,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
40.0, 200.0
},
(float) { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-7,
100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0,
0.0, 0.0
}
}
{ "x", "value", 20,
(float) { 50.0, 50.00008, 50.00007, 50.002255, 50.00533, 50.01042, 50.01783, 50.02742, 50.0395, 50.05458,
......@@ -87,8 +112,9 @@ params {
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }
}
{ "id", "value", 20,
{ "id", "value", 22,
(int) { (int)0xffffffff, 123, 123, 123, 123, 123, 124, 125, 126, 200,
(int)0xffffffff, 201,(int)0xffffffff,(int)0xffffffff,(int)0xffffffff,(int)0xffffffff,(int)0xffffffff,(int)0xffffffff,(int)0xffffffff,(int)0xffffffff}
(int)0xffffffff, 201, 201, 201, 201, 201, 201, 201, 201, 201,
301, (int)0xffffffff }
}
}
controller/dt_ctrl.ctrl
View file @ c08e7427
......@@ -108,7 +108,8 @@ blocks ($(frequency), $(delay)) {
{ "command_bool", "focusbox_plus" }
{ "command_bool", "focusbox_min" }
{ "value_bool", "false" }
{ "value_bool", "false" }
{ "value_float", "zero" }
}
alias {
......@@ -128,6 +129,7 @@ links {
{ $<Azimuth_Position>, "azimuth_error", "negative" , true }
{ "azimuth_servo_state", "reset", "azimuth_spg", "reset" , false }
{ $<Azimuth_Position>, "azimuth_spg", "reset_x" , true }
{ "zero", "value", "azimuth_spg", "track_x" , true }
{ "azimuth_spg", "x", "azimuth_servo_state", "spg_x" , true }
{ "azimuth_spg", "v", "azimuth_servo_state", "spg_v" , true }
{ "azimuth_spg", "a", "azimuth_servo_state", "spg_a" , true }
......@@ -173,6 +175,7 @@ links {
{ "elevation_servo_state", "reset", "elevation_spg", "reset" , false }
{ $<Elevation_Position>, "elevation_spg", "reset_x" , true }
{ "zero", "value", "elevation_spg", "track_x" , true }
{ "elevation_spg", "x", "elevation_servo_state", "spg_x" , true }
{ "elevation_spg", "v", "elevation_servo_state", "spg_v" , true }
{ "elevation_spg", "a", "elevation_servo_state", "spg_a" , true }
......
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