130 lines
2.8 KiB
C
130 lines
2.8 KiB
C
#include <rtai_mbx.h>
|
|
#include <rtai_sched.h>
|
|
|
|
#include <uint128.h>
|
|
#include <sys/rtai_modbus.h>
|
|
|
|
#define DEBUG 1
|
|
|
|
#define SENSOR_PART_TURNTABLE 1<<0
|
|
#define SENSOR_PART_DRILL 1<<1
|
|
#define SENSOR_PART_MESURING 1<<2
|
|
#define SENSOR_DRILL_UP 1<<3
|
|
#define SENSOR_DRILL_DOWN 1<<4
|
|
#define SENSOR_TURNTABLE_POS 1<<5
|
|
#define SENSOR_PART_TEST 1<<6
|
|
|
|
#define ACTOR_DRILL 1<<0
|
|
#define ACTOR_TURNTABLE 1<<1
|
|
#define ACTOR_DRILL_DOWN 1<<2
|
|
#define ACTOR_DRILL_UP 1<<3
|
|
#define ACTOR_PART_HOLD 1<<4
|
|
#define ACTOR_TESTER 1<<5
|
|
#define ACTOR_EXIT 1<<6
|
|
#define ACTOR_ENTRANCE 1<<7
|
|
|
|
MODULE_LICENSE("GPL");
|
|
|
|
static RT_TASK entry_task;
|
|
|
|
int readAll(int connection, int *value) {
|
|
if (rt_modbus_get(fd_node, DIGITAL_IN, 0, (unsigned short *) &value)) {
|
|
return 1;
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
int read(int connection, int part, int *result) {
|
|
int value;
|
|
int code = readAll(connection, &value);
|
|
*result = (value & part) != 0;
|
|
return code;
|
|
}
|
|
|
|
int disable(int connection, int part) {
|
|
int value;
|
|
int code = readAll(connection, &value);
|
|
if (code) return code;
|
|
if (rt_modbus_set(connection, DIGITAL_OUT, output &= part))
|
|
return 1;
|
|
return 0;
|
|
}
|
|
|
|
int enable(int connection, int part) {
|
|
int value;
|
|
int code = readAll(connection, &value);
|
|
if (code) return code;
|
|
if (rt_modbus_set(connection, DIGITAL_OUT, output |= part))
|
|
return 1;
|
|
return 0;
|
|
}
|
|
|
|
static void
|
|
turntable(long x) {
|
|
int connection;
|
|
int ready = 0;
|
|
|
|
rt_printk("turntable: task started\n");
|
|
|
|
if ((connection = rt_modbus_connect("modbus-node")) == -1) {
|
|
rt_printk("turntable: task exited\n");
|
|
return;
|
|
}
|
|
rt_printk("turntable: MODBUS communication opened\n");
|
|
|
|
while (1) {
|
|
int partOnTable;
|
|
if (read(connection, SENSOR_PART_TURNTABLE, &partOnTable)) goto fail;
|
|
|
|
if (partOnTable) {
|
|
rt_printk("halloo welt");
|
|
if (enable(connection, ACTOR_TURNTABLE)) goto fail;
|
|
|
|
} else {
|
|
if (disable(connection, ACTOR_TURNTABLE)) goto fail;
|
|
}
|
|
|
|
if (ready)
|
|
goto fail;
|
|
|
|
rt_sleep(1000 * nano2count(1000000));
|
|
}
|
|
|
|
/* Aufraeumen */
|
|
fail:
|
|
rt_modbus_disconnect(connection);
|
|
rt_printk("turntable: task exited with failure\n");
|
|
}
|
|
|
|
static void __exit
|
|
|
|
example_exit(void) {
|
|
rt_task_delete(&entry_task);
|
|
stop_rt_timer();
|
|
printk("module Bearbeiten2 unloaded\n");
|
|
}
|
|
|
|
static int __init
|
|
|
|
example_init(void) {
|
|
rt_set_oneshot_mode();
|
|
start_rt_timer(0);
|
|
modbus_init();
|
|
|
|
if (rt_task_init(&entry_task, entry, 0, 1024, 0, 0, NULL)) {
|
|
printk("cannot initialize control task\n");
|
|
goto fail;
|
|
}
|
|
|
|
rt_task_resume(&entry_task);
|
|
printk("loaded module Bearbeiten2\n");
|
|
return (0);
|
|
|
|
fail:
|
|
stop_rt_timer();
|
|
return (1);
|
|
}
|
|
|
|
module_exit(example_exit)
|
|
module_init(example_init)
|