Commit 3b378c24 authored by Jeroen Vreeken's avatar Jeroen Vreeken
Browse files

Working after test at corso

parent 8b90f674
...@@ -53,7 +53,7 @@ static void atsamx70_gmac_tx_calculate(struct controller_block *packet_tx) ...@@ -53,7 +53,7 @@ static void atsamx70_gmac_tx_calculate(struct controller_block *packet_tx)
{ {
struct controller_block_private *priv = packet_tx->private; struct controller_block_private *priv = packet_tx->private;
((char*)priv->tx_packet)[0]++; ((char*)priv->tx_packet)[11]++;
gmac_tx(&priv->gmac, priv->tx_packet, priv->tx_size); gmac_tx(&priv->gmac, priv->tx_packet, priv->tx_size);
} }
......
...@@ -49,8 +49,9 @@ static void pwm_calculate(struct controller_block *pwm) ...@@ -49,8 +49,9 @@ static void pwm_calculate(struct controller_block *pwm)
uint32_t dty = abs(duty); uint32_t dty = abs(duty);
/* Make sure we generate negative pulses for capacitor loading */ /* Make sure we generate negative pulses for capacitor loading */
// TODO make this a parameter (in seconds off)
if (dty >= period) { if (dty >= period) {
dty--; dty-=1 + period / 1000;
} }
if (duty > 0) { if (duty > 0) {
......
...@@ -25,6 +25,11 @@ ...@@ -25,6 +25,11 @@
#include <controller/controller_time.h> #include <controller/controller_time.h>
#include <log/log.h> #include <log/log.h>
enum tc_mode {
TC_MODE_POSITION = 0,
TC_MODE_SPEED = 1,
};
struct controller_block_private { struct controller_block_private {
float position; float position;
...@@ -41,45 +46,81 @@ struct controller_block_private { ...@@ -41,45 +46,81 @@ struct controller_block_private {
uint32_t tcn; uint32_t tcn;
}; };
#include <stdio.h>
static void tc_calculate(struct controller_block *tc) static void tc_calculate(struct controller_block *tc)
{ {
struct controller_block_private *priv = tc->private; struct controller_block_private *priv = tc->private;
uint16_t cnt0; uint16_t cnt0;
int16_t cnt0p; int16_t cnt0p;
int16_t cnt1; // int16_t cnt1;
uint16_t prevcnt0 = priv->prevcnt0; uint16_t prevcnt0 = priv->prevcnt0;
float speed; float speed;
float position; float position;
int rev = priv->rev; int rev = priv->rev;
cnt0 = tc_get_cv(priv->tcn, 0); cnt0 = tc_get_cv(priv->tcn, 0);
cnt1 = tc_get_cv(priv->tcn, 1); // cnt1 = tc_get_cv(priv->tcn, 1);
uint32_t qisr = tc_get_qisr(priv->tcn); uint32_t qisr = tc_get_qisr(priv->tcn);
priv->homed |= qisr & TC_QISR_IDX; priv->homed |= qisr & TC_QISR_IDX;
priv->homed &= !(*priv->reset); priv->homed &= !(*priv->reset);
cnt0p = cnt0; bool dir = qisr & TC_QISR_DIR;
// if (cnt0p > rev)
// cnt0p += rev;
int16_t diff = cnt0 - prevcnt0;
if (diff > rev/2)
diff -= rev;
if (diff < -rev/2)
diff += rev;
speed = diff * priv->speedfac; bool setspeed = true;
cnt0p = cnt0;
if (cnt0p > rev)
cnt0p += rev;
if (dir && cnt0 == 0)
cnt0 = -rev;
int diff;
// int16_t diff = cnt0 - prevcnt0;
if (!dir) {
diff = (int)cnt0 - (int)prevcnt0;
/* positive */
if (diff < 0) {
setspeed = false;
}
} else {
diff = ((int)cnt0 - 65536) - ((int)prevcnt0 - 65536);
if (diff > 0) {
setspeed = false;
}
}
if (setspeed)
priv->speed = diff * priv->speedfac;
// priv->speed = diff;
position = (/*cnt1 * rev + */cnt0p) * priv->posfac; position = (/*cnt1 * rev + */cnt0p) * priv->posfac;
priv->prevcnt0 = cnt0; priv->prevcnt0 = cnt0;
priv->speed = speed;
priv->position = position; priv->position = position;
} }
static void tc_calculate_speed(struct controller_block *tc)
{
struct controller_block_private *priv = tc->private;
uint16_t cnt0;
uint16_t prevcnt0 = priv->prevcnt0;
float speed;
float position = priv->position;
cnt0 = tc_get_cv(priv->tcn, 0);
int16_t diff = cnt0 - prevcnt0;
speed = diff * priv->speedfac;
position += diff * priv->posfac;
if (*priv->reset)
position = 0;
priv->prevcnt0 = cnt0;
priv->position = position;
priv->speed = speed;
}
static struct controller_block_outterm_list outterms[] = { static struct controller_block_outterm_list outterms[] = {
{ "position", CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, position) }, { "position", CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, position) },
...@@ -100,6 +141,7 @@ static struct controller_block * block_atsamx70_tc_create(char *name, int argc, ...@@ -100,6 +141,7 @@ static struct controller_block * block_atsamx70_tc_create(char *name, int argc,
int tc_nr; int tc_nr;
int rev; int rev;
int inverted; int inverted;
enum tc_mode tc_mode = TC_MODE_POSITION;
tc_nr = va_arg(ap, int); tc_nr = va_arg(ap, int);
if (tc_nr < 0 || tc_nr > 3) { if (tc_nr < 0 || tc_nr > 3) {
...@@ -109,6 +151,11 @@ static struct controller_block * block_atsamx70_tc_create(char *name, int argc, ...@@ -109,6 +151,11 @@ static struct controller_block * block_atsamx70_tc_create(char *name, int argc,
} }
rev = va_arg(ap, int); rev = va_arg(ap, int);
inverted = va_arg(ap, int); inverted = va_arg(ap, int);
if (argc > 3) {
tc_mode = va_arg(ap, int);
}
log_send(LOG_T_DEBUG, "Mode: %d", tc_mode);
if (!(tc = controller_block_alloc("atsamx70_tc", name, sizeof(struct controller_block_private)))) if (!(tc = controller_block_alloc("atsamx70_tc", name, sizeof(struct controller_block_private))))
goto err_alloc; goto err_alloc;
...@@ -119,7 +166,10 @@ static struct controller_block * block_atsamx70_tc_create(char *name, int argc, ...@@ -119,7 +166,10 @@ static struct controller_block * block_atsamx70_tc_create(char *name, int argc,
if (controller_block_interm_list_init(tc, interms)) if (controller_block_interm_list_init(tc, interms))
goto err_interm; goto err_interm;
tc->calculate = tc_calculate; if (tc_mode == TC_MODE_POSITION)
tc->calculate = tc_calculate;
else
tc->calculate = tc_calculate_speed;
uint32_t pa, pb, pi; uint32_t pa, pb, pi;
uint32_t fa, fb, fi; uint32_t fa, fb, fi;
...@@ -207,10 +257,13 @@ static struct controller_block * block_atsamx70_tc_create(char *name, int argc, ...@@ -207,10 +257,13 @@ static struct controller_block * block_atsamx70_tc_create(char *name, int argc,
The direction status is reported on TC_QISR. The direction status is reported on TC_QISR.
*/ */
tc_init(tc->private->tcn, TC_CH0, uint32_t cmr = TC_CMR_ABETRG_A | TC_CMR_TCCLKS_XC0;
TC_CMR_ETRGEDG_RISING | TC_CMR_ABETRG_A | TC_CMR_TCCLKS_XC0 | if (tc_mode == TC_MODE_POSITION) {
((rev * 4 < 32768) ? TC_CMR_CPCTRG : 0), cmr |= TC_CMR_ETRGEDG_RISING;
0); if (rev * 4 < 32768)
cmr |= TC_CMR_CPCTRG;
}
tc_init(tc->private->tcn, TC_CH0, cmr, 0);
tc_write_rc(tc->private->tcn, TC_CH0, rev * 4); tc_write_rc(tc->private->tcn, TC_CH0, rev * 4);
tc_init(tc->private->tcn, TC_CH1, tc_init(tc->private->tcn, TC_CH1,
...@@ -225,7 +278,7 @@ static struct controller_block * block_atsamx70_tc_create(char *name, int argc, ...@@ -225,7 +278,7 @@ static struct controller_block * block_atsamx70_tc_create(char *name, int argc,
tc_start(tc->private->tcn, TC_CH0); tc_start(tc->private->tcn, TC_CH0);
tc_start(tc->private->tcn, TC_CH1); tc_start(tc->private->tcn, TC_CH1);
tc->private->speedfac = (controller_time_frequency_get(tc->time) * M_PI * 2) / (rev * 4); tc->private->speedfac = (controller_time_frequency_get(tc->time) * M_PI * 2.0) / (double)(rev * 4);
tc->private->posfac = (M_PI * 2) / (rev * 4); tc->private->posfac = (M_PI * 2) / (rev * 4);
if (controller_block_add(tc)) if (controller_block_add(tc))
...@@ -242,5 +295,5 @@ err_alloc: ...@@ -242,5 +295,5 @@ err_alloc:
BLOCK_CREATE(atsamx70_tc) = { BLOCK_CREATE(atsamx70_tc) = {
.create = block_atsamx70_tc_create, .create = block_atsamx70_tc_create,
.args = { "int,int,int", NULL }, .args = { "int,int,int", "int,int,int,int", NULL },
}; };
...@@ -109,6 +109,7 @@ struct controller_block_private { ...@@ -109,6 +109,7 @@ struct controller_block_private {
float freq; /* ticks per second */ float freq; /* ticks per second */
float freq2; float freq2;
float freq3; float freq3;
float tick;
double t_max_a; double t_max_a;
double v_delta_from_max_a; double v_delta_from_max_a;
controller_trigger_time period; controller_trigger_time period;
...@@ -141,6 +142,8 @@ struct controller_block_private { ...@@ -141,6 +142,8 @@ struct controller_block_private {
uint32_t id; uint32_t id;
bool softreset;
struct controller_command_entry cur_command; struct controller_command_entry cur_command;
bool cur_done; bool cur_done;
bool cur_start; bool cur_start;
...@@ -248,21 +251,22 @@ static void setpoint_generator_3d_calculate(struct controller_block *spg) ...@@ -248,21 +251,22 @@ static void setpoint_generator_3d_calculate(struct controller_block *spg)
priv->cmd_x = *priv->reset_x; priv->cmd_x = *priv->reset_x;
cur_x = priv->cmd_x; cur_x = priv->cmd_x;
priv->cur_done = true; priv->cur_done = true;
priv->cur_command.type = COMMAND_PTYPE_SETPOINT; if (!priv->softreset) {
priv->cmd_v = 0.0; priv->cur_command.type = COMMAND_PTYPE_SETPOINT;
cur_v = 0.0; priv->cmd_v = 0.0;
priv->cur_a = 0.0; cur_v = 0.0;
priv->cur_j = 0.0; priv->cur_a = 0.0;
priv->start_x = cur_x; priv->cur_j = 0.0;
priv->start_v = 0.0; priv->start_x = cur_x;
priv->start_a = 0.0; priv->start_v = 0.0;
priv->start_j = 0.0; priv->start_a = 0.0;
priv->start_t = 0; priv->start_j = 0.0;
priv->id = COMMAND_ID_NONE; priv->start_t = 0;
priv->id = COMMAND_ID_NONE;
goto set_output;
}
controller_command_queue_read(priv->command, &entry); controller_command_queue_read(priv->command, &entry);
goto set_output;
} }
if (*priv->track_x_cmd) { if (*priv->track_x_cmd) {
priv->cur_done = false; priv->cur_done = false;
...@@ -297,7 +301,6 @@ static void setpoint_generator_3d_calculate(struct controller_block *spg) ...@@ -297,7 +301,6 @@ static void setpoint_generator_3d_calculate(struct controller_block *spg)
priv->cur_done = true; priv->cur_done = true;
break; break;
case COMMAND_PTYPE_SETPOINT_TRACK: case COMMAND_PTYPE_SETPOINT_TRACK:
priv->cmd_v = 0.0;
priv->cur_done = true; priv->cur_done = true;
break; break;
case COMMAND_PTYPE_SPEED: case COMMAND_PTYPE_SPEED:
...@@ -349,13 +352,14 @@ static void setpoint_generator_3d_calculate(struct controller_block *spg) ...@@ -349,13 +352,14 @@ static void setpoint_generator_3d_calculate(struct controller_block *spg)
priv->cmd_x = cur_x + priv->cmd_v; priv->cmd_x = cur_x + priv->cmd_v;
} }
if (priv->cur_command.type == COMMAND_PTYPE_SPEED_TRACK) { if (priv->cur_command.type == COMMAND_PTYPE_SPEED_TRACK) {
float track_v = *priv->track_v; float track_v = *priv->track_v * priv->tick;
ignore_x = true; ignore_x = true;
priv->cmd_v = track_v; priv->cmd_v = track_v;
priv->cmd_x = cur_x + track_v * controller_time_period_get(spg->time); priv->cmd_x = cur_x + track_v * controller_time_period_get(spg->time);
} }
if (priv->cur_command.type == COMMAND_PTYPE_SETPOINT_TRACK) { if (priv->cur_command.type == COMMAND_PTYPE_SETPOINT_TRACK) {
priv->cmd_x = *priv->track_x; priv->cmd_x = *priv->track_x;
priv->cmd_v = 0.0;
} }
if (priv->cmd_x > priv->max_x) if (priv->cmd_x > priv->max_x)
...@@ -550,7 +554,7 @@ static void setpoint_generator_3d_calculate(struct controller_block *spg) ...@@ -550,7 +554,7 @@ static void setpoint_generator_3d_calculate(struct controller_block *spg)
if (fabs(error_x_at_v) < fabs(error_x) || if (fabs(error_x_at_v) < fabs(error_x) ||
(signbit(error_x) != signbit(error_x_at_v) && !state_to_max_a && !state_at_max_a) || (signbit(error_x) != signbit(error_x_at_v) /*&& !state_to_max_a*/ && !state_at_max_a) ||
ignore_x) { ignore_x) {
if (!ignore_x && if (!ignore_x &&
signbit(error_x_at_v) == signbit(error_x) ) { signbit(error_x_at_v) == signbit(error_x) ) {
...@@ -712,9 +716,9 @@ static void setpoint_generator_3d_calculate(struct controller_block *spg) ...@@ -712,9 +716,9 @@ static void setpoint_generator_3d_calculate(struct controller_block *spg)
/* Is the difference between spg and command small enough? /* Is the difference between spg and command small enough?
If so, make outputs equal to command */ If so, make outputs equal to command */
if (!must_brake && if (!must_brake &&
fabs(priv->cmd_x - cur_x) < priv->precision_x && fabs(priv->cmd_x - cur_x) <= priv->precision_x &&
fabs(priv->cmd_v - cur_v) < priv->precision_v && fabs(priv->cmd_v - cur_v) <= priv->precision_v &&
fabs(priv->cur_a) < priv->precision_a) { fabs(priv->cur_a) <= priv->precision_a) {
priv->cur_j = 0.0; priv->cur_j = 0.0;
priv->cur_a = 0.0; priv->cur_a = 0.0;
cur_v = priv->cmd_v; cur_v = priv->cmd_v;
...@@ -857,6 +861,7 @@ static void scale(struct controller_block *spg) ...@@ -857,6 +861,7 @@ static void scale(struct controller_block *spg)
spg->private->precision_v = spg->private->precision_v_sec * tick; spg->private->precision_v = spg->private->precision_v_sec * tick;
spg->private->precision_a = spg->private->precision_a_sec * tick * tick; spg->private->precision_a = spg->private->precision_a_sec * tick * tick;
spg->private->tick = tick;
spg->private->freq = 1.0 / tick; spg->private->freq = 1.0 / tick;
spg->private->freq2 = spg->private->freq * spg->private->freq; spg->private->freq2 = spg->private->freq * spg->private->freq;
spg->private->freq3 = spg->private->freq2 * spg->private->freq; spg->private->freq3 = spg->private->freq2 * spg->private->freq;
...@@ -952,6 +957,14 @@ static int param_set_precision_a(struct controller_block *spg, char *param, ...@@ -952,6 +957,14 @@ static int param_set_precision_a(struct controller_block *spg, char *param,
return 0; return 0;
} }
static int param_set_softreset(struct controller_block *spg, char *param,
int argc, va_list val)
{
spg->private->softreset = va_arg(val, int);
return 0;
}
static struct controller_block_param_list params[] = { static struct controller_block_param_list params[] = {
{ "setpoint", true, param_set_setpoint, .args = { "double", NULL } }, { "setpoint", true, param_set_setpoint, .args = { "double", NULL } },
{ "max_x", true, param_set_max_x, .args = { "double", NULL } }, { "max_x", true, param_set_max_x, .args = { "double", NULL } },
...@@ -962,6 +975,7 @@ static struct controller_block_param_list params[] = { ...@@ -962,6 +975,7 @@ static struct controller_block_param_list params[] = {
{ "precision_x", true, param_set_precision_x, .args = { "double", NULL } }, { "precision_x", true, param_set_precision_x, .args = { "double", NULL } },
{ "precision_v", true, param_set_precision_v, .args = { "double", NULL } }, { "precision_v", true, param_set_precision_v, .args = { "double", NULL } },
{ "precision_a", true, param_set_precision_a, .args = { "double", NULL } }, { "precision_a", true, param_set_precision_a, .args = { "double", NULL } },
{ "softreset", false, param_set_softreset, .args = { "int", NULL } },
{ NULL }, { NULL },
}; };
...@@ -1001,6 +1015,7 @@ static struct controller_block * block_setpoint_generator_3d_create(char *name, ...@@ -1001,6 +1015,7 @@ static struct controller_block * block_setpoint_generator_3d_create(char *name,
if (!spg) if (!spg)
return NULL; return NULL;
spg->private->softreset = false;
spg->private->cmd_x = 0.0; spg->private->cmd_x = 0.0;
spg->private->cmd_v = 0.0; spg->private->cmd_v = 0.0;
spg->private->max_x = 0.0; spg->private->max_x = 0.0;
......
...@@ -20,7 +20,7 @@ blocks ($(frequency), $(delay)) { ...@@ -20,7 +20,7 @@ blocks ($(frequency), $(delay)) {
#{ "controller_profile", "profile" } #{ "controller_profile", "profile" }
{ "atsamx70_tc", "counter0", 0, 2500, 0 } { "atsamx70_tc", "counter0", 0, 2500, 0 }
{ "atsamx70_tc", "counter1", 1, 10000, 0 } { "atsamx70_tc", "counter1", 1, 10000, 0, 1 }
{ "atsamx70_afec", "afec0", 0 } { "atsamx70_afec", "afec0", 0 }
{ "atsamx70_pwm", "pwm0", 0, 0, 10000.0 } { "atsamx70_pwm", "pwm0", 0, 0, 10000.0 }
...@@ -37,17 +37,21 @@ blocks ($(frequency), $(delay)) { ...@@ -37,17 +37,21 @@ blocks ($(frequency), $(delay)) {
{ "gain", "w2V" } { "gain", "w2V" }
{ "gain", "V2pwm" } { "gain", "V2pwm" }
{ "deadzone", "joystick_deadzone" } { "deadzone", "joystick_deadzone" }
{ "gain", "joystick2w" } { "gain", "joystick2v" }
{ "gain", "joystick2p" }
{ "motor_model_dc_v", "motor_model" } { "motor_model_dc_v", "motor_model" }
{ "i2t", "motor_i2t" } { "i2t", "motor_i2t" }
{ "gain", "Ilim2Vlim" } { "gain", "Ilim2Vlim" }
{ "limit", "Vlim_supply" }
{ "limit_dyn", "Vlim" } { "limit_dyn", "Vlim" }
{ "not", "button0n" } { "not", "button_v" }
{ "not", "button1n" } { "not", "button_x" }
{ "add", "position_offset" }
{ "and2", "track_x_cmd" } { "and2", "track_x_cmd" }
{ "delay", "spg_reset" } { "not", "spg_reset" }
{ "delay_bool", "spg_enable" }
{ "setpoint_generator_3d", "spg", "spg", "rad" } { "setpoint_generator_3d", "spg", "spg", "rad" }
{ "subtract", "error" } { "subtract", "error" }
{ "deadzone", "error_deadzone" } { "deadzone", "error_deadzone" }
...@@ -55,27 +59,31 @@ blocks ($(frequency), $(delay)) { ...@@ -55,27 +59,31 @@ blocks ($(frequency), $(delay)) {
{ "add", "speed_ff" } { "add", "speed_ff" }
{ "value_bool", "false" } { "value_bool", "false" }
{ "value_float", "zero" } { "value", "position_calibrate" }
{ "atsamx70_gmac", "packet" } { "atsamx70_gmac", "packet" }
{ "packet_out_float_be", "out_position", "packet", 0 } { "packet_out_float_be", "out_position", "packet", 0 }
{ "packet_out_float_be", "out_stick", "packet", 4 } { "packet_out_float_be", "out_stick", "packet", 4 }
{ "packet_out_float_be", "out_speed", "packet", 8 } { "packet_out_float_be", "out_speed", "packet", 8 }
{ "packet_out_bool_byte", "out_homed", "packet", 9 } { "packet_out_float_be", "out_speedff", "packet", 12 }
{ "packet_out_float_be", "out_positionff", "packet", 16 }
{ "packet_out_float_be", "out_pid", "packet", 20 }
{ "packet_out_float_be", "out_motor_I", "packet", 24 }
{ "packet_out_bool_byte", "out_homed", "packet", 28 }
} }
alias { alias {
{ "position", "counter0", "position" } { "position", "position_offset", "out" }
{ "motor_speed", "counter1", "speed" } { "motor_speed", "counter1", "speed" }
{ "error", "error", "difference" } { "error", "error", "difference" }
{ "setpoint", "spg", "x" } { "setpoint", "spg", "x" }
} }
set w_nom 1800.0 set w_nom rpm2rads(1800.0)
set V_nom 24.0 set V_nom 24.0
set I_nom 28.0 set I_nom 28.0
set I_peak 95.0 set I_peak 95.0
set t_peak 0.5 #??? set t_peak 0.1 #???
set R_peak $(V_nom)/$(I_peak) set R_peak $(V_nom)/$(I_peak)
set V_supply 24.0 #27.6? set V_supply 24.0 #27.6?
...@@ -83,44 +91,49 @@ set V_supply 24.0 #27.6? ...@@ -83,44 +91,49 @@ set V_supply 24.0 #27.6?
set gear_ratio 100.0 * 19.33 set gear_ratio 100.0 * 19.33
set max_speed $(w_nom)/$(gear_ratio) set max_speed $(w_nom)/$(gear_ratio)
set max_accel $(max_speed) set max_accel $(max_speed)*2.0
set max_jerk $(max_jerk) set max_jerk $(max_accel)*2.0
links { links {
{ "button0", "value", "button0n", "input", true } { "button0", "value", "button_v", "input", true }
{ "button1", "value", "button1n", "input", true } { "button1", "value", "button_x", "input", true }
{ "button1n", "output", "track_x_cmd", "a", true } { "button_x", "output", "track_x_cmd", "a", true }
{ "counter0", "homed", "track_x_cmd", "b", true } { "counter0", "homed", "track_x_cmd", "b", true }
{ "counter0", "position", "position_offset", "in0", true }
{ "position_calibrate", "value", "position_offset", "in1", true }
{ "false", "value", "counter0", "reset", true } { "false", "value", "counter0", "reset", true }
{ "false", "value", "counter1", "reset", true } { "false", "value", "counter1", "reset", true }
{ "afec0", "value", "joystick_deadzone", "in", true } { "afec0", "value", "joystick_deadzone", "in", true }
{ "joystick_deadzone", "out", "joystick2w", "in", true } { "joystick_deadzone", "out", "joystick2v", "in", true }
{ "joystick_deadzone", "out", "joystick2p", "in", true }
{ $<position>, "spg", "reset_x", true } { $<position>, "spg", "reset_x", true }
{ "zero", "value", "spg", "track_x", true } { "joystick2v", "out", "spg", "track_v", true }
{ "joystick2w", "out", "spg", "track_v", true } { "joystick2p", "out", "spg", "track_x", true }
{ "track_x_cmd", "q", "spg", "track_x_cmd", true } { "track_x_cmd", "q", "spg", "track_x_cmd", true }
{ "button0n", "output", "spg", "track_v_cmd", true } { "button_v", "output", "spg", "track_v_cmd", true }
{ "counter0", "homed", "spg_reset", "in", true } { "spg_enable", "out", "spg_reset", "input", true }
{ "spg_reset", "out", "spg", "reset", true } { "spg_reset", "output", "spg", "reset", true }
{ "counter0", "homed", "spg_enable", "in", true }
{ "spg", "v", "speed_ff", "in1", true } { "spg", "v", "speed_ff", "in1", true }
{ $<setpoint>, "error", "positive", true } { $<setpoint>, "error", "positive", true }
{ $<position>, "error", "negative", true }