Skip to content
GitLab
Commit bc8ae403 authored by Jeroen Vreeken's avatar Jeroen Vreeken
Browse files

Merge branch 'corso2013' into corso2014 

Conflicts:
	Makefile
	console/htdocs/index.html
	controller/block/Makefile
parents 281bd15b d068fe3e
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++;