Commit 73954b8f authored by Jeroen Vreeken's avatar Jeroen Vreeken
Browse files

Changes needed to get new software running on an actual DT.

parent 39710d03
...@@ -110,10 +110,18 @@ static void servo_state_calculate(struct controller_block *servo_state) ...@@ -110,10 +110,18 @@ static void servo_state_calculate(struct controller_block *servo_state)
} }
switch (priv->state) { switch (priv->state) {
case SERVO_STATE_ENABLED: case SERVO_STATE_ENABLED: {
float out_v = *priv->spg_v;
priv->out_x = *priv->spg_x; priv->out_x = *priv->spg_x;
priv->out_v = *priv->spg_v;
priv->out_a = *priv->spg_a; priv->out_a = *priv->spg_a;
if (out_v > priv->max_v)
out_v = priv->max_v;
if (out_v < -priv->max_v)
out_v = -priv->max_v;
priv->out_v = out_v;
if (!safe) { if (!safe) {
priv->enable = false; priv->enable = false;
...@@ -138,6 +146,7 @@ static void servo_state_calculate(struct controller_block *servo_state) ...@@ -138,6 +146,7 @@ static void servo_state_calculate(struct controller_block *servo_state)
servo_state->name); servo_state->name);
} }
/* Fallthrough if disabling */ /* Fallthrough if disabling */
}
case SERVO_STATE_DISABLING: { case SERVO_STATE_DISABLING: {
float out_x = priv->out_x; float out_x = priv->out_x;
float out_v = priv->out_v; float out_v = priv->out_v;
......
...@@ -12,7 +12,8 @@ set frequency 250 ...@@ -12,7 +12,8 @@ set frequency 250
set delay 0.0 set delay 0.0
trigger { trigger {
{ "prestart", 0.000150 } # { "busyloop" }
{ "prestart", 0.000250 }
} }
# dt_ctrl_el.ctrl Elevation with real servo drives # dt_ctrl_el.ctrl Elevation with real servo drives
...@@ -24,13 +25,13 @@ trigger { ...@@ -24,13 +25,13 @@ trigger {
# Uncomment either the real network, or the sim network. # Uncomment either the real network, or the sim network.
# But never both! # But never both!
#import "dt_ctrl_el.ctrl" import "dt_ctrl_el.ctrl"
import "dt_ctrl_el_sim.ctrl" #import "dt_ctrl_el_sim.ctrl"
#import "dt_ctrl_az.ctrl" import "dt_ctrl_az.ctrl"
import "dt_ctrl_az_sim.ctrl" #import "dt_ctrl_az_sim.ctrl"
import "dt_ctrl_ec_sim.ctrl" #import "dt_ctrl_ec_sim.ctrl"
...@@ -225,7 +226,7 @@ links { ...@@ -225,7 +226,7 @@ links {
{ "false", "value", "dt_el_l", "ba2" , true } { "false", "value", "dt_el_l", "ba2" , true }
{ "elevation_servo_state", "reset", "elevation_torsion_spg", "reset" , false } { "elevation_servo_state", "reset", "elevation_torsion_spg", "reset" , false }
{ $<Elevation_Torsion>, "elevation_torsion_spg", "reset_x" , true } { "zero", "value", "elevation_torsion_spg", "reset_x" , true }
{ "elevation_torsion_spg", "x", "elevation_torsion_error", "positive" , true } { "elevation_torsion_spg", "x", "elevation_torsion_error", "positive" , true }
{ "elevation_input_matrix", "out1", "elevation_torsion_error", "negative" , true } { "elevation_input_matrix", "out1", "elevation_torsion_error", "negative" , true }
{ "elevation_torsion_spg", "setpoint", "elevation_torsion_setpoint_error", "positive" , true } { "elevation_torsion_spg", "setpoint", "elevation_torsion_setpoint_error", "positive" , true }
......
import "dt_ctrl_ec.ctrl" import "dt_ctrl_ec.ctrl"
# el_r labeled "EL 1"
# el_l labeled "EL 2"
blocks ($(frequency), $(delay)) { blocks ($(frequency), $(delay)) {
{ "stoeber", "dt_el_r", { "stoeber", "dt_el_r",
"ethercat", # bus name "ethercat", # bus name
0, # ethercat position 0, # ethercat position
0, # serial number 0, # serial number
1.0, # maximum torque 10.0, # maximum torque
200.0, # brake resistor 200.0, # brake resistor
40.0 # brake power 40.0 # brake power
} }
...@@ -13,7 +16,7 @@ blocks ($(frequency), $(delay)) { ...@@ -13,7 +16,7 @@ blocks ($(frequency), $(delay)) {
"ethercat", "ethercat",
1, 1,
0, 0,
1.0, 10.0,
200.0, 200.0,
40.0 40.0
} }
......
...@@ -67,6 +67,7 @@ static char *stoeber_e48_tostring(enum stoeber_e48 e) ...@@ -67,6 +67,7 @@ static char *stoeber_e48_tostring(enum stoeber_e48 e)
case STOEBER_E48_QUICKSTOP: case STOEBER_E48_QUICKSTOP:
return "Quick stop"; return "Quick stop";
} }
return "Unknown";
} }
...@@ -163,6 +164,8 @@ static void calculate_tx(struct controller_block *tx) ...@@ -163,6 +164,8 @@ static void calculate_tx(struct controller_block *tx)
torque = *private->input_torque / private->standstill_torque * 16384; torque = *private->input_torque / private->standstill_torque * 16384;
private->tx_pdo->C230 = htole16((int16_t)torque); private->tx_pdo->C230 = htole16((int16_t)torque);
private->enabled = enabled;
} }
static void calculate_rx(struct controller_block *rx) static void calculate_rx(struct controller_block *rx)
...@@ -619,14 +622,17 @@ static struct controller_block *block_stoeber_create(char *name, int argc, va_li ...@@ -619,14 +622,17 @@ static struct controller_block *block_stoeber_create(char *name, int argc, va_li
if (!stoeber) if (!stoeber)
goto err_malloc; goto err_malloc;
private = stoeber->private;
sprintf(trx_name, "%s/tx", name); sprintf(trx_name, "%s/tx", name);
stoeber_tx = controller_block_alloc("stoeber_tx", name, 0); stoeber_tx = controller_block_alloc("stoeber_tx", trx_name, 0);
if (!stoeber_tx)
goto err_malloc_tx; goto err_malloc_tx;
stoeber_tx->private = stoeber->private; stoeber_tx->private = stoeber->private;
sprintf(trx_name, "%s/rx", name); sprintf(trx_name, "%s/rx", name);
stoeber_rx = controller_block_alloc("stoeber_rx", name, 0); stoeber_rx = controller_block_alloc("stoeber_rx", trx_name, 0);
if (!stoeber_rx) if (!stoeber_rx)
goto err_malloc_rx; goto err_malloc_rx;
...@@ -764,15 +770,15 @@ err_tx_input: ...@@ -764,15 +770,15 @@ err_tx_input:
err_rx_output: err_rx_output:
/* no way to un-init the device? */ /* no way to un-init the device? */
err_stoeber_init: err_stoeber_init:
free(stoeber_rx->name); // free(stoeber_rx->name);
free(stoeber_rx); // free(stoeber_rx);
err_malloc_rx: err_malloc_rx:
free(stoeber_tx->name); // free(stoeber_tx->name);
free(stoeber_tx); // free(stoeber_tx);
err_malloc_tx: err_malloc_tx:
free(stoeber->private); // free(stoeber->private);
free(stoeber->name); // free(stoeber->name);
free(stoeber); // free(stoeber);
err_malloc: err_malloc:
return NULL; return NULL;
} }
......
Supports Markdown
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