Add DC motor model block.

With this block motor state can be derived from voltage and speed.

controller/block/block_motor_model_dc_v.c

+/*
+	Copyright Jeroen Vreeken (pe1rxq@amsat.org), 2007
+	Copyright Stichting C.A. Muller Radioastronomiestation, 2007
+
+	This program is free software: you can redistribute it and/or modify
+	it under the terms of the GNU General Public License as published by
+	the Free Software Foundation, either version 3 of the License, or
+	(at your option) any later version.
+
+	This program is distributed in the hope that it will be useful,
+	but WITHOUT ANY WARRANTY; without even the implied warranty of
+	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+	GNU General Public License for more details.
+
+	You should have received a copy of the GNU General Public License
+	along with this program.  If not, see <http://www.gnu.org/licenses/>.
+
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+
+#include <controller/controller_block.h>
+#include <log/log.h>
+
+/*
+	Motor model for DC motors.
+	Calculates Current, Torque and back EMF based on given Voltage and
+	angular velocity.
+
+       inputs      outputs
+       nr name     nr name
+
+     ----------------------
+     |                    |
+  ---| 0  V          0  I |----
+     |	                  |
+  ---| 1  w          1  T |----
+     |	                  |
+     |               2 Eb |----
+     |	                  |
+     ----------------------
+     
+ */
+
+struct controller_block_private {
+	float *V;
+	float *w;
+	float I;
+	float T;
+	float Eb;
+	
+	float kt;
+	float ke;
+	float iR;
+
+	float R;
+};
+
+static void mmdcv_calculate(struct controller_block *mmdcv)
+{
+	struct controller_block_private *priv = mmdcv->private;
+	float V = *priv->V;
+	float w = *priv->w;
+	float I, Eb, T;
+	float kt = priv->kt;
+	float ke = priv->ke;
+	float iR = priv->iR;
+
+	Eb = V - w * ke;
+	I = (Eb) * iR;
+	T = I * kt;
+
+	priv->I = I;
+	priv->T = T;
+}
+
+static int param_set_kt(struct controller_block *mmdcv, char *param, int argc,
+    va_list val)
+{
+	mmdcv->private->kt = va_arg(val, double);
+	return 0;
+}
+
+static int param_set_ke(struct controller_block *mmdcv, char *param, int argc,
+    va_list val)
+{
+	mmdcv->private->ke = va_arg(val, double);
+	return 0;
+}
+
+static int param_set_R(struct controller_block *mmdcv, char *param, int argc,
+    va_list val)
+{
+	mmdcv->private->R = va_arg(val, double);
+	mmdcv->private->iR = 1.0 / mmdcv->private->R;
+	return 0;
+}
+
+static struct controller_block_param_list params[] = {
+	{ "kt", false, param_set_kt, .args = { "double", NULL } },
+	{ "ke", false, param_set_ke, .args = { "double", NULL } },
+	{ "R",  false, param_set_R,  .args = { "double", NULL } },
+	{ NULL },
+};
+
+static struct controller_block_interm_list interms[] = {
+	{ "V", CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, V) },
+	{ "w", CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, w) },
+	{ NULL }
+};
+
+static struct controller_block_outterm_list outterms[] = {
+	{ "I",  CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, I)  },
+	{ "T",  CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, T)  },
+	{ "Eb", CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, Eb) },
+	{ NULL }
+};
+
+static struct controller_block * block_motor_model_dc_v_create(char *name, int argc, va_list val)
+{
+	struct controller_block *mmdcv;
+	
+	if (!(mmdcv = controller_block_alloc("motor_model_dc_v", name, sizeof(struct controller_block_private))))
+		return NULL;
+
+	mmdcv->private->I = 0.0;
+	mmdcv->private->T = 0.0;
+	mmdcv->private->Eb = 0.0;
+	mmdcv->private->kt = 0.0;
+	mmdcv->private->ke = 0.0;
+	mmdcv->private->R = 0.0;
+	mmdcv->private->iR = 0.0;
+
+	if (controller_block_interm_list_init(mmdcv, interms))
+		goto err_block;
+
+	if (controller_block_outterm_list_init(mmdcv, outterms))
+		goto err_block;
+
+	mmdcv->calculate = mmdcv_calculate;
+
+	if (controller_block_param_list_add(mmdcv, params))
+		goto err_block;
+
+	if (controller_block_add(mmdcv))
+		goto err_block;
+	
+	return mmdcv;
+
+err_block:
+	controller_block_free(mmdcv);
+	return NULL;
+}
+
+BLOCK_CREATE(motor_model_dc_v) = {
+	.create = block_motor_model_dc_v_create,
+	.args = { NULL },
+};

controller/block/block_motor_model_dc_v.test.ctrl

+trigger {
+	{ "immediate" }
+}
+
+blocks (1000.0, 0.0) {
+	{ "motor_model_dc_v", "mm" }
+	
+	{ "test_input_float",  "test_V"  }
+	{ "test_input_float",  "test_w"  }
+	{ "test_output_float", "test_I" }
+	{ "test_output_float", "test_T" }
+}
+
+links {
+	{ "test_V", "value", "mm", "V",         true }
+	{ "test_w", "value", "mm", "w",         true }
+	{ "mm", "I",         "test_I", "value", true }
+	{ "mm", "T",         "test_T", "value", true }
+}
+
+params {
+	{ "mm", "ke", (float) 0.01 } # V / (rad/s)
+	{ "mm", "kt", (float) 0.1  } # Nm / A
+	{ "mm", "R",  (float) 4    } 
+	
+	{ "test_V",  "value", 4, (float) { 0.0, 2.0,   -10.0,   24.0 } }
+	{ "test_w",  "value", 4, (float) { 0.0, 100.0, -100.0, 200.0 } }
+	{ "test_I", "value", 4,
+		(float) { 0.0, 0.25, -2.25, 5.5 },
+		(float) { 0.0, 0.0,    0.0, 0.0 }
+	}
+	{ "test_T", "value", 4,
+		(float) { 0.0, 0.025, -0.225, 0.55 },
+		(float) { 0.0,   0.0, 0.0001, 0.0  }
+	}
+}

controller/block/build.mk

@@ -26,6 +26,7 @@ BLOCKS := \
 	limit_var \
 	log \
 	matrix_2x2 \
+	motor_model_dc_v \
 	multiplexer \
 	pand \
 	pid \
@@ -104,6 +105,7 @@ CTRL_TESTS += \
 	$(DIR)/block_limit_var.test.output \
 	$(DIR)/block_log.test.output \
 	$(DIR)/block_matrix_2x2.test.output \
+	$(DIR)/block_motor_model_dc_v.test.output \
 	$(DIR)/block_not.test.output \
 	$(DIR)/block_oneshot.test.output \
 	$(DIR)/block_pid_aw.test.output \