Commit 05017b43 authored by Jeroen Vreeken's avatar Jeroen Vreeken
Browse files

Add DC motor model block.

With this block motor state can be derived from voltage and speed.
parent a20a0b4b
/*
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 },
};
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 }
}
}
......@@ -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 \
......
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