Commit 8b90f674 authored by Jeroen Vreeken's avatar Jeroen Vreeken
Browse files

Add delay block

Create network with spg and pid
parent 0e311aef
/*
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 },
};
...@@ -17,6 +17,7 @@ BLOCKS := \ ...@@ -17,6 +17,7 @@ BLOCKS := \
debug_bool \ debug_bool \
debug_sint32 \ debug_sint32 \
decoder_uint32_bool \ decoder_uint32_bool \
delay_bool \
ex \ ex \
filter_iir \ filter_iir \
filter_lp \ filter_lp \
......
...@@ -47,7 +47,12 @@ blocks ($(frequency), $(delay)) { ...@@ -47,7 +47,12 @@ blocks ($(frequency), $(delay)) {
{ "not", "button1n" } { "not", "button1n" }
{ "and2", "track_x_cmd" } { "and2", "track_x_cmd" }
{ "delay", "spg_reset" }
{ "setpoint_generator_3d", "spg", "spg", "rad" } { "setpoint_generator_3d", "spg", "spg", "rad" }
{ "subtract", "error" }
{ "deadzone", "error_deadzone" }
{ "pid_aw", "pid" }
{ "add", "speed_ff" }
{ "value_bool", "false" } { "value_bool", "false" }
{ "value_float", "zero" } { "value_float", "zero" }
...@@ -60,6 +65,10 @@ blocks ($(frequency), $(delay)) { ...@@ -60,6 +65,10 @@ blocks ($(frequency), $(delay)) {
} }
alias { alias {
{ "position", "counter0", "position" }
{ "motor_speed", "counter1", "speed" }
{ "error", "error", "difference" }
{ "setpoint", "spg", "x" }
} }
set w_nom 1800.0 set w_nom 1800.0
...@@ -73,38 +82,50 @@ set V_supply 24.0 #27.6? ...@@ -73,38 +82,50 @@ set V_supply 24.0 #27.6?
set gear_ratio 100.0 * 19.33 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 { links {
{ "button0", "value", "button0n", "input", true } { "button0", "value", "button0n", "input", true }
{ "button0n", "output", "counter0", "reset", true }
{ "button1", "value", "button1n", "input", true } { "button1", "value", "button1n", "input", true }
{ "button1n", "output", "track_x_cmd", "a", true } { "button1n", "output", "track_x_cmd", "a", true }
{ "counter0", "homed", "track_x_cmd", "b", true } { "counter0", "homed", "track_x_cmd", "b", true }
{ "false", "value", "counter0", "reset", true }
{ "false", "value", "counter1", "reset", true } { "false", "value", "counter1", "reset", true }
{ "afec0", "value", "joystick_deadzone", "in", true } { "afec0", "value", "joystick_deadzone", "in", true }
{ "joystick_deadzone", "out", "joystick2w", "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 } { "zero", "value", "spg", "track_x", true }
{ "joystick2w", "out", "spg", "track_v", true } { "joystick2w", "out", "spg", "track_v", true }
{ "track_x_cmd", "q", "spg", "track_x_cmd", true } { "track_x_cmd", "q", "spg", "track_x_cmd", true }
{ "button0n", "output", "spg", "track_v_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 } { "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_model", "I", "motor_i2t", "I", true }
{ "motor_i2t", "Ilim", "Ilim2Vlim", "in", true } { "motor_i2t", "Ilim", "Ilim2Vlim", "in", true }
{ "Ilim2Vlim", "out", "Vlim", "limit", true } { "Ilim2Vlim", "out", "Vlim", "limit", true }
{ "w2V", "out", "Vlim", "in", true }
{ "Vlim", "out", "V2pwm", "in", true }
{ "V2pwm", "out", "pwm0", "in", true }
# Status leds # Status leds
{ "counter0", "homed", "status1_LED", "value", true } { "counter0", "homed", "status1_LED", "value", true }
...@@ -120,16 +141,16 @@ links { ...@@ -120,16 +141,16 @@ links {
# Packet output # Packet output
{ "counter0", "position", "out_position", "value", true } { $<position>, "out_position", "value", true }
{ "afec0", "value", "out_stick", "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 } { "counter0", "homed", "out_homed", "value", true }
} }
params { params {
{ "w2V", "gain", (float)$(gear_ratio)*$(V_nom)/$(w_nom) } { "w2V", "gain", (float)$(gear_ratio)*$(V_nom)/$(w_nom) }
{ "V2pwm", "gain", (float)1.0/$(V_supply) } { "V2pwm", "gain", (float)1.0/$(V_supply) }
{ "joystick2w", "gain", (float)$(w_nom)/$(gear_ratio) } { "joystick2w", "gain", (float)$(max_speed) }
# GNM8055/4 # GNM8055/4
# 24V 28A 490W # 24V 28A 490W
...@@ -151,5 +172,20 @@ params { ...@@ -151,5 +172,20 @@ params {
{ "joystick_deadzone", "deadzone", (float)0.1 } { "joystick_deadzone", "deadzone", (float)0.1 }
{ "joystick_deadzone", "intercept", (float)1.0 } { "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 }
} }
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