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

Merge branch 'jeroen' of video.vreeken.net:public_html/dt_ctrl into jeroen

parents 18dbe0f6 dcf7fe0d
......@@ -110,10 +110,18 @@ static void servo_state_calculate(struct controller_block *servo_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_v = *priv->spg_v;
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) {
priv->enable = false;
......@@ -138,6 +146,7 @@ static void servo_state_calculate(struct controller_block *servo_state)
servo_state->name);
}
/* Fallthrough if disabling */
}
case SERVO_STATE_DISABLING: {
float out_x = priv->out_x;
float out_v = priv->out_v;
......
......@@ -12,7 +12,8 @@ set frequency 250
set delay 0.0
trigger {
{ "prestart", 0.000150 }
# { "busyloop" }
{ "prestart", 0.000250 }
}
# dt_ctrl_el.ctrl Elevation with real servo drives
......@@ -24,13 +25,13 @@ trigger {
# Uncomment either the real network, or the sim network.
# But never both!
#import "dt_ctrl_el.ctrl"
import "dt_ctrl_el_sim.ctrl"
import "dt_ctrl_el.ctrl"
#import "dt_ctrl_el_sim.ctrl"
#import "dt_ctrl_az.ctrl"
import "dt_ctrl_az_sim.ctrl"
import "dt_ctrl_az.ctrl"
#import "dt_ctrl_az_sim.ctrl"
import "dt_ctrl_ec_sim.ctrl"
#import "dt_ctrl_ec_sim.ctrl"
......@@ -225,7 +226,7 @@ links {
{ "false", "value", "dt_el_l", "ba2" , true }
{ "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_input_matrix", "out1", "elevation_torsion_error", "negative" , true }
{ "elevation_torsion_spg", "setpoint", "elevation_torsion_setpoint_error", "positive" , true }
......@@ -392,7 +393,7 @@ params {
{ "elevation_spg", "setpoint", (float) 0.0 }
{ "elevation_spg", "max_x", (float) deg2rad(90.0) }
{ "elevation_spg", "min_x", (float) deg2rad(-0.05) }
{ "elevation_spg", "min_x", (float) deg2rad(-0.055) }
{ "elevation_spg", "max_v", (float) rpm2rads(2100.0)/$(elevation_gear) }
{ "elevation_spg", "max_a", (float) 0.0003 }
{ "elevation_spg", "max_j", (float) 0.000016 }
......@@ -416,13 +417,13 @@ params {
{ "elevation_speed_limit", "max", (float) rpm2rads(0.08) }
{ "elevation_jerk_limit", "min", (float)-0.003 }
{ "elevation_jerk_limit", "max", (float) 0.003 }
{ "elevation_torsion_pid", "kp", (float) 0.2 }
{ "elevation_torsion_pid", "kp", (float) 20.0 }
{ "elevation_torsion_pid", "ki", (float) 0.0 }
{ "elevation_torsion_pid", "kd", (float) 0.0 }
{ "elevation_torsion_pid", "maxw", (float) 0.0001 }
{ "elevation_torsion_pid", "minw", (float)-0.0001 }
{ "elevation_torsion_speed_limit", "min", (float) rpm2rads(-0.001) }
{ "elevation_torsion_speed_limit", "max", (float) rpm2rads(0.001) }
{ "elevation_torsion_speed_limit", "min", (float) rpm2rads(-0.01) }
{ "elevation_torsion_speed_limit", "max", (float) rpm2rads(0.01) }
{ "elevation_torsion_range", "max", (float) 0.0002 }
......
......@@ -7,6 +7,6 @@ params {
{ "elevation_pid", "ki", (float)0 }
{ "elevation_pid", "kd", (float)0 }
# limit spg to 1000rpm
# limit spg to 2500rpm
{ "azimuth_spg", "max_v", (float) rpm2rads(2500.0)/$(azimuth_gear) }
}
import "dt_ctrl_ec.ctrl"
# el_r labeled "EL 1"
# el_l labeled "EL 2"
blocks ($(frequency), $(delay)) {
{ "stoeber", "dt_el_r",
"ethercat", # bus name
0, # ethercat position
0, # serial number
1.0, # maximum torque
10.0, # maximum torque
200.0, # brake resistor
40.0 # brake power
}
......@@ -13,7 +16,7 @@ blocks ($(frequency), $(delay)) {
"ethercat",
1,
0,
1.0,
10.0,
200.0,
40.0
}
......
......@@ -67,6 +67,7 @@ static char *stoeber_e48_tostring(enum stoeber_e48 e)
case STOEBER_E48_QUICKSTOP:
return "Quick stop";
}
return "Unknown";
}
......@@ -163,6 +164,8 @@ static void calculate_tx(struct controller_block *tx)
torque = *private->input_torque / private->standstill_torque * 16384;
private->tx_pdo->C230 = htole16((int16_t)torque);
private->enabled = enabled;
}
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
if (!stoeber)
goto err_malloc;
private = stoeber->private;
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;
stoeber_tx->private = stoeber->private;
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)
goto err_malloc_rx;
......@@ -764,15 +770,15 @@ err_tx_input:
err_rx_output:
/* no way to un-init the device? */
err_stoeber_init:
free(stoeber_rx->name);
free(stoeber_rx);
// free(stoeber_rx->name);
// free(stoeber_rx);
err_malloc_rx:
free(stoeber_tx->name);
free(stoeber_tx);
// free(stoeber_tx->name);
// free(stoeber_tx);
err_malloc_tx:
free(stoeber->private);
free(stoeber->name);
free(stoeber);
// free(stoeber->private);
// free(stoeber->name);
// free(stoeber);
err_malloc:
return NULL;
}
......
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