The build system currently in use to create distributions for the RaMstix appears to have the op- tion to create an Application Development Kit (ADT)1. This plug-in can be used in conjunction with Eclipse and supposedly makes it straightforward to build, run, and debug applications for the target platform (RaMstix in this case).
65
P Production Cell CSP Models
This chapter lists the TERRA CSP models used in the embedded control software for the Pro- duction Cell. There are two top-level models, one for each RaMstix board (Figure P.1a & Fig- ure P.1b).
All modules, except for the sorter, use the same Module TERRA CSP model. This means the code is re-used among these six modules. Specific configuration for any robot is done by sup- plying a single ID value as input, which is used during initialization to apply custom settings in the c++ code. This shared module consists of one top-level module (Figure P.2), which is in- cluded a total of 6 times in the aforementioned architecture models. In this top-level module, the loop and sequence controller are placed in parallel, but execute asynchronously due to the use of non-blocking buffered channels.
The loop controllers of all modules on single RaMstix share a timer for deterministic order of execution, thus reducing jitter on the output of each loop controller. The loop controllers con- tain three processes: a motion profile generator, 20-sim exported loop controller, and a safety block (Figure P.3).
The Motion profile block contains a c++ block which invokes a custom library to compute second-order motion profile setpoints (Figure P.4). This profile is calculated and stored as a 1D-array once upon initialization, based on configuration liketime to moveparameters. This block is commanded by the Sequence Controller through a direction variable. For any direction commanded (forward or backward), the profile algorithm checks whether the profile has been completed in this direction or not. If that is not the case, the profiles’ currently active index is either increased or decreased, depending on the direction.
The PCLoopController block is exported directly from 20-sim. Any specific configuration set- tings in the 20-sim models, like the PID parameters, have been exposed as ports and connected to the Sequence Controller. This means the Sequence Controller is able to change the values on the fly if required.
The SafetyIO block acts as the safety layer in the system. It verifies the input and output values. Also, it scales the input encoder values based on results from the calibration procedure. Addi- tionally, the Sequence Controller is connected through a channel to the safety block and is able to block all output if required. This means that all actuators can be stopped directly from for example the GUI.
(a)Architecture model for RaMstix #1
(b)Architecture model for RaMstix #2
Figure P.1
APPENDIX P. PRODUCTION CELL CSP MODELS 67
Figure P.3:Loop Controller TERRA CSP model
APPENDIX P. PRODUCTION CELL CSP MODELS 69
Figure P.6:Sequence Controller TERRA CSP model
Q Production Cell Finite State Machine Models
This chapter discusses the implementation of the Finite State Machines as have been imple- mented in the ECS for the Production Cell.
The top level behaviour of each module is the same. As such, all similar functionality is grouped in a single parent class (SequenceController). Next, four different homing methods are re- quired: The feeder robot calibrates using a single limit switch (HomingFeeder). The rotation and extraction robot use two limit switches to calibrate (HomingDouble). The molder requires a different calibration as well (HomingMolder). It also uses only one opto-switch, but this switch is used to denote start and endpoint. Finally, both belts are velocity controller and do not require any form of position calibration (HomingNone).
Each module, except for the two belts, have different behaviours implemented using the Finite State Machines. This results in a total of 5 separate classes. However, most top level behaviour is shared where possible.
For state transitions, the Finite State Machines are designed to use asynchronous events. This approach has been chosen to minimize the jitter in the hard real-time loops.
The SequenceController class requires a ConfigReader class. In the first state of the top-level FSM for the SequenceController, a state ’configuring’ uses an instance of the ConfigReader class to load a .i ni configuration file from disk. This file allows configuration of PID settings, motion profile parameters, etc. The configuration for single module is as follows:
[ RotationRobot ]
meq = 0.000017547 ; Module equivalent mass
cpt = 2000 ; Encoder counts per motor turn
ttm = 0 . 2 ; Time to move
mptype = 3 ; Motion p r o f i l e type
c t r l t y p e = 2 ; C o n t r o l l e r type
kp = 0.013 ; Manual PID kp
taud = 0.045 ; Manual PID taud
beta = 0 . 1 ; Manual PID beta
t a u i = 25000000.0 ; Manual PID t a u i
wc = 0.05 ; A n a l y t i c a l PID crossover frequency
Multiple module definitions are allowed in a single configuration file. Configuration for a mod- ule starts with the [<mod ul ename>] line. What follows is a simple listing of<v ar name>=
APPENDIX Q. PRODUCTION CELL FINITE STATE MACHINE MODELS 71
Figure Q.1:Object Model Diagram of FSM implementation.
Figure Q.4:Feeder Homing State (HomingDouble)
Figure Q.5:Molder Homing State (HomingMolder)
APPENDIX Q. PRODUCTION CELL FINITE STATE MACHINE MODELS 73
Figure Q.7:Feeder Active State (FeederSeqCtrl)
Figure Q.9:Extraction Robot Active State (ExtractionRobotSeqCtrl)
APPENDIX Q. PRODUCTION CELL FINITE STATE MACHINE MODELS 75
Figure Q.11:Belt Active State (BeltSeqCtrl)
R RT-CAN
This guide shows how to configure the RaMstix to use the virtual HRT CAN driver. First, remove existing can drivers:
$ sudo rmmod mcp251x $ sudo rmmod can_dev
Now, load the xenomai rt can driver:
$ sudo modprobe xeno_can_flexcan
Additionally, you can insert the virtual can driver:
$ sudo modprobe xeno_can_virt
Finally, you have to configure the CAN interface
$ cd /usr/xenomai/sbin/
$ sudo ./rtcanconfig rtcan0 --baudrate=125000 start $ sudo ./rtcanconfig rtcan1 --baudrate=125000 start
To check the kernel message output
$ dmesg | tail -n 10
should look something like:
[ 101.070573] RT-Socket-CAN 0.90.2 - (C) 2006 RT-Socket-CAN [...] [ 171.906344] rtcan: registered rtcan0
[ 171.910209] rtcan0: VIRT driver loaded
[ 171.914175] rtcan: registered rtcan1
[ 171.917901] rtcan1: VIRT driver loaded
Testing
Now, you can test the can bus by using the rtcansend and rtcanrecv modules in the/usr/xeno-
mai/binfolder.
Open two terminals, in one:
$ sudo ./rtcanrecv
In the other:
$ sudo ./rtcansend rtcan0 -v -i 0x0 0x82 0x3
Common errors
send: Operation not permitted
77
S 20-sim co-simulation plugin
S.1 Source code
The source code of the 20-sim co-simulation plug-in. When building a Windows DLL using this code, make sure to include the header files oflibzmq1and link againstlibzmq.
The DLL entry file:
// dllmain.cpp : Defines the entry point for the DLL application.
#include "stdafx.h"
BOOL APIENTRY DllMain( HMODULE hModule,
DWORD ul_reason_for_call, LPVOID lpReserved ) { switch (ul_reason_for_call) { case DLL_PROCESS_ATTACH: case DLL_THREAD_ATTACH: case DLL_THREAD_DETACH: case DLL_PROCESS_DETACH: break; } return TRUE; }
The plug-in implementation:
// 20SimCoSimGazebo_in.cpp : Defines the exported functions for the DLL application. // #include "stdafx.h" #include <windows.h> #include <fstream> #include <stdio.h> #include <sstream> #include <string> #include <vector> #include <zmq.h>
#include <iomanip> // setprecision
#define DLLEXPORT __declspec( dllexport )
using namespace std;
ofstream outputStream;
#ifdef _MSC_VER
#define snprintf _snprintf #endif // ZeroMq Variables void *context; void *zmqsocket; extern "C" {
for transfer to 20-sim */
const char* g_modelpath;
unsigned int port = 5555;
const unsigned int bufferSize = 10240;
/**
* This is an example of a function that can be called in your 20-sim * model using the dll() Sidops call
*
* @param inarr This double array contains all inputs that will be
* send from 20-sim to the other side.
* FIRST VALUE IS PORT NUMBER
* @param inputs 20-sim tells the dll how many elements inarr[]
* contains.
* @param outarr Result from this dll function that will be returned
* to 20-sim
* @param outputs 20-sim tells the dll how many elements it expects
* (and allocated) in outarr.
* @param major 1=major integration step, 0=minor step (e.g. an
* intermediate integration method step in Runge Kutta
* 4)
* @return int 0 = success, 1 = error
**/
DLLEXPORT int simStep(double *inarr, int inputs, double *outarr,
int outputs, int major) {
// Steps:
// 1. Send current state
// 2. await response (computation on controller side) // 3. set output of controller in outarr
// Send current state /*
* Handle outgoing */
// Place all input values of this block into a string. // Comma separated.
std::stringstream ss;
for (int i = 0; i < inputs; i++) {
ss << std::fixed << std::setprecision(20) << inarr[i];
if (i != inputs) { ss << ","; }
}
// Convert c++ string to a char array
std::string reply_str = ss.str();
char tab2[bufferSize];
strncpy(tab2, reply_str.c_str(), sizeof(tab2)); tab2[sizeof(tab2) - 1] = 0;
// Send it
outputStream << "[ZMQ] Sending: " << reply_str << "\n"; zmq_send(zmqsocket, tab2, sizeof(tab2) - 1, 0);
/*
* Handle incoming */
// Receive message, place it into a cpp string.
char buffer[bufferSize];
APPENDIX S. 20-SIM CO-SIMULATION PLUGIN 79
if (size == -1) {
outputStream << "[ZMQ] Error: Receive timeout" << std::endl; snprintf(g_lasterrormessage, 255,
"%s: timeout receiving message.", __FUNCTION__);
return 1; } if (size > bufferSize - 1) size = bufferSize - 1; buffer[size] = 0; std::string msg(buffer); outputStream << "[ZMQ] Received " << msg << "\n"; if (msg.compare("STOP") == 0) {
outputStream << "[ZMQ] Received STOP" << std::endl; snprintf(g_lasterrormessage, 255,
"%s: simulation stop requested.", __FUNCTION__);
return 1; }
// Loop string, extract comma separated (double) values.
std::vector<double> vect; std::stringstream ss_in(msg); double j; while (ss_in >> j) { vect.push_back(j); if (ss_in.peek() == ',' || ss_in.peek() == ' ') { ss_in.ignore(); } }
// Place extracted values into the output array
for (unsigned int k = 0; k < vect.size(); k++) {
if (k <= (unsigned int)outputs) {
outarr[k] = vect.at(k); }
}
return 0; // Success
}
/****** Initialization and cleanup ******/ /* Note 1:
* The Initialize(), InitializeRun(), Terminate() and TerminateRun()
* functions are optional.
* Implement them when you need to initialize something before the
* actual experiment is started and to cleanup/reset your DLL
* functionality for a next run.
* Note 2:
* When these functions are implemented, the "continue run"
* functionality in 20-sim is disabled.
*/ /**
* Initialize() [optional] *
* This function is called by the 20-sim simulator BEFORE starting the * simulation experiment (and only once in a multiple run experiment) * to initialize the dll properly.
*/
outputStream.open("c:\\temp\\20sim_cosim.log"); outputStream << "Initializing..." << std::endl;
// Setup ZeroMq
context = zmq_ctx_new();
zmqsocket = zmq_socket(context, ZMQ_REQ);
int timeout = 20000;
zmq_setsockopt(zmqsocket, ZMQ_SNDTIMEO, &timeout, sizeof(timeout)); zmq_setsockopt(zmqsocket, ZMQ_RCVTIMEO, &timeout, sizeof(timeout)); zmq_setsockopt(zmqsocket, ZMQ_CONNECT_TIMEOUT,
&timeout, sizeof(timeout));
int rc = zmq_connect(zmqsocket, "tcp://localhost:5555"); outputStream << "[ZMQ] Connect code: " << rc << std::endl;
return 0; // Indicate that the dll was initialized successfully.
}
/**
* InitializeRun() [optional] *
* This function is called by the 20-sim simulator BEFORE starting the * simulation experiment (and only once in a multiple run experiment) * to initialize the dll properly.
*/
DLLEXPORT int InitializeRun() {
/* Clear lasterrormessage before every run. */
snprintf(g_lasterrormessage, 255, "");
outputStream << "Initializing Run..." << std::endl;
return 0; /* Indicate that the dll was initialized successfully. */ }
/**
* TerminateRun() [optional] *
* This function is called by 20-sim after each finished run */
DLLEXPORT int TerminateRun() {
/* Cleanup / reset your DLL here for the next run * (e.g. in a multiple run experiment)
*/
outputStream << "Terminate of run requested..." << std::endl; outputStream << "Run Terminated!" << std::endl;
return 0; // Indicate that the DLL was terminated successfully.
}
/**
* Terminate() [optional] *
* This function is called by 20-sim on a DLL unload */
DLLEXPORT int Terminate() {
outputStream << "Unloading DLL..." << std::endl;
int result = 0;
if (zmqsocket) {
zmq_close(zmqsocket);
result = zmq_ctx_destroy(context); // Indicate that the dll was
// terminated successfully.
outputStream << "Socket destroy result: " << result
APPENDIX S. 20-SIM CO-SIMULATION PLUGIN 81
}
outputStream.close();
return result; // Indicate that the DLL was terminated successfully.
}
/**
* LastErrorMessage() [optional]
* Used by 20-sim to fetch a string with the last error that occurred * within the DLL
* @return A char pointer to a string indicating the error message */
DLLEXPORT char* LastErrorMessage() {
return g_lasterrormessage; }
T ECS Management Daemon
A management daemon to deploy and monitor ECS has been developed.
The Embedded Control Software Management Daemon allows the user to start/stop and mon- itor ECS running on embedded boards. The core library provides a simple interface. An abstract class is provided which can be used to implement any interface desired.
The current application provides a ROS interface. The ROS interface exposes services to start/stop the ECS and publishes log information a ROS topic.
The API for the ECS Management daemon is illustrated using an UML diagram in Figure T.1.
83
U Appendix 4 - Running the demo
These are the steps to run the demo on the Production Cell. Preparation:
• Make sure 2x RaMstix, 1x raspberry pi and 1x intel nuc are connected to the same network switch.
• Power up the Production Cell
• Boot RaMstix boards, raspberry pi 3 and Intel NUC Starting the control software:
• Login to RaMstix #1 (pcell-lpcontrol1) through SSH and run thepc_ecs_full_r12binary. • Login to RaMstix #3 (pcell-lpcontrol3) through SSH and run thepc_ecs_full_r3binary.
• On the Intel NUC, go to theProductionCellSimulatorROS workspace. And launch using
the bash script:start-ecs.bash
Starting the GUI:
• On the NUC, navigate to theProductionCellUIfolder and run the following command:$
meteor
• Now, in your browser, go tohttp://localhost:3000. This will open the UI and you will be able to control the production cell.
V Acknowledgements
From the RaM group at the University of Twente, I would like to thank the a few people in particular. Marcel Schwirtz for his work on the new software distribution of the RaMstix and allowing/enabling me to port ROS and the RaMstix FPGA driver to the new platform. Gerben te Riet o/g Scholten for his assistance and feedback on the design of the quality control module. Robin Wijnholt, now former student, for the fruitful discussions on the design and implement- ation of network channels. Dennis Ellery and Nicolo Botteghi for providing use-cases to test and validate my work.
Furthermore, I would like to thank my family, friends, and girlfriend for their unconditional support over the course of my study.
At last, but not least, I would like to thank my supervisors Jan Broenink and Zhou Lu for their feedback, insightful discussions, and for allowing and entrusting me to roam as free as I did. Also, for providing me the opportunity to take a break from this thesis project to work on the Aeroworks project.
85
Bibliography
Aeroworks (2014), Collaborative Aerial Workers,http://www.aeroworks2020.eu/, (Ac-
cessed on 02/12/2018).
Bastian, J., C. Clauß, S. Wolf and P. Schneider (2011), Master for co-simulation using FMI, in
Proceedings of the 8th International Modelica Conference; March 20th-22nd; Technical Univ-
eristy; Dresden; Germany, Linköping University Electronic Press, 63, pp. 115–120.
van den Berg, L. (2006), Design of a production cell setup,University of Twente.
Bezemer, M. M. (2013), Cyber-physical systems software development: way of working and tool suite.
Bezemer, M. M. and J. F. Broenink (2014), Hardware ports-getting rid of sandboxed modelled software.
Bezemer, M. M. and J. F. Broenink (2015), Connecting ROS to a real-time control framework for
embedded computing, inEmerging Technologies & Factory Automation (ETFA), 2015 IEEE
20th Conference on, IEEE, pp. 1–6.
Bezemer, M. M. and R. J. Wilterdink (2011), Luna: Hard real-time, multi-threaded, csp-capable
execution framework, inCommunicating Process Architectures, CPA 2011: 33th WoTUG Con-
ference on Concurrent and Parallel Programming, IOS Press.
Bezemer, M. M., R. J. Wilterdink and J. F. Broenink (2012), Design and use of csp meta-model for embedded control software development.
Borgerink, D. J., J. Stegenga, D. M. Brouwer, H. Wortche and S. Stramigioli (2014), Rail-guided robotic end-effector position error due to rail compliance and ship motion, inIntelligent
Robots and Systems (IROS 2014), 2014 IEEE/RSJ International Conference on, IEEE, pp. 3463–
3468.
Breedveld, P. C. (1985), Multibond graph elements in physical systems theory,vol. 319, no.1-2, pp. 1–36.
Broenink, J. F. and Y. Ni (2012), Model-driven robot-software design using integrated models
and co-simulation, inEmbedded Computer Systems (SAMOS), 2012 International Conference
on, IEEE, pp. 339–344.
Bruyninckx, H., M. Klotzbücher, N. Hochgeschwender, G. Kraetzschmar, L. Gherardi and D. Brugali (2013), The BRICS component model: a model-based development paradigm for
complex robotics software systems, inProceedings of the 28th Annual ACM Symposium on
Applied Computing, ACM, pp. 1758–1764.
Controllab Products (2008), 20-sim. http://www.20-sim.com/
Ellery, Dennis, D. D. (2017), Writing reusable code for robotics. Foundation, O. S. R. (2014), Gazebo: Robot simulation made easy,
http://gazebosim.org/, (Accessed on 02/20/2018).
Gomes, C., C. Thule, D. Broman, P. G. Larsen and H. Vangheluwe (2017), Co-simulation: State of the art,arXiv preprint arXiv:1702.00686.
Hoare, C. A. R. (1978), Communicating sequential processes, inThe origin of concurrent
programming, Springer, pp. 413–443.
Hoogendijk, T. (2013),Design of a generic software component for embedded control software
using CSP, Ph.D. thesis, MSc thesis 014RAM2013, Robotics and Mechatronics, University of
Twente.
I-Botics (2016), Joint innovation centre for interaction robotics,http://i-botics.com/, (Accessed on 02/12/2018).
Ivaldi, S., V. Padois and F. Nori (2014), Tools for dynamics simulation of robots: a survey based on user feedback,arXiv preprint arXiv:1402.7050.
Kempenaar, J. (2014),Communication Component for Multiplatform Distribution of Control
Algorithms, Master’s thesis, University of Twente.
Kiszka, J. (2005), The real-time driver model and first applications, in7th Real-Time Linux
Workshop, Lille, France.
Kuipers, F. P., R. Wester, J. Kuper and J. F. Broenink (2016), Mapping CSP Models to Hardware Using CλaSH.
Lelli, J., C. Scordino, L. Abeni and D. Faggioli (2016), Deadline scheduling in the linux kernel,
vol. 46, no.6, pp. 821–839.
Lofaro, D. M., A. Asokan and E. M. Roderick (2015), Feasibility of cloud enabled humanoid robots: Development of low latency geographically adjacent real-time cloud control, in
Humanoid Robots (Humanoids), 2015 IEEE-RAS 15th International Conference on, IEEE, pp.
519–526.
Lu, Z. and J. Broenink (2017), Co-simulation Design towards Cyber-Physical Robotic Applications.
Marconi, L., C. Melchiorri, M. Beetz, D. Pangercic, R. Siegwart, S. Leutenegger, R. Carloni, S. Stramigioli, H. Bruyninckx, P. Doherty et al. (2012), The SHERPA project: Smart
collaboration between humans and ground-aerial robots for improving rescuing activities in alpine environments, inSafety, Security, and Rescue Robotics (SSRR), 2012 IEEE
International Symposium on, IEEE, pp. 1–4.
Meijer, M., M. M. Bezemer and J. F. Broenink (2013), Finite State Machines And The Embedded