Praktikum: abgabe
This commit is contained in:
parent
7f23e3f994
commit
001628f14d
166
Bearbeiten2.c
166
Bearbeiten2.c
|
|
@ -1,15 +1,14 @@
|
|||
/*#include <rtai_mbx.h>
|
||||
#include <rtai_mbx.h>
|
||||
#include <rtai_sched.h>
|
||||
#include <rtai_sem.h>
|
||||
#include <sys/rtai_modbus.h>*/
|
||||
#include <sys/rtai_modbus.h>
|
||||
|
||||
#include "include/rtai_mbx.h"
|
||||
#include "include/rtai_sched.h"
|
||||
#include "include/rtai_sem.h"
|
||||
#include "include/rtai_modbus.h"
|
||||
#include <stdbool.h>
|
||||
|
||||
//#include <uint128.h>
|
||||
//======================
|
||||
//Author: Charlotte Friedemann, Johannes Theiner
|
||||
//Description: Praktikum EZDV Gruppe A5(Bearbeiten 2)
|
||||
//Created: 19.10.2020
|
||||
//Finished: 30.11.2020
|
||||
//======================
|
||||
|
||||
#define SENSOR_PART_TURNTABLE 1<<0
|
||||
#define SENSOR_PART_DRILL 1<<1
|
||||
|
|
@ -39,6 +38,10 @@ int connection;
|
|||
|
||||
char node[] = "modbus-node";
|
||||
|
||||
/**
|
||||
* deinitialize all tasks, mailboxes, modbus connections & semaphores
|
||||
* @param fail should a error message be printed.
|
||||
*/
|
||||
void end(bool fail) {
|
||||
rt_modbus_disconnect(connection);
|
||||
rt_task_delete(&turntable_task);
|
||||
|
|
@ -59,7 +62,7 @@ void end(bool fail) {
|
|||
stop_rt_timer();
|
||||
|
||||
if(fail) {
|
||||
rt_printk("failed to read/send Modbus message.\n");
|
||||
rt_printk("an error has occurred.\n");
|
||||
rt_printk("module needs to be restarted.\n");
|
||||
}
|
||||
}
|
||||
|
|
@ -77,14 +80,15 @@ int readAll(int type, int *result) {
|
|||
}
|
||||
|
||||
/**
|
||||
*
|
||||
* read a single bit for the specified part
|
||||
* this function is thread safe.
|
||||
* @param part
|
||||
* @return
|
||||
* @param part bitmask to read the sensor/actor
|
||||
* @return read bit.
|
||||
*/
|
||||
int readData(int part) {
|
||||
rt_sem_wait(&semaphore);
|
||||
int value = 0;
|
||||
rt_sem_wait(&semaphore);
|
||||
//the semaphore needs to be locked before this code executes, so we can't adhere to the ISO standard here.
|
||||
int code = readAll(DIGITAL_IN, &value);
|
||||
int result = (part & value);
|
||||
rt_sem_signal(&semaphore);
|
||||
|
|
@ -98,13 +102,14 @@ int readData(int part) {
|
|||
* @param actor bitmask of the actor to disable
|
||||
*/
|
||||
void disable(int actor) {
|
||||
rt_sem_wait(&semaphore);
|
||||
int value = 0;
|
||||
int code = readAll(DIGITAL_OUT, &value);
|
||||
if(code) end(true);
|
||||
rt_sem_wait(&semaphore);
|
||||
if(readAll(DIGITAL_OUT, &value)) end(true);
|
||||
else {
|
||||
int result = rt_modbus_set(connection, DIGITAL_OUT, 0, value &= ~actor);
|
||||
rt_sem_signal(&semaphore);
|
||||
if (result) end(true);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
@ -113,26 +118,41 @@ void disable(int actor) {
|
|||
* @param actor bitmask of the actor to enable
|
||||
*/
|
||||
void enable(int actor) {
|
||||
rt_sem_wait(&semaphore);
|
||||
int output = 0;
|
||||
int code = readAll(DIGITAL_OUT, &output);
|
||||
if (code) end(true);
|
||||
int result;
|
||||
result = rt_modbus_set(connection, DIGITAL_OUT, 0, output |= actor);
|
||||
rt_sem_wait(&semaphore);
|
||||
if (readAll(DIGITAL_OUT, &output)) end(true);
|
||||
else {
|
||||
int result = rt_modbus_set(connection, DIGITAL_OUT, 0, output |= actor);
|
||||
rt_sem_signal(&semaphore);
|
||||
if(result) end(true);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
/**
|
||||
* send a blocking message to the specified mailbox
|
||||
* @param mailbox where should the message be send to ?
|
||||
* @param msg message to send
|
||||
*/
|
||||
void sendMail(MBX * mailbox, int msg) {
|
||||
int mbxStatus = rt_mbx_send(mailbox, &msg, sizeof(int));
|
||||
if(mbxStatus == EINVAL) end(true);
|
||||
}
|
||||
|
||||
/**
|
||||
* send a non blocking message to the specified mailbox
|
||||
* @param mailbox where should the message be send to ?
|
||||
* @param msg message to send
|
||||
*/
|
||||
void sendMailNonBlocking(MBX * mailbox, int msg) {
|
||||
int mbxStatus = rt_mbx_send_if(mailbox, &msg, sizeof(int));
|
||||
if(mbxStatus == EINVAL) end(true);
|
||||
}
|
||||
|
||||
/**
|
||||
* receive a blocking message
|
||||
* @param mailbox mailbox to receive the message
|
||||
*/
|
||||
int receiveMail(MBX * mailbox) {
|
||||
int msg = 0;
|
||||
int mbxStatus = rt_mbx_receive(mailbox, &msg, sizeof(int));
|
||||
|
|
@ -140,6 +160,10 @@ int receiveMail(MBX * mailbox) {
|
|||
return msg;
|
||||
}
|
||||
|
||||
/**
|
||||
* receive a non blocking message
|
||||
* @param mailbox mailbox to receive the message
|
||||
*/
|
||||
int receiveMailNonBlocking(MBX * mailbox) {
|
||||
int msg = 0;
|
||||
int mbxStatus = rt_mbx_receive_if(mailbox, &msg, sizeof(int));
|
||||
|
|
@ -147,48 +171,81 @@ int receiveMailNonBlocking(MBX * mailbox) {
|
|||
return msg;
|
||||
}
|
||||
|
||||
/**
|
||||
* sleep for x milliseconds
|
||||
* @param ms specified time to sleep in milliseconds.
|
||||
*/
|
||||
void sleepMs(int ms) {
|
||||
rt_sleep(ms * nano2count(1000000));
|
||||
}
|
||||
|
||||
/**
|
||||
* turntable task
|
||||
*/
|
||||
static void turntable(long data) {
|
||||
int times = 0;
|
||||
rt_printk("started turntable task\n");
|
||||
|
||||
//reset everything first
|
||||
disable(ACTOR_TURNTABLE);
|
||||
disable(ACTOR_ENTRANCE);
|
||||
|
||||
while (1) {
|
||||
receiveMail(&turntable_status_mbx);
|
||||
receiveMail(&turntable_status_mbx);
|
||||
receiveMail(&turntable_status_mbx);
|
||||
int partOnTable = readData(SENSOR_PART_TURNTABLE);
|
||||
int msg1 = receiveMailNonBlocking(&turntable_data_mbx);
|
||||
int msg2 = receiveMailNonBlocking(&turntable_data_mbx);
|
||||
if(partOnTable || msg1 || msg2) {
|
||||
//throw out all parts that might be on the table.
|
||||
do {
|
||||
enable(ACTOR_TURNTABLE);
|
||||
sleepMs(1000);
|
||||
disable(ACTOR_TURNTABLE);
|
||||
sleepMs(1000);
|
||||
sleepMs(500);
|
||||
enable(ACTOR_EXIT);
|
||||
sleepMs(500);
|
||||
disable(ACTOR_EXIT);
|
||||
times++;
|
||||
}while (times < 5);
|
||||
|
||||
//start processing
|
||||
while (1) {
|
||||
//receive status mail from: tester, drill & output
|
||||
receiveMail(&turntable_status_mbx);
|
||||
receiveMail(&turntable_status_mbx);
|
||||
receiveMail(&turntable_status_mbx);
|
||||
//always read from mailbox to make sure that no overflow occurs.
|
||||
int msg1 = receiveMailNonBlocking(&turntable_data_mbx);
|
||||
int msg2 = receiveMailNonBlocking(&turntable_data_mbx);
|
||||
if(readData(SENSOR_PART_TURNTABLE) || msg1 || msg2) {//if a part is on any of the sensors
|
||||
enable(ACTOR_TURNTABLE);
|
||||
sleepMs(100);
|
||||
if (!readData(SENSOR_TURNTABLE_POS)) {
|
||||
do {
|
||||
sleepMs(10);
|
||||
} while (readData(SENSOR_TURNTABLE_POS) == 0);
|
||||
}
|
||||
disable(ACTOR_TURNTABLE);
|
||||
sleepMs(500);
|
||||
}
|
||||
//send status mails
|
||||
sendMail(&tester_status_mbx, 1);
|
||||
sendMail(&drill_status_mbx, 1);
|
||||
sendMail(&output_status_mbx, 1);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* tester task
|
||||
*/
|
||||
static void tester(long data) {
|
||||
rt_printk("started tester task\n");
|
||||
|
||||
//reset everything first
|
||||
disable(ACTOR_TESTER);
|
||||
sendMail(&turntable_status_mbx, 1);
|
||||
//start processing
|
||||
while (1) {
|
||||
receiveMail(&tester_status_mbx);
|
||||
int partOnTable = readData(SENSOR_PART_TESTER);
|
||||
if(partOnTable) {
|
||||
sendMailNonBlocking(&turntable_data_mbx, 1);
|
||||
if(readData(SENSOR_PART_TESTER)) {
|
||||
sendMailNonBlocking(&turntable_data_mbx, 1);//turntable should turn on the next turn.
|
||||
enable(ACTOR_TESTER);
|
||||
sleepMs(1000);
|
||||
int partNormal = readData(SENSOR_PART_TEST);
|
||||
if(partNormal) {
|
||||
sleepMs(500);
|
||||
if(readData(SENSOR_PART_TEST)) {//should the drill be active on the next turn ?
|
||||
sendMailNonBlocking(&drill_data_mbx, 1);
|
||||
}else {
|
||||
sendMailNonBlocking(&drill_data_mbx, 0);
|
||||
|
|
@ -199,7 +256,12 @@ static void tester(long data) {
|
|||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* drill task
|
||||
*/
|
||||
static void drill(long data) {
|
||||
rt_printk("started drill task\n");
|
||||
//reset everything first
|
||||
disable(ACTOR_DRILL);
|
||||
disable(ACTOR_PART_HOLD);
|
||||
disable(ACTOR_DRILL_DOWN);
|
||||
|
|
@ -207,24 +269,23 @@ static void drill(long data) {
|
|||
enable(ACTOR_DRILL_UP);
|
||||
if (!readData(SENSOR_DRILL_UP)) {
|
||||
do {
|
||||
sleepMs(10);
|
||||
} while (readData(SENSOR_DRILL_UP) == 0);
|
||||
}
|
||||
disable(ACTOR_DRILL_UP);
|
||||
sendMail(&turntable_status_mbx, 1);
|
||||
|
||||
//start processing
|
||||
while (1) {
|
||||
receiveMail(&drill_status_mbx);
|
||||
int partOnTable = readData(SENSOR_PART_DRILL);
|
||||
int msg = receiveMailNonBlocking(&drill_data_mbx);
|
||||
if(partOnTable) {
|
||||
sendMailNonBlocking(&turntable_data_mbx, 1);
|
||||
int msg = receiveMailNonBlocking(&drill_data_mbx);
|
||||
if(msg) {
|
||||
if(readData(SENSOR_PART_DRILL)) {//if part is in drill
|
||||
sendMailNonBlocking(&turntable_data_mbx, 1);//turntable should be active on the next turn.
|
||||
if(receiveMailNonBlocking(&drill_data_mbx)) {//read test result from mailbox
|
||||
enable(ACTOR_PART_HOLD);
|
||||
enable(ACTOR_DRILL);
|
||||
enable(ACTOR_DRILL_DOWN);
|
||||
if (!readData(SENSOR_DRILL_DOWN)) {
|
||||
do {
|
||||
sleepMs(10);
|
||||
} while (readData(SENSOR_DRILL_DOWN) == 0);
|
||||
}
|
||||
sleepMs(1000);
|
||||
|
|
@ -233,29 +294,32 @@ static void drill(long data) {
|
|||
enable(ACTOR_DRILL_UP);
|
||||
if (!readData(SENSOR_DRILL_UP)) {
|
||||
do {
|
||||
sleepMs(10);
|
||||
} while (readData(SENSOR_DRILL_UP) == 0);
|
||||
}
|
||||
disable(ACTOR_DRILL_UP);
|
||||
disable(ACTOR_PART_HOLD);
|
||||
}
|
||||
sendMailNonBlocking(&output_data_mbx, 1);
|
||||
sendMailNonBlocking(&output_data_mbx, 1);//should the output be active on the next turn ?
|
||||
}
|
||||
|
||||
|
||||
sendMail(&turntable_status_mbx, 1);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* output task
|
||||
*/
|
||||
static void output(long data) {
|
||||
rt_printk("started output task\n");
|
||||
//reset everything first
|
||||
disable(ACTOR_EXIT);
|
||||
sendMail(&turntable_status_mbx, 1);
|
||||
//start processing
|
||||
while (1) {
|
||||
receiveMail(&output_status_mbx);
|
||||
int msg = receiveMailNonBlocking(&output_data_mbx);
|
||||
|
||||
if(msg) {
|
||||
if(receiveMailNonBlocking(&output_data_mbx)) {//should the output be activated
|
||||
enable(ACTOR_EXIT);
|
||||
sleepMs(1000);
|
||||
sleepMs(500);
|
||||
disable(ACTOR_EXIT);
|
||||
}
|
||||
sendMail(&turntable_status_mbx, 1);
|
||||
|
|
|
|||
Loading…
Reference in New Issue