Skip to content

GitLab

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

Change terminal names of servo state block

parent e1fbd0f7
Hide whitespace changes
Inline Side-by-side
controller/controller/block_servo_state.c
View file @ f3e1ff7a
......@@ -32,11 +32,11 @@
---------------------------
| |
---| 0 spg0 0 out0 |----
---| 0 spg_x 0 out_x |----
| |
---| 1 spg1 1 out1 |----
---| 1 spg_v 1 out_v |----
| |
---| 2 spg2 2 out2 |----
---| 2 spg_a 2 out_a |----
| |
---| 3 safe 3 spg_rst |----
| |
......@@ -47,21 +47,21 @@
*/
struct controller_block_private {
float *spg0;
float *spg1;
float *spg2;
float *spg_x;
float *spg_v;
float *spg_a;
bool *safe;
float out0;
float out1;
float out2;
float out_x;
float out_v;
float out_a;
bool reset;
bool enable;
bool emergency;
float min0;
float max0;
float max1;
float max2;
float min_x;
float max_x;
float max_v;
float max_a;
enum {
SERVO_STATE_DISABLED,
SERVO_STATE_ENABLED,
......@@ -86,9 +86,9 @@ static void calculate(struct controller_block *servo_state)
}
if (servo_state->private->override) {
servo_state->private->out0 = *servo_state->private->spg0;
servo_state->private->out1 = *servo_state->private->spg1;
servo_state->private->out2 = *servo_state->private->spg2;
servo_state->private->out_x = *servo_state->private->spg_x;
servo_state->private->out_v = *servo_state->private->spg_v;
servo_state->private->out_a = *servo_state->private->spg_a;
servo_state->private->enable = true;
servo_state->private->reset = false;
return;
......@@ -96,12 +96,12 @@ static void calculate(struct controller_block *servo_state)
switch (servo_state->private->state) {
case SERVO_STATE_ENABLED:
servo_state->private->out0 =
*servo_state->private->spg0;
servo_state->private->out1 =
*servo_state->private->spg1;
servo_state->private->out2 =
*servo_state->private->spg2;
servo_state->private->out_x =
*servo_state->private->spg_x;
servo_state->private->out_v =
*servo_state->private->spg_v;
servo_state->private->out_a =
*servo_state->private->spg_a;
if (!safe) {
servo_state->private->enable = false;
......@@ -128,9 +128,9 @@ static void calculate(struct controller_block *servo_state)
}
/* Fallthrough if disabling */
case SERVO_STATE_DISABLING: {
float out0 = servo_state->private->out0;
float out1 = servo_state->private->out1;
float out2 = 0.0;
float out_x = servo_state->private->out_x;
float out_v = servo_state->private->out_v;
float out_a = 0.0;
float t = controller_sample_period();
......@@ -145,23 +145,23 @@ static void calculate(struct controller_block *servo_state)
break;
}
if (out1 > 0.0) {
out2 = -servo_state->private->max2;
out1 += out2 * t;
if (out1 < 0.0) {
out2 += out1;
out1 = 0.0;
if (out_v > 0.0) {
out_a = -servo_state->private->max_a;
out_v += out_a * t;
if (out_v < 0.0) {
out_a += out_v;
out_v = 0.0;
}
} else if (out1 < 0.0) {
out2 = servo_state->private->max2;
out1 += out2 * t;
if (out1 > 0.0) {
out2 -= out1;
out1 = 0.0;
} else if (out_v < 0.0) {
out_a = servo_state->private->max_a;
out_v += out_a * t;
if (out_v > 0.0) {
out_a -= out_v;
out_v = 0.0;
}
}
if (out1 == 0.0) {
if (out_v == 0.0) {
servo_state->private->state =
SERVO_STATE_DISABLED;
log_send(LOG_T_INFO,
......@@ -169,16 +169,16 @@ static void calculate(struct controller_block *servo_state)
servo_state->name);
}
out0 += out1 * t;
out_x += out_v * t;
if (out0 > servo_state->private->max0)
out0 = servo_state->private->max0;
if (out0 < servo_state->private->min0)
out0 = servo_state->private->min0;
if (out_x > servo_state->private->max_x)
out_x = servo_state->private->max_x;
if (out_x < servo_state->private->min_x)
out_x = servo_state->private->min_x;
servo_state->private->out0 = out0;
servo_state->private->out1 = out1;
servo_state->private->out2 = out2;
servo_state->private->out_x = out_x;
servo_state->private->out_v = out_v;
servo_state->private->out_a = out_a;
servo_state->private->enable = true;
servo_state->private->reset = true;
......@@ -186,10 +186,10 @@ static void calculate(struct controller_block *servo_state)
break;
}
case SERVO_STATE_DISABLED:
servo_state->private->out0 =
*servo_state->private->spg0;
servo_state->private->out1 = 0.0;
servo_state->private->out2 = 0.0;
servo_state->private->out_x =
*servo_state->private->spg_x;
servo_state->private->out_v = 0.0;
servo_state->private->out_a = 0.0;
servo_state->private->enable = false;
servo_state->private->reset = true;
......@@ -206,11 +206,11 @@ static void calculate(struct controller_block *servo_state)
}
static struct controller_block_param_list params[] = {
{ "min0", true },
{ "max0", true },
{ "max1", true },
{ "max2", true },
{ "enabled", true },
{ "min_x", true },
{ "max_x", true },
{ "max_v", true },
{ "max_a", true },
{ "enabled", true },
{ "emergency", true },
{ "override", true },
{ NULL },
......@@ -220,17 +220,17 @@ static void param_get(struct controller_block *servo_state, int param, void *val
{
switch (param) {
case 0:
*(float*)val = servo_state->private->min0;
*(float*)val = servo_state->private->min_x;
break;
case 1:
*(float*)val = servo_state->private->max0;
*(float*)val = servo_state->private->max_x;
break;
case 2:
*(float*)val = servo_state->private->max1;
*(float*)val = servo_state->private->max_v;
break;
case 3:
*(float*)val = servo_state->private->max2;
*(float*)val = servo_state->private->max_a;
break;
case 4:
*(bool*)val = servo_state->private->enable_param;
......@@ -248,16 +248,16 @@ static void param_set(struct controller_block *servo_state, int param, va_list v
{
switch (param) {
case 0:
servo_state->private->min0 = va_arg(val, double);
servo_state->private->min_x = va_arg(val, double);
break;
case 1:
servo_state->private->max0 = va_arg(val, double);
servo_state->private->max_x = va_arg(val, double);
break;
case 2:
servo_state->private->max1 = va_arg(val, double);
servo_state->private->max_v = va_arg(val, double);
break;
case 3:
servo_state->private->max2 = va_arg(val, double);
servo_state->private->max_a = va_arg(val, double);
break;
case 4:
servo_state->private->enable_param = va_arg(val, int);
......@@ -299,9 +299,9 @@ struct controller_block * block_servo_state_create(char *name)
servo_state->private = malloc(sizeof(struct controller_block_private));
if (!servo_state->private)
goto err_name;
servo_state->private->out0 = 0.0;
servo_state->private->out1 = 0.0;
servo_state->private->out2 = 0.0;
servo_state->private->out_x = 0.0;
servo_state->private->out_v = 0.0;
servo_state->private->out_a = 0.0;
servo_state->private->reset = true;
servo_state->private->enable = false;
servo_state->private->enable_param = false;
......@@ -313,17 +313,17 @@ struct controller_block * block_servo_state_create(char *name)
servo_state->input = malloc(sizeof(struct controller_block_interm) * 4);
if (!servo_state->input)
goto err_private;
servo_state->input[0].name = "spg0";
servo_state->input[0].name = "spg_x";
servo_state->input[0].type = CONTROLLER_BLOCK_TERM_FLOAT;
servo_state->input[0].value.f = &servo_state->private->spg0;
servo_state->input[0].value.f = &servo_state->private->spg_x;
servo_state->input[0].ghostof = NULL;
servo_state->input[1].name = "spg1";
servo_state->input[1].name = "spg_v";
servo_state->input[1].type = CONTROLLER_BLOCK_TERM_FLOAT;
servo_state->input[1].value.f = &servo_state->private->spg1;
servo_state->input[1].value.f = &servo_state->private->spg_v;
servo_state->input[1].ghostof = NULL;
servo_state->input[2].name = "spg2";
servo_state->input[2].name = "spg_a";
servo_state->input[2].type = CONTROLLER_BLOCK_TERM_FLOAT;
servo_state->input[2].value.f = &servo_state->private->spg2;
servo_state->input[2].value.f = &servo_state->private->spg_a;
servo_state->input[2].ghostof = NULL;
servo_state->input[3].name = "safe";
servo_state->input[3].type = CONTROLLER_BLOCK_TERM_BOOL;
......@@ -334,17 +334,17 @@ struct controller_block * block_servo_state_create(char *name)
servo_state->output = malloc(sizeof(struct controller_block_outterm) * 5);
if (!servo_state->output)
goto err_input;
servo_state->output[0].name = "out0";
servo_state->output[0].name = "out_x";
servo_state->output[0].type = CONTROLLER_BLOCK_TERM_FLOAT;
servo_state->output[0].value.f = &servo_state->private->out0;
servo_state->output[0].value.f = &servo_state->private->out_x;
servo_state->output[0].source = servo_state;
servo_state->output[1].name = "out1";
servo_state->output[1].name = "out_v";
servo_state->output[1].type = CONTROLLER_BLOCK_TERM_FLOAT;
servo_state->output[1].value.f = &servo_state->private->out1;
servo_state->output[1].value.f = &servo_state->private->out_v;
servo_state->output[1].source = servo_state;
servo_state->output[2].name = "out2";
servo_state->output[2].name = "out_a";
servo_state->output[2].type = CONTROLLER_BLOCK_TERM_FLOAT;
servo_state->output[2].value.f = &servo_state->private->out2;
servo_state->output[2].value.f = &servo_state->private->out_a;
servo_state->output[2].source = servo_state;
servo_state->output[3].name = "reset";
servo_state->output[3].type = CONTROLLER_BLOCK_TERM_BOOL;
......
controller/dt_ctrl.ctrl
View file @ f3e1ff7a
......@@ -53,18 +53,18 @@ links {
{ "azimuth_position_offset_sum", "out", "azimuth_error", "negative" , true }
{ "azimuth_servo_state", "reset", "azimuth_spg", "reset" , false }
{ "azimuth_position_offset_sum", "out", "azimuth_spg", "reset_x" , true }
{ "azimuth_spg", "x", "azimuth_servo_state", "spg0" , true }
{ "azimuth_spg", "v", "azimuth_servo_state", "spg1" , true }
{ "azimuth_spg", "a", "azimuth_servo_state", "spg2" , 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 }
{ "azimuth_safety", "safe_out", "azimuth_servo_state", "safe" , false }
{ "azimuth_servo_state", "out0", "azimuth_error", "positive" , true }
{ "azimuth_servo_state", "out_x", "azimuth_error", "positive" , true }
{ "azimuth_position_offset_sum", "out", "azimuth_setpoint_error", "negative" , true }
{ "azimuth_spg", "setpoint", "azimuth_setpoint_error", "positive" , true }
{ "azimuth_servo_state", "out1", "azimuth_speed_ff", "in0" , true }
{ "azimuth_servo_state", "out_v", "azimuth_speed_ff", "in0" , true }
{ "azimuth_error", "difference", "azimuth_pid", "in" , true }
{ "azimuth_servo_state", "enable", "azimuth_pid", "enable" , true }
{ "azimuth_pid", "out", "azimuth_pid_limit", "in" , true }
{ "azimuth_servo_state", "out1", "azimuth_pid_limit", "limit" , true }
{ "azimuth_servo_state", "out_v", "azimuth_pid_limit", "limit" , true }
{ "azimuth_pid_limit", "out", "azimuth_pid_filter", "in" , true }
{ "azimuth_pid_filter", "out", "azimuth_speed_ff", "in1" , true }
{ "azimuth_speed_ff", "out", "azimuth_speed_limit", "in" , true }
......@@ -84,12 +84,12 @@ links {
{ "elevation_servo_state", "reset", "elevation_spg", "reset" , false }
{ "elevation_input_matrix", "out0", "elevation_spg", "reset_x" , true }
{ "elevation_spg", "x", "elevation_servo_state", "spg0" , true }
{ "elevation_spg", "v", "elevation_servo_state", "spg1" , true }
{ "elevation_spg", "a", "elevation_servo_state", "spg2" , 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 }
{ "elevation_safety", "safe_out", "elevation_servo_state", "safe" , false }
{ "elevation_servo_state", "out0", "elevation_error", "positive" , true }
{ "elevation_servo_state", "out1", "elevation_speed_ff", "in0" , true }
{ "elevation_servo_state", "out_x", "elevation_error", "positive" , true }
{ "elevation_servo_state", "out_v", "elevation_speed_ff", "in0" , true }
{ "dt_el_r", "position", "elevation_position_offset_r_sum", "in0" , true }
{ "dt_el_l", "position", "elevation_position_offset_l_sum", "in0" , true }
{ "elevation_position_offset_r", "value", "elevation_position_offset_r_sum", "in1" , true }
......@@ -208,10 +208,10 @@ params {
{ "azimuth_spg", "precision_x", (float) 0.000001 }
{ "azimuth_spg", "precision_a", (float) 0.000001 }
{ "azimuth_spg", "precision_v", (float) 0.000001 }
{ "azimuth_servo_state", "max0", (float) 4.712388980384689858 }
{ "azimuth_servo_state", "min0", (float)-4.712388980384689858 }
{ "azimuth_servo_state", "max1", (float) 0.01466076571675236845 }
{ "azimuth_servo_state", "max2", (float) 0.0002 }
{ "azimuth_servo_state", "max_x", (float) 4.712388980384689858 }
{ "azimuth_servo_state", "min_x", (float)-4.712388980384689858 }
{ "azimuth_servo_state", "max_v", (float) 0.01466076571675236845 }
{ "azimuth_servo_state", "max_a", (float) 0.0002 }
{ "azimuth_pid", "kp", (float) 0.20 }
{ "azimuth_pid", "ki", (float) 0.00 }
{ "azimuth_pid", "kd", (float) 0.0 }
......@@ -259,10 +259,10 @@ params {
{ "elevation_spg", "precision_x", (float) 0.000001 }
{ "elevation_spg", "precision_a", (float) 0.000001 }
{ "elevation_spg", "precision_v", (float) 0.000001 }
{ "elevation_servo_state", "max0", (float) 1.570796326794896619 }
{ "elevation_servo_state", "min0", (float)-0.0008726646259971647885 }
{ "elevation_servo_state", "max1", (float) 0.004921828490624009407 }
{ "elevation_servo_state", "max2", (float) 0.0003 }
{ "elevation_servo_state", "max_x", (float) 1.570796326794896619 }
{ "elevation_servo_state", "min_x", (float)-0.0008726646259971647885 }
{ "elevation_servo_state", "max_v", (float) 0.004921828490624009407 }
{ "elevation_servo_state", "max_a", (float) 0.0003 }
{ "elevation_torsion_spg", "setpoint", (float) 0.0 }
{ "elevation_torsion_spg", "t", (float) 0.004 }
{ "elevation_torsion_spg", "max_x", (float) 0.01 }
......
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