Skip to content
GitLab
Projects
Groups
Snippets
/
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in
Toggle navigation
Menu
Open sidebar
auke.klazema
dt_ctrl
Commits
bc8ae403
Commit
bc8ae403
authored
Aug 24, 2014
by
Jeroen Vreeken
Browse files
Merge branch 'corso2013' into corso2014
Conflicts: Makefile console/htdocs/index.html controller/block/Makefile
parents
281bd15b
d068fe3e
Changes
26
Hide whitespace changes
Inline
Side-by-side
Makefile
View file @
bc8ae403
...
...
@@ -33,9 +33,13 @@ help:
libnova.la
:
cd
libnova-0.13.0
;
./configure
-enable-static
-disable-shared
--prefix
=
${CURDIR}
;
make
;
make
install
applications
:
controller console
$(MAKE)
-C
applications/corso2013
clean
:
subdirs_CLEAN
rm
-rf
common/lib/
*
cd
libnova-0.13.0
;
make clean
||
true
$(MAKE)
-C
applications/corso2013 clean
# Rules for building a release
...
...
applications/corso2013/.gitignore
0 → 100644
View file @
bc8ae403
corso_cmd
corso_joystick
corso_slave
*.ctrl.dot
*.ctrl.pdf
applications/corso2013/Makefile
0 → 100644
View file @
bc8ae403
include
../../console/build.mk
LDFLAGS
+=
-L
../../common/lib/
-lutils
-static
LDFLAGS
+=
`
pkg-config gstreamer-1.0
--libs
`
CFLAGS
+=
`
pkg-config gstreamer-1.0
--cflags
`
-Wall
-O3
-I
../../common/utils/
-I
../../common/include
all
:
corso_cmd corso_slave corso_joystick
corso_cmd
:
setpoint.o corso_cmd.o
corso_slave
:
corso_slave.o
corso_joystick
:
corso_joystick.o setpoint.o
clean
:
rm
-rf
*
.o
*
.d lib/
*
*
.lo
*
.a
*
.la .libs
rm
-f
corso_cmd
rm
-f
corso_slave
rm
-f
corso_joystick
applications/corso2013/corso2013.ctrl
0 → 100644
View file @
bc8ae403
frequency 1000
blocks {
{ "ec", "ethercat", "eth0", 6, 0 }
{ "beckhoff_el2xxx", "output_top", "EL2008", 1 }
{ "beckhoff_el2xxx", "output_low", "EL2008", 2 }
{ "beckhoff_el1xxx", "inputblock1", "EL1008", 4 }
{ "beckhoff_el3xxx", "analog_in", "EL3102", 5 }
{ "value_bool", "false" }
{ "value_bool", "true" }
{ "value", "zero" }
{ "filter_lp", "distance_l_lp" }
{ "filter_lp", "distance_r_lp" }
{ "inverse_proportional", "distance_l" }
{ "inverse_proportional", "distance_r" }
{ "matrix_2x2", "input_matrix" }
{ "rangecheck", "diff_check" }
{ "rangecheck", "diff_deadband" }
{ "trigger", "spg_enable" }
{ "trigger", "spg_disable" }
{ "setreset", "spg_enabled" }
{ "or3", "panic_or" }
{ "not", "panic" }
{ "not", "disabled" }
{ "and2", "diff_enable" }
{ "setpoint_generator", "lift_spg", "lift_spg", "m" }
{ "subtract", "distance_error" }
{ "pid_aw", "distance_pid" }
{ "pid_aw", "diff_pid" }
{ "add", "lift_add" }
{ "matrix_2x2", "output_matrix" }
{ "bridge_pwm", "lift_ll_pwm" }
{ "bridge_pwm", "lift_lh_pwm" }
{ "bridge_pwm", "lift_rl_pwm" }
{ "bridge_pwm", "lift_rh_pwm" }
}
links {
{ "false", "value", "output_low", "output1", true }
{ "false", "value", "output_low", "output2", true }
{ "false", "value", "output_low", "output5", true }
{ "false", "value", "output_low", "output6", true }
{ "false", "value", "output_low", "output7", true }
{ "false", "value", "output_low", "output8", true }
{ "spg_enable", "q", "spg_enabled", "set", true }
{ "inputblock1", "input3", "panic", "input", true }
{ "panic", "output", "panic_or", "a", true }
{ "spg_disable", "q", "panic_or", "b", true }
{ "panic_or", "q", "spg_enabled", "reset", true }
{ "panic_or", "q", "output_low", "output3", true }
{ "spg_enabled", "output", "disabled", "input", true }
{ "spg_enabled", "output", "output_low", "output4", true }
{ "analog_in", "input1", "distance_l_lp", "in", true }
{ "analog_in", "input2", "distance_r_lp", "in", true }
{ "distance_l_lp", "out", "distance_l", "in", true }
{ "distance_r_lp", "out", "distance_r", "in", true }
{ "distance_l", "out", "input_matrix", "in0", true }
{ "distance_r", "out", "input_matrix", "in1", true }
{ "disabled", "output", "lift_spg", "reset", true }
{ "input_matrix", "out0", "lift_spg", "reset_x", true }
{ "lift_spg", "x", "distance_error", "positive", true }
{ "input_matrix", "out0", "distance_error", "negative", true }
{ "input_matrix", "out1", "diff_check", "in", true }
{ "diff_check", "invalid", "panic_or", "c", true }
{ "input_matrix", "out1", "diff_deadband", "in", true }
{ "diff_check", "valid", "diff_enable", "a", true }
{ "spg_enabled", "output", "diff_enable", "b", true }
{ "distance_error", "difference", "distance_pid", "in", true }
{ "spg_enabled", "output", "distance_pid", "enable", true }
{ "lift_spg", "v", "lift_add", "in0", true }
{ "distance_pid", "out", "lift_add", "in1", true }
{ "lift_add", "out", "output_matrix", "in0", true }
{ "input_matrix", "out1", "diff_pid", "in", true }
{ "diff_pid", "out", "output_matrix", "in1", true }
{ "diff_enable", "q", "diff_pid", "enable", true }
{ "output_matrix", "out0", "lift_ll_pwm", "in", true }
{ "output_matrix", "out0", "lift_lh_pwm", "in", true }
{ "output_matrix", "out1", "lift_rl_pwm", "in", true }
{ "output_matrix", "out1", "lift_rh_pwm", "in", true }
{ "lift_ll_pwm", "b", "output_top", "output1", true }
{ "lift_ll_pwm", "a", "output_top", "output2", true }
{ "lift_lh_pwm", "b", "output_top", "output3", true }
{ "lift_lh_pwm", "a", "output_top", "output4", true }
{ "lift_rl_pwm", "b", "output_top", "output5", true }
{ "lift_rl_pwm", "a", "output_top", "output6", true }
{ "lift_rh_pwm", "b", "output_top", "output7", true }
{ "lift_rh_pwm", "a", "output_top", "output8", true }
}
set motor_speed 0.018
params {
{ "true", "value", true }
{ "lift_spg", "setpoint", (float)0.0 }
{ "lift_spg", "t", (float)0.001 }
{ "lift_spg", "max_x", (float)0.8 }
{ "lift_spg", "min_x", (float)0.0 }
{ "lift_spg", "max_v", (float)0.015 }
{ "lift_spg", "max_a", (float)0.01 }
{ "lift_spg", "max_j", (float)0.01 }
{ "lift_spg", "precision_x", (float)0.005 }
{ "lift_spg", "precision_v", (float)0.005 }
{ "lift_spg", "precision_a", (float)0.005 }
{ "distance_l_lp", "t", (float)0.001 }
{ "distance_l_lp", "tau", (float)2.0 }
{ "distance_r_lp", "t", (float)0.001 }
{ "distance_r_lp", "tau", (float)2.0 }
{ "distance_l", "c0", (float)-0.495196 }
{ "distance_l", "c1", (float)1.42175 }
{ "distance_l", "c2", (float)0.515971 }
{ "distance_r", "c0", (float)-0.407968 }
{ "distance_r", "c1", (float)1.18524}
{ "distance_r", "c2", (float)0.374365 }
{ "input_matrix", "constants", (float) { 0.5, 0.5 },
(float) { 1.0, -1.0 } }
{ "diff_check", "max", (float) 0.10 }
{ "diff_check", "min", (float) -0.10 }
{ "diff_deadband", "max", (float) 0.06 }
{ "diff_deadband", "min", (float) -0.06 }
{ "distance_pid", "kp", (float)0.05 }
{ "distance_pid", "ki", (float)0.0 }
{ "distance_pid", "kd", (float)0.0 }
{ "distance_pid", "maxw", (float)0.005 }
{ "distance_pid", "minw", (float)-0.005 }
{ "diff_pid", "kp", (float)0.0 }
{ "diff_pid", "ki", (float)0.0 }
{ "diff_pid", "kd", (float)0.0 }
{ "diff_pid", "maxw", (float)0.005 }
{ "diff_pid", "minw", (float)-0.005 }
{ "output_matrix", "constants", (float) { 1.0/$(motor_speed), -0.5/$(motor_speed) },
(float) { 1.0/$(motor_speed), 0.5/$(motor_speed) } }
{ "lift_ll_pwm", "divider", 10 }
{ "lift_ll_pwm", "continuous_on", false }
{ "lift_lh_pwm", "divider", 10 }
{ "lift_lh_pwm", "continuous_on", false }
{ "lift_rl_pwm", "divider", 10 }
{ "lift_rl_pwm", "continuous_on", false }
{ "lift_rh_pwm", "divider", 10 }
{ "lift_rh_pwm", "continuous_on", false }
}
traces {
{ "distance", "m/s", "input_matrix", "out0" }
}
applications/corso2013/corso2013_backup.ctrl
0 → 100644
View file @
bc8ae403
frequency 1000
blocks {
{ "ec", "ethercat", "eth0", 6, 0 }
{ "beckhoff_el2xxx", "output_top", "ethercat", 1, "EL2008" }
{ "beckhoff_el2xxx", "output_low", "ethercat", 2, "EL2008" }
{ "beckhoff_el1xxx", "inputblock1", "ethercat", 4, "EL1008" }
{ "beckhoff_el3xxx", "analog_in", "ethercat", 5, "EL3102" }
{ "value_bool", "false" }
{ "value_bool", "true" }
{ "value", "zero" }
{ "filter_lp", "distance_l_lp" }
{ "filter_lp", "distance_r_lp" }
{ "inverse_proportional", "distance_l" }
{ "inverse_proportional", "distance_r" }
{ "matrix_2x2", "input_matrix" }
{ "rangecheck", "diff_check" }
{ "rangecheck", "diff_deadband" }
{ "trigger", "spg_enable" }
{ "trigger", "spg_disable" }
{ "setreset", "spg_enabled" }
{ "or3", "panic_or" }
{ "not", "panic" }
{ "not", "disabled" }
{ "and2", "diff_enable" }
{ "setpoint_generator", "lift_spg", "lift_spg", "m" }
{ "matrix_2x2", "output_matrix" }
{ "bridge_pwm", "lift_ll_pwm" }
{ "bridge_pwm", "lift_lh_pwm" }
{ "bridge_pwm", "lift_rl_pwm" }
{ "bridge_pwm", "lift_rh_pwm" }
}
links {
{ "false", "value", "output_low", "output1", true }
{ "false", "value", "output_low", "output2", true }
{ "false", "value", "output_low", "output5", true }
{ "false", "value", "output_low", "output6", true }
{ "false", "value", "output_low", "output7", true }
{ "false", "value", "output_low", "output8", true }
{ "spg_enable", "q", "spg_enabled", "set", true }
{ "inputblock1", "input3", "panic", "input", true }
{ "panic", "output", "panic_or", "a", true }
{ "spg_disable", "q", "panic_or", "b", true }
{ "panic_or", "q", "spg_enabled", "reset", true }
{ "panic_or", "q", "output_low", "output3", true }
{ "spg_enabled", "output", "disabled", "input", true }
{ "spg_enabled", "output", "output_low", "output4", true }
{ "analog_in", "input1", "distance_l_lp", "in", true }
{ "analog_in", "input2", "distance_r_lp", "in", true }
{ "distance_l_lp", "out", "distance_l", "in", true }
{ "distance_r_lp", "out", "distance_r", "in", true }
{ "distance_l", "out", "input_matrix", "in0", true }
{ "distance_r", "out", "input_matrix", "in1", true }
{ "disabled", "output", "lift_spg", "reset", true }
{ "input_matrix", "out0", "lift_spg", "reset_x", true }
{ "input_matrix", "out1", "diff_check", "in", true }
{ "diff_check", "invalid", "panic_or", "c", true }
{ "input_matrix", "out1", "diff_deadband", "in", true }
{ "diff_check", "valid", "diff_enable", "a", true }
{ "spg_enabled", "output", "diff_enable", "b", true }
{ "lift_spg", "v", "output_matrix", "in0", true }
{ "zero", "value", "output_matrix", "in1", true }
{ "output_matrix", "out0", "lift_ll_pwm", "in", true }
{ "output_matrix", "out0", "lift_lh_pwm", "in", true }
{ "output_matrix", "out1", "lift_rl_pwm", "in", true }
{ "output_matrix", "out1", "lift_rh_pwm", "in", true }
{ "lift_ll_pwm", "b", "output_top", "output1", true }
{ "lift_ll_pwm", "a", "output_top", "output2", true }
{ "lift_lh_pwm", "b", "output_top", "output3", true }
{ "lift_lh_pwm", "a", "output_top", "output4", true }
{ "lift_rl_pwm", "b", "output_top", "output5", true }
{ "lift_rl_pwm", "a", "output_top", "output6", true }
{ "lift_rh_pwm", "b", "output_top", "output7", true }
{ "lift_rh_pwm", "a", "output_top", "output8", true }
}
set motor_speed 0.018
params {
{ "true", "value", true }
{ "lift_spg", "setpoint", (float)0.0 }
{ "lift_spg", "t", (float)0.001 }
{ "lift_spg", "max_x", (float)10.0 }
{ "lift_spg", "min_x", (float)-10.0 }
{ "lift_spg", "max_v", (float)0.015 }
{ "lift_spg", "max_a", (float)0.01 }
{ "lift_spg", "max_j", (float)0.01 }
{ "lift_spg", "precision_x", (float)0.005 }
{ "lift_spg", "precision_v", (float)0.005 }
{ "lift_spg", "precision_a", (float)0.005 }
{ "distance_l_lp", "t", (float)0.001 }
{ "distance_l_lp", "tau", (float)2.0 }
{ "distance_r_lp", "t", (float)0.001 }
{ "distance_r_lp", "tau", (float)2.0 }
{ "distance_l", "c0", (float)-0.495196 }
{ "distance_l", "c1", (float)1.42175 }
{ "distance_l", "c2", (float)0.515971 }
{ "distance_r", "c0", (float)-0.407968 }
{ "distance_r", "c1", (float)1.18524}
{ "distance_r", "c2", (float)0.374365 }
{ "input_matrix", "constants", (float) { 0.5, 0.5 },
(float) { 1.0, -1.0 } }
{ "diff_check", "max", (float) 0.15 }
{ "diff_check", "min", (float) -0.15 }
{ "diff_deadband", "max", (float) 0.06 }
{ "diff_deadband", "min", (float) -0.06 }
{ "output_matrix", "constants", (float) { 1.0/$(motor_speed), -0.5/$(motor_speed) },
(float) { 1.0/$(motor_speed), 0.5/$(motor_speed) } }
{ "lift_ll_pwm", "divider", 10 }
{ "lift_ll_pwm", "continuous_on", false }
{ "lift_lh_pwm", "divider", 10 }
{ "lift_lh_pwm", "continuous_on", false }
{ "lift_rl_pwm", "divider", 10 }
{ "lift_rl_pwm", "continuous_on", false }
{ "lift_rh_pwm", "divider", 10 }
{ "lift_rh_pwm", "continuous_on", false }
}
traces {
{ "distance", "m/s", "input_matrix", "out0" }
}
applications/corso2013/corso2013_stub.ctrl
0 → 100644
View file @
bc8ae403
frequency 1000
blocks {
{ "setpoint_generator", "lift_spg", "lift_spg", "m" }
{ "value", "zero" }
{ "value_bool", "false" }
{ "value", "distance" }
{ "value", "diff" }
{ "matrix_2x2", "output_matrix" }
}
set motor_speed 0.018
params {
{ "lift_spg", "setpoint", (float)0.0 }
{ "lift_spg", "t", (float)0.001 }
{ "lift_spg", "max_x", (float)0.8 }
{ "lift_spg", "min_x", (float)0.0 }
{ "lift_spg", "max_v", (float)0.015 }
{ "lift_spg", "max_a", (float)0.01 }
{ "lift_spg", "max_j", (float)0.01 }
{ "lift_spg", "precision_x", (float)0.005 }
{ "lift_spg", "precision_v", (float)0.005 }
{ "lift_spg", "precision_a", (float)0.005 }
{ "distance", "value", (float)1.1 }
{ "diff", "value", (float)-0.035 }
{ "output_matrix", "constants", (float) { 1.0/$(motor_speed), -0.5/$(motor_speed) },
(float) { 1.0/$(motor_speed), 0.5/$(motor_speed) } }
}
links {
{ "zero", "value", "lift_spg", "reset_x", true }
{ "false", "value", "lift_spg", "reset", true }
{ "lift_spg", "v", "output_matrix", "in0", true }
{ "zero", "value", "output_matrix", "in1", true }
}
traces {
{ "oscillator", "Boolean", "oscillator0", "output" }
{ "distance", "m/s", "input_matrix", "out0" }
{ "input_matrix.out0", "test", "lift_spg", "x" }
{ "input_matrix.out1", "test", "diff", "value" }
}
applications/corso2013/corso_cmd.c
0 → 100644
View file @
bc8ae403
#include
<stdlib.h>
#include
<stdio.h>
#include
<gst/gst.h>
#include
<pthread.h>
#include
<unistd.h>
#include
"setpoint.h"
#include
"status_server.h"
#include
"corso_cmd.h"
#include
"tcp_connect.h"
#include
<string.h>
#include
<sys/types.h>
#include
<sys/stat.h>
#include
<fcntl.h>
#include
<linux/joystick.h>
#include
<signal.h>
static
char
*
command_host
=
"localhost"
;
static
int
command_port
=
21000
;
static
char
*
command_spg
=
"lift_spg"
;
static
char
*
shell_host
=
"localhost"
;
static
int
shell_port
=
11100
;
enum
state
{
STATE_IDLE
,
STATE_CYCLE
,
STATE_MANUAL
,
}
state
=
STATE_IDLE
;
char
*
enable_param
=
"controller 'params {"
" {
\"
spg_enable
\"
,
\"
trigger
\"
, true }"
" } '
\n
"
;
char
*
disable_param
=
"controller 'params {"
" {
\"
spg_disable
\"
,
\"
trigger
\"
, true }"
" } '
\n
"
;
void
send_shell
(
char
*
cmd
)
{
int
fd
;
fd
=
tcp_connect
(
shell_host
,
shell_port
);
if
(
fd
>=
0
)
{
write
(
fd
,
cmd
,
strlen
(
cmd
));
write
(
fd
,
"
\n
"
,
1
);
fsync
(
fd
);
usleep
(
100000
);
close
(
fd
);
}
else
{
printf
(
"Could not open shell port
\n
"
);
}
}
GstElement
*
play
;
struct
status_server
*
stat_srv
;
struct
setpoint_command
*
sp_command
;
gboolean
bus_callback
(
GstBus
*
bus
,
GstMessage
*
msg
,
gpointer
data
)
{
// GstElement *play = GST_ELEMENT(data);
switch
(
GST_MESSAGE_TYPE
(
msg
))
{
case
GST_MESSAGE_EOS
:
printf
(
"eos
\n
"
);
/* restart playback if at end */
break
;
default:
break
;
}
return
TRUE
;
}
void
*
gst_thread
(
void
*
arg
)
{
GMainLoop
*
loop
=
arg
;
/* now run */
g_main_loop_run
(
loop
);
printf
(
"Dropping out of g_main_loop?
\n
"
);
return
NULL
;
}
static
void
handle_joystick
(
int
fdjoy
)
{
struct
js_event
event
;
ssize_t
ret
;
ret
=
read
(
fdjoy
,
&
event
,
sizeof
(
event
));
if
(
ret
==
sizeof
(
event
))
{
if
(
event
.
type
&
JS_EVENT_BUTTON
&&
event
.
number
==
0
&&
event
.
value
)
{
state
=
STATE_CYCLE
;
printf
(
"CYCLE
\n
"
);
}
if
(
event
.
type
&
JS_EVENT_BUTTON
&&
event
.
number
==
1
&&
event
.
value
)
{
state
=
STATE_IDLE
;
printf
(
"IDLE
\n
"
);
gst_element_set_state
(
play
,
GST_STATE_READY
);
status_server_send
(
stat_srv
,
"0 one
\n
"
);
send_shell
(
disable_param
);
}
if
(
event
.
type
&
JS_EVENT_BUTTON
&&
event
.
number
==
2
&&
event
.
value
)
{
state
=
STATE_MANUAL
;
printf
(
"MANUAL
\n
"
);
// gst_element_set_state (play, GST_STATE_READY);
// status_server_send(stat_srv, "0 one\n");
}
if
(
event
.
type
&
JS_EVENT_BUTTON
&&
event
.
number
==
3
&&
event
.
value
)
{
printf
(
"Enabling
\n
"
);
send_shell
(
enable_param
);
}
if
((
state
==
STATE_MANUAL
||
state
==
STATE_CYCLE
)
&&
event
.
type
&
JS_EVENT_AXIS
&&
event
.
number
==
1
)
{
float
speed
;
speed
=
(
float
)
event
.
value
;
speed
*=
-
0
.
02
/
32768
.
0
;
printf
(
"speed: %f
\n
"
,
speed
);
setpoint_command_speed
(
sp_command
,
speed
);
}
}
}
int
main
(
int
argc
,
char
**
argv
)
{
struct
sched_param
param
;
pthread_t
thread
;
GMainLoop
*
loop
;
GstBus
*
bus
;
struct
timespec
t
;
char
statline
[
1000
];
int
fdjoy
;
char
*
joystick_device
=
"/dev/input/js0"
;
signal
(
SIGPIPE
,
SIG_IGN
);
stat_srv
=
status_server_create
(
CORSO_PORT
,
0
,
10
);
/* init GStreamer */
gst_init
(
&
argc
,
&
argv
);
loop
=
g_main_loop_new
(
NULL
,
FALSE
);
/* make sure we have a URI */
if
(
argc
!=
2
)
{
printf
(
"Usage: %s <URI>
\n
"
,
argv
[
0
]);
return
-
1
;
}
fdjoy
=
open
(
joystick_device
,
O_RDONLY
);
if
(
fdjoy
<
0
)
{
perror
(
"Could not open joystick device"
);
return
1
;
// fdjoy = 0;
// state = STATE_CYCLE;
}
ioctl
(
fdjoy
,
FIONBIO
,
&
(
int
){
1
});
do
{
sp_command
=
setpoint_command_init
(
command_host
,
command_port
,
command_spg
,
"corso_cmd"
);
if
(
!
sp_command
)
{
printf
(
"Could not open connection for commands
\n
"
);
sleep
(
1
);
}
}
while
(
!
sp_command
);
/* set up */
play
=
gst_element_factory_make
(
"playbin"
,
"play"
);
bus
=
gst_pipeline_get_bus
(
GST_PIPELINE
(
play
));
gst_bus_add_watch
(
bus
,
bus_callback
,
play
);
gst_object_unref
(
bus
);
param
.
sched_priority
=
90
;
if
(
sched_setscheduler
(
0
,
SCHED_FIFO
,
&
param
)
==
-
1
)
{
printf
(
"sched_setscheduler() failed: %d
\n
"
,
errno
);
}
pthread_create
(
&
thread
,
NULL
,
gst_thread
,
loop
);
clock_gettime
(
CLOCK_REALTIME
,
&
t
);
t
.
tv_nsec
=
0
;
t
.
tv_sec
++
;