block_limit_switch.c 2.9 KB
Newer Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
/*
	Copyright Jeroen Vreeken (jeroen@vreeken.net), 2015

	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 |----
     |	                  |
  ---| 1  enable_pos      |
     |	                  |
  ---| 2  enable_neg      |
     |	                  |
38
39
  ---| 3  enable          |
     |	                  |
40
41
42
43
44
45
46
47
     ----------------------
     
 */

struct controller_block_private {
	float *in;
	bool *enable_pos;
	bool *enable_neg;
48
	bool *enable;
49
50
51
52
53
54
55
56
57
58
	float out;
};

static void limit_switch_calculate(struct controller_block *limit)
{
	struct controller_block_private *priv = limit->private;
	float out;
	float in = *priv->in;
	bool pos = *priv->enable_pos;
	bool neg = *priv->enable_neg;
59
	bool en = *priv->enable;
60
61

	out = 0.0;
62
63
64
65
66

	en |= (in > 0.0) & pos;
	en |= (in < 0.0) & neg;

	if (en)
67
68
69
70
71
72
73
74
75
		out = in;

	priv->out = out;
}

static struct controller_block_interm_list interms[] = {
	{ "in", CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, in) },
	{ "enable_pos", CONTROLLER_BLOCK_TERM_BOOL, offsetof(struct controller_block_private, enable_pos) },
	{ "enable_neg", CONTROLLER_BLOCK_TERM_BOOL, offsetof(struct controller_block_private, enable_neg) },
76
	{ "enable", CONTROLLER_BLOCK_TERM_BOOL, offsetof(struct controller_block_private, enable) },
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
	{ NULL }
};

static struct controller_block_outterm_list outterms[] = {
	{ "out", CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, out) },
	{ NULL }
};


static struct controller_block * block_limit_switch_create(char *name, int argc, va_list val)
{
	struct controller_block *limit;
	
	if (!(limit = controller_block_alloc("limit_switch", name, sizeof(struct controller_block_private))))
		return NULL;

	limit->private->out = 0.0;

	if (controller_block_interm_list_init(limit, interms))
		goto err_block;

	if (controller_block_outterm_list_init(limit, outterms))
		goto err_block;
	
	limit->calculate = limit_switch_calculate;

	if (controller_block_add(limit))
		goto err_block;

	return limit;

err_block:
	controller_block_free(limit);
	return NULL;
}

BLOCK_CREATE(limit_switch) = {
	.create = block_limit_switch_create,
	.args = { NULL },
};