/*
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 .
*/
#include
#include
#include
#include
#include
/*
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, Er, T;
float kt = priv->kt;
float ke = priv->ke;
float iR = priv->iR;
Eb = w * ke;
Er = V - Eb;
I = (Er) * iR;
T = I * kt;
priv->I = I;
priv->T = T;
priv->Eb = Eb;
}
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 },
};