parent
05ce7df364
commit
810311fabb
|
@ -3,6 +3,7 @@
|
|||
|
||||
#include <uint128.h>
|
||||
#include <sys/rtai_modbus.h>
|
||||
#include <stdbool.h>
|
||||
#include "include/rtai_lxrt.h"
|
||||
#include "include/rtai_modbus.h"
|
||||
#include "include/rtai_sem.h"
|
||||
|
@ -35,7 +36,7 @@ int connection;
|
|||
|
||||
char node[] = "modbus-node";
|
||||
|
||||
void fail() {
|
||||
void end(bool fail) {
|
||||
rt_modbus_disconnect(connection);
|
||||
rt_task_delete(&turntable_task);
|
||||
rt_task_delete(&tester_task);
|
||||
|
@ -48,8 +49,10 @@ void fail() {
|
|||
|
||||
stop_rt_timer();
|
||||
|
||||
rt_printk("failed to read/send Modbus message.\n");
|
||||
rt_printk("module needs to be restarted.\n");
|
||||
if(fail) {
|
||||
rt_printk("failed to read/send Modbus message.\n");
|
||||
rt_printk("module needs to be restarted.\n");
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
|
@ -72,13 +75,15 @@ int readAll(int type, int *result) {
|
|||
* @param result value to write the result to
|
||||
* @return status code(0: success, 1: failure)
|
||||
*/
|
||||
int readData(int part, int *result) {
|
||||
int readData(int part) {
|
||||
rt_sem_wait(&semaphore);
|
||||
int value;
|
||||
int * result;
|
||||
int code = readAll(DIGITAL_IN, &value);
|
||||
*result = (value & part) != 0;
|
||||
rt_sem_signal(&semaphore);
|
||||
return code;
|
||||
if(code) end(true);
|
||||
return *result;
|
||||
}
|
||||
|
||||
/**
|
||||
|
@ -91,10 +96,10 @@ void disable(int actor) {
|
|||
rt_sem_wait(&semaphore);
|
||||
int value;
|
||||
int code = readAll(DIGITAL_OUT, &value);
|
||||
if(code) fail();
|
||||
if(code) end(true);
|
||||
int result = rt_modbus_set(connection, DIGITAL_OUT, value &= ~actor)
|
||||
rt_sem_signal(&semaphore);
|
||||
if (result) fail();
|
||||
if (result) end(true);
|
||||
}
|
||||
|
||||
/**
|
||||
|
@ -107,10 +112,10 @@ void enable(int actor) {
|
|||
rt_sem_wait(&semaphore);
|
||||
int output;
|
||||
int code = readAll(DIGITAL_OUT, &output);
|
||||
if (code) fail();
|
||||
if (code) end(true);
|
||||
int result = rt_modbus_set(connection, DIGITAL_OUT, output |= actor)
|
||||
rt_sem_signal(&semaphore);
|
||||
if(result) fail();
|
||||
if(result) end(true);
|
||||
}
|
||||
|
||||
void sleepStuff() {
|
||||
|
@ -123,8 +128,7 @@ static void turntable(long x) {
|
|||
disable(ACTOR_TURNTABLE);
|
||||
|
||||
while (1) {
|
||||
int partOnTable;
|
||||
readData(SENSOR_PART_TURNTABLE, &partOnTable);
|
||||
int partOnTable = readData(SENSOR_PART_TURNTABLE);
|
||||
if (partOnTable) {
|
||||
enable(ACTOR_TURNTABLE);
|
||||
|
||||
|
@ -148,12 +152,10 @@ static void drill(long x) {
|
|||
|
||||
enable(ACTOR_DRILL_UP);
|
||||
rt_sem_wait(&semaphore);
|
||||
int drill_up = 0;
|
||||
readData(SENSOR_DRILL_UP, drill_up);
|
||||
if (!drill_up) {
|
||||
|
||||
if (!readData(SENSOR_DRILL_UP)) {
|
||||
do {
|
||||
readData(SENSOR_DRILL_UP, drill_up);
|
||||
} while (drill_up == 0);
|
||||
} while (readData(SENSOR_DRILL_UP) == 0);
|
||||
}
|
||||
rt_sem_signal(&semaphore);
|
||||
disable(ACTOR_DRILL_UP);
|
||||
|
@ -185,7 +187,6 @@ example_init(void) {
|
|||
start_rt_timer(0);
|
||||
modbus_init();
|
||||
|
||||
|
||||
rt_printk("init: task started\n");
|
||||
if ((connection = rt_modbus_connect(node)) == -1) {
|
||||
rt_printk("init: could not connect to %s\n", node);
|
||||
|
@ -216,18 +217,13 @@ example_init(void) {
|
|||
return (0);
|
||||
|
||||
fail:
|
||||
stop_rt_timer();
|
||||
end(true);
|
||||
return (1);
|
||||
}
|
||||
|
||||
static void __exit
|
||||
example_exit(void) {
|
||||
rt_task_delete(&turntable_task);
|
||||
rt_task_delete(&drill_task);
|
||||
rt_task_delete(&tester_task);
|
||||
rt_task_delete(&output_task);
|
||||
stop_rt_timer();
|
||||
rt_sem_delete(&semaphore);
|
||||
end(false);
|
||||
printk("module Bearbeiten2 unloaded\n");
|
||||
}
|
||||
|
||||
|
|
19
diagram.puml
19
diagram.puml
|
@ -25,9 +25,9 @@ while(Dauerschleife) is (true)
|
|||
if(Werkstück vorhanden ?) then (true)
|
||||
:Prüfer ausfahren;
|
||||
if(Werkstück Normallage ?) then (true)
|
||||
:Bohrer(on)>
|
||||
:Sende Bohrer(on)>
|
||||
else (false)
|
||||
:Bohrer(off)>
|
||||
:Sende Bohrer(off)>
|
||||
endif
|
||||
:Prüfer einfahren;
|
||||
else (false)
|
||||
|
@ -46,18 +46,17 @@ start
|
|||
while(Dauerschleife) is (true)
|
||||
|
||||
if(Werkstück vorhanden?) then(true)
|
||||
:Bohrer<
|
||||
:Auswerfer>
|
||||
if(Teil in Normallage) then(true)
|
||||
:Empfange Lage des Werkstücks<
|
||||
:Sende Auswerfer>
|
||||
if(Teil in Normallage?) then(true)
|
||||
:Werkstück festhalten;
|
||||
:Bohrer anschalten;
|
||||
:Bohrer herunterfahren;
|
||||
:sleep 500ms;
|
||||
:Warte bis Bohrer unten;
|
||||
:Bohrer hochfahren;
|
||||
|
||||
|
||||
:Bohrer ausschalten;
|
||||
:Werkstück loslassen;
|
||||
:Warte bis Bohrer oben;
|
||||
:Bohrer ausschalten;
|
||||
:Werkstück loslassen;
|
||||
else(false)
|
||||
endif
|
||||
|
||||
|
|
Loading…
Reference in New Issue