Commit 39aa815a authored by Jeroen Vreeken's avatar Jeroen Vreeken

Add control loop for focusbox

(including simulator)
parent ef66e84e
......@@ -243,6 +243,7 @@ status_dt.button_disable.onclick = function() {
azimuth_enable_command.send(0);
elevation_enable_command.send(0);
elevation_recover_command.send(0);
status_dt.fb_enable_command.send(0);
}
......
......@@ -55,8 +55,8 @@ function dt_status(element_name) {
this.val_az_safe = new dt_ui_value_trace(element_name + "_val_az_safe", "Azimuth_Safe", 0);
this.val_el_safe = new dt_ui_value_trace(element_name + "_val_el_safe", "Elevation_Safe", 0);
this.button_fb_p = new dt_ui_button(element_name + "_fb_p");
this.button_fb_m = new dt_ui_button(element_name + "_fb_m");
this.text_fb_setpoint = new dt_ui_text(element_name + "_fb_setpoint");
this.button_fb_e = new dt_ui_button(element_name + "_fb_e");
this.button_az_enable = new dt_ui_button(element_name + "_az_enable");
this.button_el_enable = new dt_ui_button(element_name + "_el_enable");
......@@ -72,7 +72,7 @@ function dt_status(element_name) {
this.val_az_en, this.val_el_en,
this.val_az_safe, this.val_el_safe,
this.az_view, this.el_view,
this.button_fb_p, this.button_fb_m,
this.text_fb_setpoint, this.button_fb_e,
this.button_az_enable, this.button_el_enable, this.button_disable,
this.val_az_sp, this.val_el_sp, this.val_az_north]);
......@@ -94,10 +94,10 @@ function dt_status(element_name) {
this.val_az_sp.suffix_set("°");
this.val_el_sp.suffix_set("°");
this.button_fb_p.text_set("+");
this.button_fb_m.text_set("-");
this.button_fb_p.color_set("black");
this.button_fb_m.color_set("black");
this.text_fb_setpoint.length_set(3);
this.text_fb_setpoint.width_set(100);
this.button_fb_e.text_set("set");
this.button_fb_e.color_set("black");
this.button_az_enable.text_set("Azimuth Enable");
this.button_el_enable.text_set("Elevation Enable");
this.button_disable.text_set("Disable");
......@@ -115,8 +115,6 @@ function dt_status(element_name) {
this.window.resize_equal([
this.val_az_sp, this.val_el_sp, this.val_az_north]);
this.window.resize_equal([this.button_fb_p, this.button_fb_m]);
this.window.resize_equal([
this.button_az_enable, this.button_el_enable, this.button_disable]);
......@@ -128,7 +126,7 @@ function dt_status(element_name) {
this.window.align_horizontal([
this.lbl_el, this.val_el_pos, this.val_el_en, this.val_el_safe], 5);
this.window.align_horizontal([
this.lbl_fb, this.val_fb_pos, this.button_fb_m, this.button_fb_p], 5);
this.lbl_fb, this.val_fb_pos, this.text_fb_setpoint, this.button_fb_e ], 5);
this.window.align_horizontal([
this.lbl_t, this.val_t], 5);
......@@ -286,26 +284,14 @@ function dt_status(element_name) {
this.button_disable.onclick = function() {
}
this.fb_plus_command = new controller_command("focusbox_plus", "bool");
this.fb_min_command = new controller_command("focusbox_min", "bool");
this.button_fb_p.element.addEventListener("mousedown", function() {
dt_status_this.fb_plus_command.send(1);
});
function fb_p_disable(e) {
dt_status_this.fb_plus_command.send(0);
}
this.button_fb_p.element.addEventListener("mouseup", fb_p_disable);
this.button_fb_p.element.addEventListener("mouseout", fb_p_disable);
this.fb_setpoint_command = new controller_command("focusbox_setpoint", "float");
this.fb_enable_command = new controller_command("focusbox_enable", "bool");
this.button_fb_m.element.addEventListener("mousedown", function() {
dt_status_this.fb_min_command.send(1);
});
function fb_m_disable(e) {
dt_status_this.fb_min_command.send(0);
}
this.button_fb_m.element.addEventListener("mouseup", fb_m_disable);
this.button_fb_m.element.addEventListener("mouseout", fb_m_disable);
this.button_fb_e.onclick = function() {
fb_val = dt_status_this.text_fb_setpoint.value_get() / 100.0;
dt_status_this.fb_setpoint_command.send(fb_val);
dt_status_this.fb_enable_command.send(1);
}
}
......
/*
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>
/*
inputs outputs
nr name nr name
----------------------
| |
---| 0 in 0 out |----
| |
----------------------
*/
struct controller_block_private {
bool *in;
float out;
float gain;
};
static void bool2float_calculate(struct controller_block *bool2float)
{
struct controller_block_private *priv = bool2float->private;
priv->out = *priv->in * priv->gain;
}
static int param_set(struct controller_block *bool2float, char *param, int argc,
va_list val)
{
bool2float->private->gain = va_arg(val, double);
return 0;
}
static struct controller_block_param_list params[] = {
{ "gain", false, param_set, .args = { "double", NULL } },
{ NULL },
};
static struct controller_block_interm_list interms[] = {
{ "in", CONTROLLER_BLOCK_TERM_BOOL, offsetof(struct controller_block_private, in) },
{ NULL }
};
static struct controller_block_outterm_list outterms[] = {
{ "out", CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, out) },
{ NULL }
};
static struct controller_block * block_bool2float_create(char *name, int argc, va_list val)
{
struct controller_block *bool2float;
if (!(bool2float = controller_block_alloc("bool2float", name, sizeof(struct controller_block_private))))
return NULL;
bool2float->private->out = 0.0;
bool2float->private->gain = 1.0;
if (controller_block_interm_list_init(bool2float, interms))
goto err_block;
if (controller_block_outterm_list_init(bool2float, outterms))
goto err_block;
bool2float->calculate = bool2float_calculate;
if (controller_block_param_list_add(bool2float, params))
goto err_block;
if (controller_block_add(bool2float))
goto err_block;
return bool2float;
err_block:
controller_block_free(bool2float);
return NULL;
}
BLOCK_CREATE(bool2float) = {
.create = block_bool2float_create,
.args = { NULL },
};
......@@ -44,7 +44,7 @@ struct io {
};
struct controller_block_private {
struct io io[0];
struct io io[2];
};
static void calculate_2(struct controller_block *block)
......@@ -92,7 +92,7 @@ static struct controller_block * block_ex2_create(char *name, int argc, va_list
{
struct controller_block *block;
if (!(block = controller_block_alloc("ex2", name, sizeof(struct controller_block_private) + sizeof(bool *) * 2)))
if (!(block = controller_block_alloc("ex2", name, sizeof(struct controller_block_private))))
return NULL;
if (controller_block_interm_list_init(block, interms_2))
......
......@@ -246,8 +246,10 @@ static void setpoint_generator_3d_calculate(struct controller_block *spg)
t_max_a = priv->t_max_a;
if (*priv->reset) {
/* Drain the queue */
struct controller_command_entry entry;
controller_command_queue_read(priv->command, &entry);
priv->cmd_x = *priv->reset_x;
cur_x = priv->cmd_x;
priv->cur_done = true;
......@@ -266,7 +268,6 @@ static void setpoint_generator_3d_calculate(struct controller_block *spg)
goto set_output;
}
controller_command_queue_read(priv->command, &entry);
}
if (*priv->track_x_cmd) {
priv->cur_done = false;
......
......@@ -20,6 +20,7 @@
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <log/log.h>
#include <controller/controller_block.h>
......
......@@ -6,6 +6,7 @@ $(eval $(call SUBDIR,il2c))
BLOCKS := \
add \
and \
bool2float \
bridge_pwm \
command_bool \
command_float \
......
......@@ -66,10 +66,12 @@ int controller_block_trace_add(char *blockname, char *outterm,
block = controller_block_get(i);
log_send(LOG_T_DEBUG, "block %s", block->name);
if (strcmp(blockname, block->name))
continue;
for (j = 0; j < block->outputs; j++) {
log_send(LOG_T_DEBUG, "block %s %s", block->name, block->output[j].name);
if (!strcmp(outterm, block->output[j].name)){
ptr = block->output[j].value.v;
type = block->output[j].type;
......@@ -78,8 +80,8 @@ int controller_block_trace_add(char *blockname, char *outterm,
}
if (!ptr) {
log_send(LOG_T_ERROR,
"Terminal %s of block %s not found, cannot add trace",
blockname, outterm);
"Terminal '%s' of block '%s' not found, cannot add trace",
outterm, blockname);
return -1;
}
......
......@@ -107,9 +107,16 @@ blocks ($(frequency), $(delay)) {
{ "log", "elevation_torsion_range_log" }
{ "log", "elevation_position_safe_log" }
{ "command_bool", "focusbox_plus" }
{ "command_bool", "focusbox_min" }
{ "ex2", "focusbox_exclusive" }
{ "command_float", "focusbox_setpoint" }
{ "command_bool", "focusbox_enable" }
{ "ex2", "focusbox_exclusive" }
{ "subtract", "focusbox_error" }
{ "comparator", "focusbox_plus" }
{ "comparator", "focusbox_min" }
{ "and2", "focusbox_plus_enable" }
{ "and2", "focusbox_min_enable" }
{ "value_float", "focusbox_lim_plus", 0.25 }
{ "value_float", "focusbox_lim_min", -0.25 }
{ "value_bool", "false" }
{ "value_float", "zero" }
......@@ -228,8 +235,18 @@ links {
{ "elevation_torque_l", "value", "dt_el_l", "torque" , true }
{ "elevation_servo_state", "enable", "dt_el_r", "enable" , true }
{ "elevation_servo_state", "enable", "dt_el_l", "enable" , true }
{ "focusbox_plus", "value", "focusbox_exclusive", "a" , true }
{ "focusbox_min", "value", "focusbox_exclusive", "b" , true }
{ "dt_az", "ae1", "focusbox_error", "negative" , true }
{ "focusbox_setpoint", "value", "focusbox_error", "positive" , true }
{ "focusbox_error", "difference", "focusbox_plus", "positive" , true }
{ "focusbox_lim_plus", "value", "focusbox_plus", "negative" , true }
{ "focusbox_lim_min", "value", "focusbox_min", "positive" , true }
{ "focusbox_error", "difference", "focusbox_min", "negative" , true }
{ "focusbox_plus", "out", "focusbox_plus_enable", "a" , true }
{ "focusbox_enable", "value", "focusbox_plus_enable", "b" , true }
{ "focusbox_min", "out", "focusbox_min_enable", "a" , true }
{ "focusbox_enable", "value", "focusbox_min_enable", "b" , true }
{ "focusbox_plus_enable", "q", "focusbox_exclusive", "a" , true }
{ "focusbox_min_enable", "q", "focusbox_exclusive", "b" , true }
{ "focusbox_exclusive", "qa", "dt_el_r", "ba1" , true }
{ "focusbox_exclusive", "qb", "dt_el_r", "ba2" , true }
{ "false", "value", "dt_el_l", "ba1" , true }
......
......@@ -16,9 +16,12 @@ blocks ($(frequency), $(delay)) {
{ "value_bool", "dt_az_safety_p" }
{ "not", "dt_az_safety_p_not" }
{ "value_bool", "dt_az_bex" }
{ "value", "dt_az_focusbox" }
{ "not", "dt_az_ba1" }
{ "not", "dt_az_ba2" }
{ "subtract", "dt_sim_focusbox_dir" }
{ "pid_aw", "dt_sim_focusbox" }
{ "value_bool", "dt_sim_focusbox_enable" }
module ("dt_az") {
{
......@@ -38,13 +41,18 @@ blocks ($(frequency), $(delay)) {
{ "be3", "dt_az_safety_p_not", "output" }
{ "be4", "dt_az_safety_p", "value" }
{ "be5", "dt_az_bex", "value" }
{ "ae1", "dt_az_focusbox", "value" }
{ "ae1", "dt_sim_focusbox", "out" }
{ "external_enable", "dt_az_bex", "value" }
}
}
}
links {
{ "dt_sim_focusbox_plus", "out", "dt_sim_focusbox_dir", "positive", false }
{ "dt_sim_focusbox_min", "out", "dt_sim_focusbox_dir", "negative", false }
{ "dt_sim_focusbox_dir", "difference", "dt_sim_focusbox", "in", true }
{ "dt_sim_focusbox_enable", "value", "dt_sim_focusbox", "enable", true }
{ "dt_az_speed_limit", "out", "dt_az_speed_quantize", "in", true }
{ "dt_az_speed_quantize", "out", "dt_az_speed_fb", "positive", true }
{ "dt_az_speed_fb", "difference", "dt_az_driver", "in", true }
......@@ -65,6 +73,9 @@ links {
}
params {
{ "dt_sim_focusbox", "ki", (float)0.1 }
{ "dt_sim_focusbox", "maxw", (float)10.0 }
{ "dt_sim_focusbox", "minw", (float)0.0 }
{ "dt_az_speed_limit", "max", (float) rpm2rads(3000.0) }
{ "dt_az_speed_limit", "min", (float)-rpm2rads(3000.0) }
{ "dt_az_speed_quantize", "quantum", (float) rpm2rads(3000.0)/16384.0 }
......@@ -82,9 +93,9 @@ params {
{ "dt_az_speed2pos", "ki", (float)1.0 }
{ "dt_az_speed2pos", "kd", (float)0.0 }
{ "dt_az_pos_quantize", "quantum", (float)2.0 * pi / 16384.0 }
{ "dt_az_focusbox", "value", (float)5.0 }
{ "dt_az_offset", "value", (float)-21730.3997815 }
{ "dt_az_bex", "value", (int)0 }
{ "dt_az_safety_p", "value", (int)1 }
{ "dt_az_safety_n", "value", (int)1 }
{ "dt_sim_focusbox_enable", "value", (int)1 }
}
......@@ -38,8 +38,8 @@ blocks ($(frequency), $(delay)) {
{ "not", "dt_el_safety_b_not" }
{ "value_bool", "dt_el_bex" }
{ "value", "dt_el_ae1" }
{ "log", "dt_el_ba1_r" }
{ "log", "dt_el_ba2_r" }
{ "bool2float", "dt_sim_focusbox_plus" }
{ "bool2float", "dt_sim_focusbox_min" }
{ "not", "dt_el_ba1_l" }
{ "not", "dt_el_ba2_l" }
......@@ -48,8 +48,8 @@ blocks ($(frequency), $(delay)) {
{ "speed", "dt_el_speed_limit_r", "in" }
{ "torque", "dt_el_torque_gain_r", "in" }
{ "enable", "dt_el_enabled", "input" }
{ "ba1", "dt_el_ba1_r", "condition" }
{ "ba2", "dt_el_ba2_r", "condition" }
{ "ba1", "dt_sim_focusbox_plus", "in" }
{ "ba2", "dt_sim_focusbox_min", "in" }
}
{
{ "position", "dt_el_pos_quantize_r", "out" }
......@@ -137,11 +137,6 @@ params {
{ "dt_el_safety_t", "value", true }
{ "dt_el_safety_b", "value", (bool)1 }
{ "dt_el_bex", "value", (bool)0 }
{ "dt_el_ba1_r", "msg_up", 2, "Focusbox Plus activated" }
{ "dt_el_ba1_r", "msg_down", 2, "Focusbox Plus deactivated" }
{ "dt_el_ba2_r", "msg_up", 2, "Focusbox Min activated" }
{ "dt_el_ba2_r", "msg_down", 2, "Focusbox Min deactivated" }
}
links {
......
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