/* 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 }, };