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

controller/block/block_servo_state.c

@@ -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;

controller/dt_ctrl.ctrl

@@ -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 }

controller/dt_ctrl.param

@@ -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) }
 }

controller/dt_ctrl_el.ctrl

 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
 	}

controller/ec/block_stoeber.c

@@ -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;
 }