Commit 574ed079 authored by Jeroen Vreeken's avatar Jeroen Vreeken
Browse files

New state machine block

Added integer variables (nice for defining named states in the state machine)
Various improvements to a few blocks
	joystick uses controller bus
	multiplexer and trajectplayer can handle non existing selects
(cherry picked from commit 82abe0ef)
parent 35a7e2fb
......@@ -26,15 +26,20 @@
#include <fcntl.h>
#include <controller/controller_block.h>
#include <controller/controller_bus.h>
#include <log/log.h>
#include <linux/joystick.h>
#include <errno.h>
struct controller_block_private {
float *axes;
bool *buttons;
char *device;
int fd;
struct controller_bus *bus;
};
static void calculate(struct controller_block *block)
......@@ -43,6 +48,9 @@ static void calculate(struct controller_block *block)
struct js_event event;
int ret;
if (priv->fd < 0)
return;
do {
ret = read (priv->fd, &event, sizeof(struct js_event));
if (ret == sizeof(struct js_event)) {
......@@ -54,8 +62,55 @@ static void calculate(struct controller_block *block)
}
}
} while (ret == sizeof(struct js_event));
if (ret <= 0 && errno != EAGAIN) {
close(priv->fd);
priv->fd = -1;
log_send(LOG_T_ERROR, "%s: Error reading device", block->name);
}
}
static int joystick_open(char *device)
{
int fd;
int ret;
fd = open(device, O_RDONLY);
if (fd < 0)
return -1;
ret = ioctl(fd, FIONBIO, &(int){1});
if (ret)
goto err_ioctl;
return fd;
err_ioctl:
close(fd);
return -1;
}
static enum controller_bus_state joystick_poll(struct controller_bus *bus)
{
struct controller_block *block = bus->owner;
int fd = block->private->fd;
if (fd >= 0)
return CONTROLLER_BUS_STATE_OK;
fd = joystick_open(block->private->device);
if (fd < 0)
return CONTROLLER_BUS_STATE_ERROR;
block->private->fd = fd;
log_send(LOG_T_WARNING, "%s: re-opened %s", block->name,
block->private->device);
return CONTROLLER_BUS_STATE_OK;
}
struct controller_block * block_joystick_create(char *name, va_list ap)
{
struct controller_block *block;
......@@ -63,38 +118,27 @@ struct controller_block * block_joystick_create(char *name, va_list ap)
char *device;
int nr_axes = 0, nr_buttons = 0;
int i, ret;
bool sim = false;
device = va_arg(ap, char *);
fd = open(device, O_RDONLY);
fd = joystick_open(device);
if (fd < 0) {
printf("Could not open device %s\n", device);
printf("Going to simulate a joystick\n");
sim = true;
device = "simulated joystick";
nr_axes = 6;
nr_buttons = 8;
} else {
ret = ioctl(fd, FIONBIO, &(int){1});
if (ret)
goto err_ioctl;
ret = ioctl(fd, JSIOCGAXES, &nr_axes);
if (ret)
goto err_ioctl;
ret = ioctl(fd, JSIOCGBUTTONS, &nr_buttons);
if (ret)
goto err_ioctl;
log_send(LOG_T_ERROR, "Could not open device %s", device);
return NULL;
}
printf("%s: %d axes, %d buttons\n", device, nr_axes, nr_buttons);
ret = ioctl(fd, JSIOCGAXES, &nr_axes);
if (ret)
goto err_ioctl;
ret = ioctl(fd, JSIOCGBUTTONS, &nr_buttons);
if (ret)
goto err_ioctl;
log_send(LOG_T_DEBUG, "%s: %d axes, %d buttons",
device, nr_axes, nr_buttons);
block = malloc(sizeof(struct controller_block));
if (!block)
goto err_block;
block->private = malloc(sizeof(struct controller_block_private));
if (!block->private)
goto err_private;
if (!(block = controller_block_alloc("joystick", name, sizeof(struct controller_block_private))))
goto err_block;
block->private->axes = calloc(sizeof(float), nr_axes);
if (!block->private->axes)
......@@ -103,8 +147,7 @@ struct controller_block * block_joystick_create(char *name, va_list ap)
if (!block->private->buttons)
goto err_buttons;
block->type = sim ? "joystick_sim" : "joystick";
block->name = strdup(name);
block->private->device = strdup(device);
block->inputs = 0;
block->input = NULL;
......@@ -131,11 +174,14 @@ struct controller_block * block_joystick_create(char *name, va_list ap)
sprintf(block->output[outnr].name, "button%d", i);
}
if (sim) {
block->calculate = NULL;
} else {
block->calculate = calculate;
}
block->private->bus = controller_bus_create("joystick", name, block, 0);
if (!block->private->bus)
goto err_bus;
controller_bus_poll_set(block->private->bus, joystick_poll, NULL);
block->calculate = calculate;
block->params = 0;
block->param = NULL;
block->param_get = NULL;
......@@ -146,15 +192,16 @@ struct controller_block * block_joystick_create(char *name, va_list ap)
controller_block_add(block);
return block;
err_bus:
err_output:
free(block->private->buttons);
free(block->private->device);
err_buttons:
free(block->private->axes);
err_axes:
free(block->private);
err_private:
free(block);
err_block:
controller_block_free(block);
err_ioctl:
close(fd);
return NULL;
......
......@@ -50,8 +50,13 @@ struct controller_block_private {
static void calculate(struct controller_block *mul)
{
struct controller_block_private *priv = mul->private;
int n = priv->n;
int select = *priv->select;
priv->out = *priv->in[*priv->select];
if (select > n)
select = n;
priv->out = *priv->in[select];
}
......@@ -62,19 +67,11 @@ struct controller_block * block_multiplexer_create(char *name, va_list ap)
n = va_arg(ap, int);
mul = malloc(sizeof(struct controller_block));
if (!mul)
if (!(mul = controller_block_alloc("multiplexer", name,
sizeof(struct controller_block_private))))
return NULL;
mul->type = "multiplexer";
mul->name = malloc(strlen(name)+1);
if (!mul->name)
goto err_name;
strcpy(mul->name, name);
mul->private = malloc(sizeof(struct controller_block_private));
if (!mul->private)
goto err_private;
mul->private->n = n - 1;
mul->private->out = 0.0;
mul->private->in = calloc(sizeof(float*), n);
......@@ -117,14 +114,9 @@ struct controller_block * block_multiplexer_create(char *name, va_list ap)
return mul;
err_output:
free(mul->input);
err_input:
free(mul->private->in);
err_in:
free(mul->private);
err_private:
free(mul->name);
err_name:
free(mul);
controller_block_free(mul);
return NULL;
}
/*
Copyright Jeroen Vreeken (jeroen@vreeken.net), 2014
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
----------------------
| |
---| reset state |----
| |
---| in0 out0 |----
| |
| .. ... ..... |
| |
---| in(n-1) out(n-1) |----
| |
----------------------
*/
struct transition {
uint32_t to;
bool *in;
bool *care;
};
struct state {
uint32_t state;
bool *out;
int transition_nr;
struct transition *transitions;
};
struct controller_block_private {
int no;
int ni;
int cur_state;
int state_nr;
struct state *state_table;
uint32_t state;
bool *reset;
bool **in;
bool *out;
};
static void calculate(struct controller_block *sm)
{
struct controller_block_private *priv = sm->private;
int cur_state = priv->cur_state;
struct state *state = &priv->state_table[cur_state];
int i, j;
int new_state = cur_state;
if (*priv->reset) {
new_state = 0;
} else {
for (i = 0; i < state->transition_nr; i++) {
struct transition *tr = &state->transitions[i];
bool match = true;
for (j = 0; j < priv->ni; j++) {
if (!tr->care[j])
continue;
if (tr->in[j] != *priv->in[j]) {
match = false;
break;
}
}
if (match) {
new_state = tr->to;
break;
}
}
}
if (new_state != cur_state) {
priv->cur_state = new_state;
state = &priv->state_table[new_state];
priv->state = state->state;
for (i = 0; i < priv->no; i++) {
priv->out[i] = state->out[i];
}
log_send(LOG_T_DEBUG, "%s: %d -> %d", sm->name,
priv->state_table[cur_state].state,
priv->state_table[new_state].state);
}
}
struct controller_block_param_list params[] = {
{ "state", true },
{ "transition", true },
{ NULL },
};
/* make sure state exists in table (create with default values if needed) */
static void ensure_state(struct controller_block *sm, uint32_t state)
{
struct controller_block_private *priv = sm->private;
int i;
for (i = 0; i < priv->state_nr; i++) {
if (priv->state_table[i].state == state)
return;
}
priv->state_table = realloc(priv->state_table,
sizeof(struct state) * (priv->state_nr + 1));
memset(priv->state_table + priv->state_nr, 0, sizeof(struct state));
priv->state_table[priv->state_nr].state = state;
priv->state_table[priv->state_nr].out =
calloc(priv->no, sizeof(bool));
priv->state_nr++;
}
static int get_state(struct controller_block *sm, uint32_t state)
{
struct controller_block_private *priv = sm->private;
int i;
for (i = 0; i < priv->state_nr; i++) {
if (priv->state_table[i].state == state)
return i;
}
return 0;
}
static void add_state(struct controller_block *sm, uint32_t state, bool *out)
{
struct controller_block_private *priv = sm->private;
int statenr;
int i;
ensure_state(sm, state);
statenr = get_state(sm, state);
for (i = 0; i < priv->no; i++) {
priv->state_table[statenr].out[i] = out[i];
}
log_send(LOG_T_DEBUG, "%s: new state %d (%d)", sm->name, state, statenr);
}
static void add_transition(struct controller_block *sm,
uint32_t from, uint32_t to, bool *in, bool *care)
{
struct controller_block_private *priv = sm->private;
int statenr, to_statenr;
int nr;
int i;
ensure_state(sm, to);
ensure_state(sm, from);
statenr = get_state(sm, from);
to_statenr = get_state(sm, to);
nr = priv->state_table[statenr].transition_nr;
priv->state_table[statenr].transitions = realloc(
priv->state_table[statenr].transitions,
sizeof(struct transition) * (nr + 1));
priv->state_table[statenr].transitions[nr].in = calloc(priv->ni, sizeof(bool));
priv->state_table[statenr].transitions[nr].care = calloc(priv->ni, sizeof(bool));
priv->state_table[statenr].transitions[nr].to = to_statenr;
for (i = 0; i < priv->ni; i++) {
priv->state_table[statenr].transitions[nr].in[i] = in[i];
priv->state_table[statenr].transitions[nr].care[i] = care[i];
}
priv->state_table[statenr].transition_nr = nr + 1;
log_send(LOG_T_DEBUG, "%s: transition %d: %d -> %d (%d -> %d)",
sm->name, nr, from, to, statenr, to_statenr);
}
static void param_set(struct controller_block *sm, int param, va_list val)
{
struct controller_block_private *priv = sm->private;
int i;
switch (param) {
case 0: {
uint32_t state = va_arg(val, int);
int *outputs = va_arg(val, int *);
bool out[priv->no];
for (i = 0; i < priv->no; i++) {
out[i] = outputs[i];
}
add_state(sm, state, out);
break;
}
case 1: {
uint32_t from = va_arg(val, int);
uint32_t to = va_arg(val, int);
int *inputs = va_arg(val, int *);
int *cares = va_arg(val, int *);
bool in[priv->ni];
bool care[priv->ni];
for (i = 0; i < priv->ni; i++) {
in[i] = inputs[i];
care[i] = cares[i];
}
add_transition(sm, from, to, in, care);
break;
}
}
}
struct controller_block * block_state_machine_create(char *name, va_list ap)
{
struct controller_block *sm;
int ni, no, i;
ni = va_arg(ap, int);
no = va_arg(ap, int);
if (!(sm = controller_block_alloc("state_machine", name,
sizeof(struct controller_block_private))))
return NULL;
sm->private->state = 0;
sm->private->cur_state = 0;
sm->private->ni = ni;
sm->private->no = no;
ensure_state(sm, 0);
sm->private->in = calloc(sizeof(bool*), ni);
if (!sm->private->in)
goto err_in;
sm->private->out = calloc(sizeof(bool), no);
if (!sm->private->out)
goto err_out;
sm->inputs = 1 + ni;
sm->input = malloc(sizeof(struct controller_block_interm)*(ni + 1));
if (!sm->input)
goto err_input;
sm->input[0].name = "reset";
sm->input[0].type = CONTROLLER_BLOCK_TERM_BOOL;
sm->input[0].value.b = &sm->private->reset;
sm->input[0].ghostof = NULL;
for (i = 0; i < ni; i++) {
sm->input[1+i].name = malloc(16);
sprintf(sm->input[1+i].name, "in%d", i);
sm->input[1+i].type = CONTROLLER_BLOCK_TERM_BOOL;
sm->input[1+i].value.b = &sm->private->in[i];
sm->input[1+i].ghostof = NULL;
}
sm->outputs = 1 + no;
sm->output = malloc(sizeof(struct controller_block_outterm) * (no + 1));
if (!sm->output)
goto err_output;
sm->output[0].name = "state";
sm->output[0].type = CONTROLLER_BLOCK_TERM_UINT32;
sm->output[0].value.u32 = &sm->private->state;
sm->output[0].source = sm;
for (i = 0; i < no; i++) {
sm->output[1+i].name = malloc(16);
sprintf(sm->output[1+i].name, "out%d", i);
sm->output[1+i].type = CONTROLLER_BLOCK_TERM_BOOL;
sm->output[1+i].value.b = &sm->private->out[i];
sm->output[1+i].source = sm;
}
sm->calculate = calculate;
if (controller_block_param_list_init(sm, params))
goto err_params;
sm->param_get = NULL;
sm->param_set = param_set;
controller_block_add(sm);
return sm;
err_params:
err_output:
err_input:
free(sm->private->out);
err_out:
free(sm->private->in);
err_in:
controller_block_free(sm);
return NULL;
}
......@@ -55,6 +55,7 @@ struct controller_block_private {
int cur;
float playtime;
bool err;
int nr;
struct block_trajectplayer_param **trajectories;
......@@ -122,8 +123,11 @@ static void calculate(struct controller_block *player)
if (*priv->start) {
int cur = findtraject(priv, *priv->select);
if (cur >= priv->nr) {
log_send(LOG_T_ERROR,
"Illegal select, trajectory not available!");
if (!priv->err)
log_send(LOG_T_ERROR,
"%s: Illegal select, trajectory %d not available!",
player->name, *priv->select);
priv->err = true;
} else {
priv->cur = cur;
traject = priv->trajectories[cur];
......@@ -131,6 +135,7 @@ static void calculate(struct controller_block *player)
priv->done = false;
playtime = 0.0;
priv->position = traject->position[0];
priv->err = false;
}
}
}
......@@ -167,7 +172,7 @@ static void param_set(struct controller_block *player, int param, va_list ap)
if (traject_nr == priv->nr) {
priv->trajectories = realloc(priv->trajectories,
sizeof(struct block_trajectplayer_param *) *
priv->nr);
priv->nr + 1);
priv->trajectories[priv->nr] = calloc(
sizeof(struct block_trajectplayer_param), 1);
priv->nr++;
......@@ -196,24 +201,28 @@ static void param_set(struct controller_block *player, int param, va_list ap)
break;
}
}
static struct controller_block_interm_list interms[] = {
{ "start", CONTROLLER_BLOCK_TERM_BOOL, offsetof(struct controller_block_private, start) },