Commit 9288e273 authored by Jeroen Vreeken's avatar Jeroen Vreeken
Browse files

Add rangecheck block. (cherry pick from the corso2013 branch)

Add speed warning to azimuth controller.
parent e1199f99
......@@ -34,6 +34,7 @@ BLOCKSRCS= \
block_quadrature_decoder.c \
block_quantize.c \
block_random.c \
block_rangecheck.c \
block_register.c \
block_setpoint_generator.c \
block_setreset.c \
......
/*
Copyright Jeroen Vreeken (jeroen@vreeken.net), 2013
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_block.h"
/*
inputs outputs
nr name nr name
----------------------
| |
---| 0 in 0 valid |----
| 1 invalid |
----------------------
*/
struct controller_block_private {
float *in;
bool valid;
bool invalid;
float min;
float max;
};
static void rangecheck_calculate(struct controller_block *check)
{
struct controller_block_private *priv = check->private;
float in = *priv->in;
bool invalid;
invalid = (in < priv->min) | (in > priv->max);
priv->invalid = invalid;
priv->valid = !invalid;
}
static struct controller_block_param_list params[] = {
{ "max", false },
{ "min", false },
{ NULL },
};
static void param_get(struct controller_block *check, int param, void *val)
{
switch (param) {
case 0:
*(float*)val = check->private->max;
break;
case 1:
*(float*)val = check->private->min;
break;
}
}
static void param_set(struct controller_block *check, int param, va_list val)
{
switch (param) {
case 0:
check->private->max = va_arg(val, double);
break;
case 1:
check->private->min = va_arg(val, double);
break;
}
}
static struct controller_block_interm_list interms[] = {
{ "in", CONTROLLER_BLOCK_TERM_FLOAT,
offsetof(struct controller_block_private, in) },
{ NULL },
};
static struct controller_block_outterm_list outterms[] = {
{ "valid", CONTROLLER_BLOCK_TERM_BOOL,
offsetof(struct controller_block_private, valid) },
{ "invalid", CONTROLLER_BLOCK_TERM_BOOL,
offsetof(struct controller_block_private, invalid) },
{ NULL },
};
struct controller_block * block_rangecheck_create(char *name)
{
struct controller_block *check;
check = controller_block_alloc("rangecheck", name,
sizeof(struct controller_block_private));
if (!check)
return NULL;
check->private->invalid = true;
check->private->valid = false;
check->private->max = 0.0;
check->private->min = 0.0;
if (controller_block_interm_list_init(check, interms))
goto err_block;
if (controller_block_outterm_list_init(check, outterms))
goto err_block;
check->calculate = rangecheck_calculate;
if (controller_block_param_list_init(check, params))
goto err_block;
check->param_get = param_get;
check->param_set = param_set;
controller_block_add(check);
return check;
err_block:
controller_block_free(check);
return NULL;
}
......@@ -41,6 +41,7 @@ blocks {
{ "value", "azimuth_position_offset" }
{ "add", "azimuth_position_offset_sum" }
{ "gain", "azimuth_position_gain" }
{ "rangecheck", "azimuth_speed_warning" }
{ "matrix_2x2", "elevation_input_matrix" }
{ "setpoint_generator", "elevation_spg", "Elevation_Setpoint", "rad" }
......@@ -103,7 +104,8 @@ links {
{ "azimuth_speed_servo", "out", "dt_az", "speed" , true }
{ "azimuth_safety", "torque_out", "dt_az", "torque" , true }
{ "azimuth_servo_state", "enable", "dt_az", "enable" , true }
{ "false", "value", "dt_az", "ba1" , true }
{ "dt_az", "speed", "azimuth_speed_warning", "in" , true }
{ "azimuth_speed_warning", "invalid", "dt_az", "ba1" , true }
{ "false", "value", "dt_az", "ba2" , true }
{ "elevation_servo_state", "reset", "elevation_spg", "reset" , false }
......@@ -257,6 +259,9 @@ params {
(float) { 1.0, 2.0 }
}
{ "azimuth_speed_warning", "max", (double) rpm2rads(200.0) }
{ "azimuth_speed_warning", "min", (double) rpm2rads(-200.0) }
# Due to high vibrations observed at high speed it was decided to limit
# the pid controller. At low speed the value is untouched, but the
# output is limited when speed increases
......
Markdown is supported
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