Browse Source

Add delay block

Create network with spg and pid
jeroen
Jeroen Vreeken 3 years ago
parent
commit
8b90f6745d
3 changed files with 143 additions and 13 deletions
  1. +93
    -0
      controller/block/block_delay_bool.c
  2. +1
    -0
      controller/block/build.mk
  3. +49
    -13
      controller/ctrl_embedded.ctrl

+ 93
- 0
controller/block/block_delay_bool.c View File

@ -0,0 +1,93 @@
/*
Copyright Jeroen Vreeken (pe1rxq@amsat.org), 2018
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 |----
| |
----------------------
out is delayed by a sample
TODO: make delay configurable
*/
struct controller_block_private {
bool *in;
bool out;
bool tmp;
};
static void delay_calculate(struct controller_block *delay)
{
struct controller_block_private *priv = delay->private;
priv->out = priv->tmp;
priv->tmp = *priv->in;
}
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_BOOL, offsetof(struct controller_block_private, out) },
{ NULL }
};
static struct controller_block * block_delay_bool_create(char *name, int argc, va_list val)
{
struct controller_block *delay;
if (!(delay = controller_block_alloc("delay_bool", name, sizeof(struct controller_block_private))))
return NULL;
if (controller_block_interm_list_init(delay, interms))
goto err_block;
if (controller_block_outterm_list_init(delay, outterms))
goto err_block;
delay->calculate = delay_calculate;
if (controller_block_add(delay))
goto err_block;
return delay;
err_block:
controller_block_free(delay);
return NULL;
}
BLOCK_CREATE(delay_bool) = {
.create = block_delay_bool_create,
.args = { NULL },
};

+ 1
- 0
controller/block/build.mk View File

@ -17,6 +17,7 @@ BLOCKS := \
debug_bool \
debug_sint32 \
decoder_uint32_bool \
delay_bool \
ex \
filter_iir \
filter_lp \


+ 49
- 13
controller/ctrl_embedded.ctrl View File

@ -47,7 +47,12 @@ blocks ($(frequency), $(delay)) {
{ "not", "button1n" }
{ "and2", "track_x_cmd" }
{ "delay", "spg_reset" }
{ "setpoint_generator_3d", "spg", "spg", "rad" }
{ "subtract", "error" }
{ "deadzone", "error_deadzone" }
{ "pid_aw", "pid" }
{ "add", "speed_ff" }
{ "value_bool", "false" }
{ "value_float", "zero" }
@ -60,6 +65,10 @@ blocks ($(frequency), $(delay)) {
}
alias {
{ "position", "counter0", "position" }
{ "motor_speed", "counter1", "speed" }
{ "error", "error", "difference" }
{ "setpoint", "spg", "x" }
}
set w_nom 1800.0
@ -73,38 +82,50 @@ set V_supply 24.0 #27.6?
set gear_ratio 100.0 * 19.33
set max_speed $(w_nom)/$(gear_ratio)
set max_accel $(max_speed)
set max_jerk $(max_jerk)
links {
{ "button0", "value", "button0n", "input", true }
{ "button0n", "output", "counter0", "reset", true }
{ "button1", "value", "button1n", "input", true }
{ "button1n", "output", "track_x_cmd", "a", true }
{ "counter0", "homed", "track_x_cmd", "b", true }
{ "false", "value", "counter0", "reset", true }
{ "false", "value", "counter1", "reset", true }
{ "afec0", "value", "joystick_deadzone", "in", true }
{ "joystick_deadzone", "out", "joystick2w", "in", true }
{ "joystick2w", "out", "w2V", "in", true }
{ "w2V", "out", "Vlim", "in", true }
{ "Vlim", "out", "V2pwm", "in", true }
{ "V2pwm", "out", "pwm0", "in", true }
{ "counter0", "position", "spg", "reset_x", true }
{ $<position>, "spg", "reset_x", true }
{ "zero", "value", "spg", "track_x", true }
{ "joystick2w", "out", "spg", "track_v", true }
{ "track_x_cmd", "q", "spg", "track_x_cmd", true }
{ "button0n", "output", "spg", "track_v_cmd", true }
{ "false", "value", "spg", "reset", true }
{ "counter0", "homed", "spg_reset", "in", true }
{ "spg_reset", "out", "spg", "reset", true }
{ "spg", "v", "speed_ff", "in1", true }
{ $<setpoint>, "error", "positive", true }
{ $<position>, "error", "negative", true }
{ $<error>, "error_deadzone", "in", true }
{ "error_deadzone", "out", "pid", "in", true }
{ "pid", "out", "speed_ff", "in0", true }
{ "speed_ff", "out", "w2V", "in", true }
{ "w2V", "out", "motor_model", "V", false }
{ "counter1", "speed", "motor_model", "w", true }
{ $<motor_peed>, "motor_model", "w", true }
{ "motor_model", "I", "motor_i2t", "I", true }
{ "motor_i2t", "Ilim", "Ilim2Vlim", "in", true }
{ "Ilim2Vlim", "out", "Vlim", "limit", true }
{ "w2V", "out", "Vlim", "in", true }
{ "Vlim", "out", "V2pwm", "in", true }
{ "V2pwm", "out", "pwm0", "in", true }
# Status leds
{ "counter0", "homed", "status1_LED", "value", true }
@ -120,16 +141,16 @@ links {
# Packet output
{ "counter0", "position", "out_position", "value", true }
{ $<position>, "out_position", "value", true }
{ "afec0", "value", "out_stick", "value", true }
{ "counter1", "speed", "out_speed", "value", true }
{ $<motor_speed>, "out_speed", "value", true }
{ "counter0", "homed", "out_homed", "value", true }
}
params {
{ "w2V", "gain", (float)$(gear_ratio)*$(V_nom)/$(w_nom) }
{ "V2pwm", "gain", (float)1.0/$(V_supply) }
{ "joystick2w", "gain", (float)$(w_nom)/$(gear_ratio) }
{ "joystick2w", "gain", (float)$(max_speed) }
# GNM8055/4
# 24V 28A 490W
@ -151,5 +172,20 @@ params {
{ "joystick_deadzone", "deadzone", (float)0.1 }
{ "joystick_deadzone", "intercept", (float)1.0 }
{ "spg", "max_x", (float)deg2rad( 45.0) }
{ "spg", "min_x", (float)deg2rad(-30.0) }
{ "spg", "max_v", (float)$(max_speed) }
{ "spg", "max_a", (float)$(max_accel) }
{ "spg", "max_j", (float)$(max_jerk) }
{ "error_deadzone", "deadzone", (float)deg2rad(1.0) }
{ "error_deadzone", "intercepts", (float)deg2rad(45.0) }
{ "pid", "kp", (float)$(max_speed) }
{ "pid", "ki", (float)$(max_speed)/10.0 }
{ "pid", "kd", (float)0.0 }
{ "pid", "maxw", (float)$(max_speed)/10.0 }
{ "pid", "minw", (float)$(max_speed)/-10.0 }
}

Loading…
Cancel
Save