--- title: "Project#RoboDK x TwinCAT x MQTT x Opencv Color Dectector" url: "https://maker.wiznet.io/praveen_k/projects/ProjectRoboDK-x-TwinCAT-x-MQTT-x-Opencv-Color-Dectector/" markdown_url: "https://maker.wiznet.io/praveen_k/projects/ProjectRoboDK-x-TwinCAT-x-MQTT-x-Opencv-Color-Dectector/md" type: "UCC: User Created Content" author: "praveen_k" original_author: "praveen_k" original_url: "http://soup01.com/en/2022/04/03/projectrobodk-x-twincat-x-mqtt-x-opencv-color-dectector-2/" published: "2022-04-11" language: "en" hardware: ["M5 stack", "raspberry pi 3"] likes: 0 views: 340 comments: 0 source: "WIZnet Makers (https://maker.wiznet.io/)" --- # Project#RoboDK x TwinCAT x MQTT x Opencv Color Dectector > Project#RoboDK x TwinCAT x MQTT x Opencv Color Dectector Original author: praveen_k (source: http://soup01.com/en/2022/04/03/projectrobodk-x-twincat-x-mqtt-x-opencv-color-dectector-2/) ## Components - **M5 stack** x 1 - **raspberry pi 3** x 1 ## Documents and links - [Source code](https://github.com/soup01Threes/TwinCAT3/blob/main/Project_RoboDK_Color_Detector.zip) (code) ## Article Welcome to my blog! This is my other self-project to use RoboDK as a main station ? operate the robot and indicate the current status by LED Towner, we will use opencv to implement a color detector in python to detect Red/Yellow/Green Colors ,and publish these statuses via MQTT. Beckhoff TwinCAT TF6701 is used to subscribe to the topic also and implement a Data logger,Finally, A M5Core2+W5500 Lan Module will also subscribe to the topics from MQTT Broker that are built from Raspberry-pi3. There are some goals in this project and I will explain them step by step; - Create a RoboDK station with LED Tower display - Play with the RoboDK 2D Camera simulation interface - understand the opencv-python - how to read the images from filesystem - how to read the images from RoboDK - What is the RGB/HSV Color Model? - how to make a color detector? - how to draw the Contours? - implement the python MQTT Client - understand and program the Beckhoff TwinCAT Function TF6701 - Get the local-time and shifting the data 1 by 1 in the logging function - setup the MQTT Broker in Raspberry-pi3 - setup the M5Stack Lan Module - Implementing the MQTT Client with M5Stack in C Language ![](https://maker.wiznet.io/upload/mirror/body/b0720f4c1b4a610f6f2d.png) ## Other Project Here is my other project;) Please feel free to access it. > [Beckhoff#TwinCAT x RoboDK x Pi4 x Coral USB ML ? Image Classification](http://soup01.com/en/2022/02/27/beckhofftwincat-x-robodk-x-pi4-x-coral-usb-ml-image-classification-2/) ## Color Detecor ### Opencv installation Please use this command to install Opencv in your machine. | pip?**install**?opencv-python | | --- | #### if Trouble > [Python#Building wheel for opencv-python (PEP 517) … Never End](http://soup01.com/en/2022/03/05/pythonbuilding-wheel-for-opencv-python-pep-517-never-end-2/) > [Python#ImportError: DLL load failed while importing cv2: The specified module could not be found](http://soup01.com/en/2022/02/03/pythonimporterror-dll-load-failed-while-importing-cv2-the-specified-module-could-not-be-found-2/) ### Concept Now I will explain some basic concepts in opencv and skills that are used in this tutorial. #### Import images/Show Here is a sample code to import an image into opencv and show it. | **import**?cv2 **import**?numpy?**as**?np # Reading the image img = cv2.imread(‘image.jpg’) # Showing the output cv2.imshow(“Image”, img) cv2.waitKey(0) cv2.destroyAllWindows() | | --- | You would see the image like this. ![](https://lh6.googleusercontent.com/XH4ZmQiIeZFw6mlriVNFAhqmaidsbElp2S7OaxSSui2g595Xc2UCXjanOLhIvsCIQHoIgEI6mSZVB5bsbJgQXFK_b775h02JWRwyqIer_3obY-DxMiJPGzIq1SFwdyL6v_F0dtha) #### RGB color model RGB color values are supported in all browsers. An RGB color value is specified with: rgb(red, green, blue). Each parameter (red, green, and blue) defines the intensity of the color as an integer between 0 and 255. For example, rgb(0, 0, 255) is rendered as blue, because the blue parameter is set to its highest value (255) and the others are set to 0. ![](https://lh3.googleusercontent.com/fWo1-6en69qsoXTP_R2MOCmLsXGaIZBP_hQaR01x0rjNlGEtH1kgn8XF3w7Xe4AsKbCenZ4Lxgd4NJc_g53Oxj_86jz8-lULy44EtbuLBbw2_YafhcZPUZiXheh3EnPXB7hWVXRd) please reference w3c school to get more information [https://www.w3schools.com/colors/colors_rgb.asp?color=rgb(253,%20205,%20161)](https://www.w3schools.com/colors/colors_rgb.asp?color=rgb%28253,%20205,%20161%29) ##### Becareful! In Opencv, the sequence is not RGB and BGR. ##### RGB Calculator You can find thousands of RGB simulators on the internet and please practice by yourself;) ![](https://lh3.googleusercontent.com/BlywmwjrhfL3bMYh4A_9SYL_nUaTiS9iOfyHH5MUY_-KmofNNqPFGRSypZqsQkrMLoFuLBidpWeK8E_JJikqmA766tY4kBxoqs3wBRD18OHmWWPCDAJlVESWpkpkn3JUu9rkQ1RO) #### HSV Model The HSV color model shares most of its properties with HSL. (It is recommended that you read about HSL first as this page will build on the page about HSL). The range is 0-360 ,0-100%,0-100%. Hues are the three primary colors (red, blue, and yellow) and the three secondary colors (orange, green, and violet) that appear in the color wheel or color circle. When you refer to hue, you are referring to pure color, or the visible spectrum of basic colors that can be seen in a rainbow. Color saturation is the purity and intensity of a color as displayed in an image. The higher the saturation of a color, the more vivid and intense it is. The lower a color’s saturation, or chroma, the closer it is to pure gray on the grayscale. Learn more about saturation in our guide here. Color value refers to the relative lightness or darkness of a color. We perceive color value based on the quantity of light reflected off of a surface and absorbed by the human eye. We refer to the intensity of the light that reaches the eye as “luminance.” Please reference this link to get more information: ![](https://lh4.googleusercontent.com/cbOQOY7LUHDm8UlQJbsF84F_NcmeWMXwdExBOpskZ8Wpee7X4d67-J_V3_jGjynU2_lFHLC3fxteyuE1v4g-Ef7whfYMwq5Hq8u2Q7hNJplmXBcd01542ktoV_dsSaO-bmKISY7m) ##### Becareful! For HSV in opencv, hue range is [0,179], saturation range is [0,255], and value range is [0,255]. And different software uses different scales. ##### HSV Calculator You can find thousands of HSV simulators on the internet and please practice by yourself;) ![](https://lh3.googleusercontent.com/VOyOkfV4MaJ16MBBCed0sT-8VOL7ID2hBu4gY465mweD5rnfNCqmjV1nnV33ShgE3kCg6tg5Sim-CBgyYmT5LgPqJNmoFvSL2TuF2xvklrDuW0I7st2BY8CSb7_3MCBPypAgMD2W) ##### Mask We will use HSV instead of RGB Model to detect the color in the image. Here is a table to show the range of Red,Green and Blue. | | Rnage | Range in Opencv | | --- | --- | --- | | Red | 0-60,300-360 | 0-30,150-179 | | Green | 60-189 | 30-90 | | Blue | 180-300 | 90-150 | ###### Sample Here is an example to show you how to detect the yellow color in a banana image. we will set the lower_bound and upper_bound of the yellow color as a numpy and create a mask to filter it. | import cv2 import numpy as np # Reading the image img = cv2.imread(‘image.jpg’) # convert to hsv colorspace hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV) # lower bound and upper bound for Green color lower_bound = np.array([20, 50, 50]) upper_bound = np.array([100, 255, 255]) # find the colors within the boundaries mask = cv2.inRange(hsv, lower_bound, upper_bound) # Segment only the detected region segmented_img = cv2.bitwise_and(img, img, mask=mask) cv2.imshow(“Image”, segmented_img) cv2.waitKey(0) cv2.destroyAllWindows() | | --- | Good, here is the banana! ![](https://lh6.googleusercontent.com/1P_DG59PsZkUUfeRUaiFaWYb0n3oxMeVFVIDI2QLln5iJx-Y_db7zKclup9Q5KCN4e84akfWB-Wk1s5XX4hmcX70_DdYMcOBIvyAwf9GRUmZz3nZM9INIdAVYlP1dprjkz6vsGyF) ##### Contours Contours are the concepts that I explained in the last tutorial with Beckhoff Tc3_Vision. Please reference this tutorial to get more information: > [Beckhoff#Using TwinCAT TF7000 to read the BarCode](http://soup01.com/en/2022/02/21/3681/) ###### Sample In the previous sample, we detected the yellow color in a banana image. Now we will draw the Contours inside the image. | import cv2 import numpy as np # Reading the image img = cv2.imread(‘image.jpg’) # convert to hsv colorspace hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV) # lower bound and upper bound for Green color lower_bound = np.array([20, 50, 50]) upper_bound = np.array([100, 255, 255]) # find the colors within the boundaries mask = cv2.inRange(hsv, lower_bound, upper_bound) # Segment only the detected region segmented_img = cv2.bitwise_and(img, img, mask=mask) # Find contours from the mask contours, hierarchy = cv2.findContours(mask.copy(), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) output = cv2.drawContours(segmented_img, contours,-1,(0,255,0),3) # Showing the output cv2.imshow(“Output”, output) cv2.waitKey(0) cv2.destroyAllWindows() | | --- | Contours is drawn. ![](https://lh3.googleusercontent.com/jznyDwdaVdGTjYXW444Rafa90u8c-1o-ux0vKSjPHzpTYvI6EH4FAHM_Tr2c_qzrS1zl6ML5EanJ7fYr9WmzIP2ElSmwCg5vxkOcIELpspQFaj4T9rMV_aV2pKhykE-Ljy9chHXB) ### WorkFlow Finally, you can follow this flow to do the color detector. ![](https://maker.wiznet.io/upload/mirror/body/2121e1adb2affc3de931.png) ## RoboDK Side Here is the RoboDK station, Camera,3 colors of Signal Tower and A Robot. In this tutorial I will just start the Robot program manually(click the robot program). If you want, please follow my reference link to program your station to start it from PLC/Other Devices. ![](https://lh4.googleusercontent.com/1Dk3eYL0s5vZgL6u_GB4V2vMxzK3iJLw6phQB67wNsni1oRQMPj0gsvWejMq7aUgtbxLJ9NuqWcfu08juqINxJwXIT6hl93GJgZaXx7GVJ1ewIJWgnvYUQoqX-bROEv0dWKxCnd5) ### Reference Link > [RoboDK#Using 2D Camera Simulation Interface](http://soup01.com/en/2022/02/24/robodkusing-2d-camera-simulation-interface/) > [RoboDK#Start Program from TwinCAT By OPCUA](http://soup01.com/en/2022/02/25/robodkstart-program-from-twincat-by-opcua/) > [MQTT#using Python Library as a Subscriber and Publisher](http://soup01.com/en/2022/03/16/mqttusing-python-library-as-a-subscriber-and-publisher-2/) ### Light Tower Signal Here is the tutorial that shows you how to create the Light ON/OFF control. The Idea is very easy ? Place a Light with deep color and place another one with light color. Finally, using Event to Show/Hide the objects. ![](https://lh3.googleusercontent.com/e6Hwcn9_rkON7opTnSYy7D__6zGXi1aTS6BYG_0-tqUllvOi2OgXJIsLnJVf5e2krEjT1zG3rm0z5zuiRlj2UtIlWq26pVhx8hxtvoYXIrAyh4ygLtVwiFe9Uw81g249nbLcMQKF) #### Red Here is the Red Color. ![](https://lh3.googleusercontent.com/G8rWFlzEGsWZtryBil3NG7hIP0X5b_4_X6CMnLexZewgcAw_LIb117wgYsLQ-mNiYw0veN2Ua29KC7vns7oXBfbXRspi978ljVK2PHUG6p1PDlB1iF9XyV4n0eSUci3PRKy9Ac_0) #### Yellow Here is the Yellow Color. ![](https://lh3.googleusercontent.com/4ypWrCH7u65Uw_z6TXzOsOGiD6GULNHG9QcnHxnn4lOPtGiq8E4ngGsCdxthIBAqtgP23MKiE4TnwlomKOU6j8G0kd86vZl4DrufatqCCyVUjT5-sMwhEpsn584U9d9iolD0US0t) #### Green Here is the Green Color. ![](https://lh3.googleusercontent.com/vBROLPjMVULPcXQ27jbyZUlQ9CpzXvP-clcX-dfYoWcraHMGRSJ1ZAGLZTfqQsykwY4wSaoamu3AZqxmPUGeb9Hs34rj4AiU0nPQJjjUUbebbESYeY16yWHrGjOOw5WzwgS-1Qgl) ### Signal Flow The Color tower will be turned on with different colors, depending on the status of the Robot. If the Robot Program is started>Green will be on,Yellow and Red will be off. If the Robot Program is started and the status is waiting for IO_5>Yellow will also be on. If the Robot Program is started and the status is passed for the IO_5 waiting Step, Yellow will be off. If the Robot Program ends>Green will be off and red will be on. ![](https://maker.wiznet.io/upload/mirror/body/4b5e5bd0d8443c371d19.png) ### WorkFlow The working Flow of the Color Detector is integrated into the program from the previous opencv program,and also adds the features of publishing the topic to an MQTT Broker. The topic would be the current status of Lamp Tower. ? ![](https://maker.wiznet.io/upload/mirror/body/5cff9741cb6882aecac0.png) #### Code Here is the Code in RoboDK to implement a Color detector and MQTT Client. | **from**?robodk?**import**?robolink? ? # RoboDK API **from**?robodk?**import**?robomath? ? # Robot toolbox **import**?cv2 **import**?numpy?**as**?np **import**?paho.mqtt.client?**as**?mqtt #MQTT Client **def**?**on_connect**(client,userdata,flag,rc): print(‘Connected with result code ‘+str(rc)) client.subscribe(‘RoboDK/Station/s1/Lamp/Red’) **def**?**on_disconnect**(client,userdata,flag,rc): **if**?rc !=0: print(‘Something Wrong..’) **def**?**on_publish**(client,userdata,mid): print(‘publish: {0}’.format(mid)) RDK = robolink.Robolink() count=0 CAM_NAME=’Camera1′ # GREEN_LOWER_BOUND=np.array([40, 150, 150]) GREEN_HIGHER_BOUND=np.array([100, 255, 255]) RED_LOWER_BOUND=np.array([130, 150, 150]) REG_HIGHER_BOUND=np.array([179, 255, 255]) YELLOW_LOWER_BOUND=np.array([20, 150, 150]) YELLOW_HIGHER_BOUND=np.array([35, 255, 255]) cam_item = RDK.Item(CAM_NAME) **if**?**not**?cam_item.Valid(): **raise**?Exception(“Camera not found! %s” % CAM_NAME) cam_item.setParam(‘Open’, 1)? # Force the camera view to open BROCKER_URL=’192.168.1.50′ BROCKET_PORT=1883 MQTT_BASIC_TOPIC=’RoboDK/Station/s1/’ #???????????- # Settings PROCESS_COUNT = 300? # How many frames to process before exiting. -1 means indefinitely. client=mqtt.Client() client.on_connect=on_connect client.on_disconnect=on_disconnect client.on_publish=on_publish client.connect(BROCKER_URL,BROCKET_PORT,60) **while**?count < PROCESS_COUNT?**or**?PROCESS_COUNT < 0: Green,Red,Yellow=**False**,**False**,**False** print(“=============================================”) print(“Processing image %i” % count) count += 1 #???????????????- # Get the image from RoboDK bytes_img = RDK.Cam2D_Snapshot(“”, cam_item) **if**?bytes_img == b”: **raise** # Image from RoboDK are BGR, uchar, (h,w,3) nparr = np.frombuffer(bytes_img, np.uint8) img = cv2.imdecode(nparr, cv2.IMREAD_UNCHANGED) hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV) mask_y = cv2.inRange(hsv, YELLOW_LOWER_BOUND, YELLOW_HIGHER_BOUND) mask_r = cv2.inRange(hsv, RED_LOWER_BOUND, REG_HIGHER_BOUND) mask_g = cv2.inRange(hsv, GREEN_LOWER_BOUND, GREEN_HIGHER_BOUND) # Find contours from the mask contours_y, hierarchy = cv2.findContours(mask_y.copy(), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) contours_r, hierarchy = cv2.findContours(mask_r.copy(), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) contours_g, hierarchy = cv2.findContours(mask_g.copy(), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) output = cv2.drawContours(img, contours_y,-1,(255,197,250),3) output = cv2.drawContours(img, contours_r,-1,(255,197,250),3) output = cv2.drawContours(img, contours_g,-1,(255,197,250),3) output = cv2.resize(output, (int(output.shape[1] * .3), int(output.shape[0] * .3))) **if**?img?**is**?**None**?**or**?img.shape == (): **raise** key = cv2.waitKey(200) **if**?key == 27: **break**? # User pressed ESC, exit cv2.imshow(“Output”, output) client.publish(MQTT_BASIC_TOPIC+’Lamp/Yellow’,str(len(contours_y))) client.publish(MQTT_BASIC_TOPIC+’Lamp/Red’,str(len(contours_r))) client.publish(MQTT_BASIC_TOPIC+’Lamp/Green’,str(len(contours_g))) **if**?len(contours_y) >0: Yellow=**Trueif**?len(contours_r) >0: Red=**Trueif**?len(contours_g) >0: Green=**True** cv2.waitKey(500) cv2.destroyAllWindows() | | --- | ## Install Broker Some Useful Link while using Raspberry > [Raspberrypi#Using VNC Viewer](http://soup01.com/en/2022/03/04/raspberrypiusing-vnc-viewer-2/) > [RaspberryPI#Set the static ip without starting the OS](http://soup01.com/en/2022/03/04/raspberrypiset-the-static-ip-without-starting-the-os/) > [RaspberryPI#Enable the SSH Interface without start the OS](http://soup01.com/en/2022/03/04/raspberrypienable-the-ssh-interface-without-start-the-os/) Here is the tutorial to build a broker in the raspberry pi/ubuntu. > [MQTT#Broker Setup](http://soup01.com/en/2022/03/15/mqttbroker-setup/) If the connection is refused: > [MQTT#Connection is refuesd solution](http://soup01.com/en/2022/03/14/mqttconnection-is-refuesd-solution-2/) ## TwinCAT Side ### Reference Link Please reference these links to operate with MQTT in TwinCAT. > [Beckhoff#Using TwinCAT TF6701 to Subscribe Topics](http://soup01.com/en/2022/03/22/beckhoffusing-twincat-tf6701-to-subscribe-topics/) > [Beckhoff#Using TwinCAT TF6701 to Publish message inAWS IOT Core](http://soup01.com/en/2022/01/18/beckhoffusing-twincat-tf6701-to-publish-message-inaws-iot-core/) ### WorkFlow In the TwinCATSide, the work flow is very simple. Get the Local time cyclically>set the MQTT Connection Parameters>Subscribe the Topic> if any new messages come from these topics> define the topic title>set the logger trigger flag>logged the status>loop back. ![](https://maker.wiznet.io/upload/mirror/body/7447f48fd198235173ad.png) ### Program #### DUT_Log This is the DUT of data logging, A String format of timestamp and the status is defined here. | **TYPE**?DUT_Log : STRUCT timeStamp:STRING; **Status**?:DINT; END_STRUCT END_TYPE | | --- | #### GVL_Logger The Global variables of the logging data. You can adjust the array length as you like. | VAR_GLOBAL **Red**,**Green**,**Yellow**:ARRAY[0..99]OF DUT_Log; END_VAR | | --- | #### Function Block FB_GetLocalTime Here is the Function Block to get the Local time of the runtime. Please reference this tutorial? to get more information: > [Beckhoff#Get the TwinCAT3 Local Time](http://soup01.com/en/2022/03/27/beckhoffget-the-twincat3-local-time/) ##### VAR_INPUT True=Get the Local Time | VAR_INPUT i_bEnable:BOOL:=**TRUE**; END_VAR | | --- | ##### VAR Some variables for internal use and the instance of FB_LocaSystemTime. | VAR **bTimeSyncError?**:**BOOL; **TimeStruct :TIMESTRUCT; InitTimeStruct :TIMESTRUCT; LocalSystemTime :FB_LocalSystemTime; dwCycle:INT:=1; TON:TON; END_VAR | | --- | ##### PROGRAM Here is the main program, it is a very simple cyclic called the FB_LocalSystemTime() and detects any error of this function block. | LocalSystemTime( **bEnable**:=i_bEnable ,dwCycle:=1 ); TON( **IN**:=NOT LocalSystemTime.bValid ,PT:=INT_TO_TIME( (**dwCycle+ciTimeValidCheckOffset**)*1000 ) ); TimeStruct:=LocalSystemTime.systemTime; IF TON.Q THEN bTimeSyncError:=TRUE; END_IF | | --- | ##### METHOD Init : BOOL A method to reset the current time value to default. | TimeStruct:=InitTimeStruct; | | --- | ##### METHOD Reset : BOOL A method to reset the error flag of function block. | bTimeSyncError:=FALSE; | | --- | ##### METHOD TimeInString : String This is a method to change the time value to a String format and use the CONCAT function to join them. Here is the format:yy-mm-dd-hh:mm:ss | VAR _string:STRING; END_VAR _string:=CONCAT(WORD_TO_STRING(TimeStruct.wYear),’-‘); _string:=CONCAT(_string,WORD_TO_STRING(Timestruct.wMonth)); _string:=CONCAT(_string,’-‘); _string:=CONCAT(_string,WORD_TO_STRING(Timestruct.wDay)); _string:=CONCAT(_string,’-‘); _string:=CONCAT(_string,WORD_TO_STRING(TimeStruct.wHour)); _string:=CONCAT(_string,’:’); _string:=CONCAT(_string,WORD_TO_STRING(TimeStruct.wMinute)); _string:=CONCAT(_string,’:’); _string:=CONCAT(_string,WORD_TO_STRING(TimeStruct.wSecond)); TimeInString:=_string; | | --- | ##### PROPERTY LocalTimeStruct : TIMESTRUCT ###### GET The property to get the current system time. | LocalTimeStruct:=LocalSystemTime.systemTime; | | --- | ##### PROPERTY LocalTimeValid : Bool ###### GET The property to get the current system time is valid or not. | LocalTimeValid:=LocalSystemTime.bValid; | | --- | ##### PROPERTY TimeSyncError : Bool ###### GET The property to get the current function block is normal or not. | TimeSyncError:=bTimeSyncError; | | --- | #### FUNCTION_BLOCK FB_Logger Here is a function block to save the LED Tower status and the time. 0=Off、1=On. ##### VAR_INPUT | **VAR_INPUTi_TimeStamp**:STRING; **i_Value**?? ? :DINT; **END_VAR** | | --- | ##### VAR_IN_OUT | VAR_IN_OUT io_logData:**ARRAY**[*]**OF**?DUT_Log; END_VAR | | --- | ##### VAR | **VARindex**:INT:=0; END_VAR | | --- | ##### METHOD ClearLog : BOOL A method to clear the log. ###### VAR_INPUT | VAR_INPUT bStart:**BOOL; **END_VAR | | --- | ###### VAR_INST | **VAR_INSTR_TRIG**:R_TRIG; **END_VAR** | | --- | ###### VAR | **VAR** i:DINT; END_VAR | | --- | ###### PRGORAM | R_TRIG(CLK:=bStart); **IF**?R_TRIG.Q?**THENFOR**?i:=ArrayLower?**TO**?ArrayHigher?**DO** io_logData[i].Status:=0; io_logData[i].timeStamp:=’ ‘; END_FOR END_IF | | --- | ##### METHOD datalog : BOOL The method to save and “shift” the log. ###### VAR_INPUT | VAR_INPUT bStart:**BOOL; **END_VAR | | --- | ###### VAR_INST | **VAR_INSTR_TRIG**:R_TRIG; **END_VAR** | | --- | ###### VAR | **VAR** i:DINT:=0; END_VAR | | --- | ###### PROGRAM Here is a small program to auto calculate the DUT size that needs to be shifted. MEMCPY is a function to copy the memory from source to destination. [0] is the newest value. | datalog:=FALSE; R_TRIG(CLK:=bStart); IF R_TRIG.Q THEN MEMCPY( destAddr:=ADR(io_logData[1]) ,srcAddr:=ADR(io_logData[0]) ,n:=SIZEOF(io_logData[0])*ArrayHigher ); io_logData[0].Status:=i_Value; io_logData[0].timeStamp:=i_TimeStamp; datalog:=TRUE; END_IF | | --- | ##### PROPERTY ArrayHigher : DINT The property gets the array lower and Upper. ###### GET | ArrayHigher:=UPPER_BOUND(io_logData,1); | | --- | ##### PROPERTY ArrayLower : DINT ###### GET | ArrayLower:=LOWER_BOUND(io_logData,1); | | --- | #### MAIN ##### VAR | PROGRAM MAIN VAR MQTTClient:FB_IotMqttClient; payload:STRING(255); payload_r,payload_y,payload_g:STRING(20); fbMessageQueue : FB_IotMqttMessageQueue; fbMessage? ? ? : FB_IotMqttMessage; sTopic :STRING; **bSetNullTermination?**:**BOOL; **bScbscribed:**BOOL; **sTopicReceived:STRING(255); Value_r,Value_g,Value_y:**DINT; ** GetLocalTime:FB_GetLocalTime; stTimeInString:STRING; datalog_r,datalog_g,datalog_y:FB_Logger; LastTimeStamp_r,LastTimeStamp_g,LastTimeStamp_y:STRING; i1,i2:**DINT; **bTest:**BOOL; **binit:**BOOL; ** logTrigger_r,logTrigger_g,logTrigger_y:**BOOL; **hmi_LP_r,hmi_LP_g,hmi_LP_y:**BOOL; **END_VAR | | --- | ##### PROGRAM | //Get Local Time GetLocalTime( i_bEnable:=TRUE ); IF GetLocalTime.LocalTimeValid THEN stTimeInString:=GetLocalTime.TimeInString(); END_IF; //init IF binit THEN LastTimeStamp_r:=stTimeInString; LastTimeStamp_g:=stTimeInString; LastTimeStamp_y:=stTimeInString; binit:=FALSE; END_IF //MQTT Client Setup //Parameters MQTTClient.sHostName:=’192.168.1.50′; MQTTClient.nKeepAlive:=60; MQTTClient.ipMessageQueue:=fbMessageQueue; //Execute() must always Run MQTTClient.Execute(bConnect:=TRUE); //Subscribe Topic sTopic:=’RoboDK/Station/s1/#’; IF MQTTClient.bConnected THEN IF NOT bScbscribed THEN MQTTClient.Subscribe( sTopic:=sTopic ,eQoS:=0 ); bScbscribed:=TRUE; END_IF END_IF //Check Message From Broker IF fbMessageQueue.nQueuedMessages > 0 THEN IF fbMessageQueue.Dequeue(fbMessage:=fbMessage) THEN //Get the Topic fbMessage.GetTopic(pTopic:=ADR(sTopicReceived), nTopicSize:=SIZEOF(sTopicReceived)); //Compare IF fbMessage.CompareTopic(sTopic:=’RoboDK/Station/s1/Lamp/Red’) THEN payload_r:=’ ‘; fbMessage.GetPayload(pPayload:=ADR(payload_r), nPayloadSize:=SIZEOF(payload_r), bSetNullTermination:=FALSE); LastTimeStamp_r:=GetLocalTime.TimeInString(); logTrigger_r:=TRUE; ELSIF fbMessage.CompareTopic(sTopic:=’RoboDK/Station/s1/Lamp/Yellow’) THEN payload_y:=’ ‘; fbMessage.GetPayload(pPayload:=ADR(payload_y), nPayloadSize:=SIZEOF(payload_y), bSetNullTermination:=FALSE); LastTimeStamp_y:=GetLocalTime.TimeInString(); logTrigger_y:=TRUE; ELSIF fbMessage.CompareTopic(sTopic:=’RoboDK/Station/s1/Lamp/Green’) THEN payload_g:=’ ‘; fbMessage.GetPayload(pPayload:=ADR(payload_g), nPayloadSize:=SIZEOF(payload_g), bSetNullTermination:=FALSE); LastTimeStamp_g:=GetLocalTime.TimeInString(); logTrigger_g:=TRUE; END_IF END_IF END_IF //Convert the Value from String to Numbers Value_r:=STRING_TO_DINT(payload_r); Value_g:=STRING_TO_DINT(payload_g); Value_y:=STRING_TO_DINT(payload_y); //Hmi hmi_LP_r:=Value_r =1; hmi_LP_g:=Value_g =1; hmi_LP_y:=Value_y =1; //Data logger //Green Lamp datalog_g( i_TimeStamp:=stTimeInString ,i_Value:=Value_g ,io_logData:=GVL_Logger.Green ); IF datalog_g.datalog(bStart:=logTrigger_g) THEN logTrigger_g:=FALSE; END_IF //Yellow Lamp datalog_y( i_TimeStamp:=stTimeInString ,i_Value:=Value_y ,io_logData:=GVL_Logger.Yellow ); IF datalog_y.datalog(bStart:=logTrigger_y) THEN logTrigger_y:=FALSE; END_IF //Red Lamp datalog_r( i_TimeStamp:=stTimeInString ,i_Value:=Value_r ,io_logData:=GVL_Logger.Red ); IF datalog_r.datalog(bStart:=logTrigger_r) THEN logTrigger_r:=FALSE; END_IF | | --- | ### Visulization A screen to show the LED Tower status in Beckhoff TE1800. ![](https://lh5.googleusercontent.com/cNctZslrXM4r3AZTnunuRkBCXxQ3upyoqpTyoKfvFQ2IQP21PWr281VkO1yujmwvArESVy1Oh-IDyztZ4lgwPeIMTl_C-EM_kWcNJD6j2BKh-l_yRq2CmCIM8Lrux0DQ82kFlGLt) ## M5Core2 Side Please reference this tutorial to setup a M5Stack with W5500 LAN Module. > [M5Stack#LAN W5500 with MQTT](http://soup01.com/en/2022/03/31/m5stacklan-w5500-with-mqtt-2/) ### Code Here is the Code in M5Stack to implement an MQTT Client in C Language, Subscribe the Topic from RoboDK and Display the Current LED Tower status in the LED Display. | #include <M5Core2.h> #include <SPI.h> #include <Ethernet2.h> #include <PubSubClient.h> #define SCK 18 #define MISO 19 #define MOSI 23 #define CS 26 **byte**?mac[] = { 0x90, 0xA2, 0xDA, 0x0F, 0x08, 0xE1 }; IPAddress Local(192, 168, 1, 211); EthernetClient ethClient; PubSubClient mqttClient(ethClient); //MQTT Broker **const**?**char**?*ENDPOINT=”192.168.001.050″; **const**?**int**?PORT=1883; **char**?*MYID=”M5Stack”; **char**?*PUBLISHTOPIC=”M5Stack/pub”; **char**?*SUBTOPIC=”RoboDK/Station/s1/#”; // **int**?GreenStatus=0; **int**?RedStatus=0; **int**?YellowStatus=0; // **char**?*SubtopicYellow=”RoboDK/Station/s1/Lamp/Yellow”; **char**?*SubtopicGreen=”RoboDK/Station/s1/Lamp/Green”; **char**?*SubtopicRed=”RoboDK/Station/s1/Lamp/Red”; **void**?callback(**char*** topic,?**byte*** payload,?**unsigned**?**int**?length) { Serial.print(“Message arrived [“); Serial.print(topic); **int**?result_yellow=strcmp(topic,SubtopicYellow); **int**?result_red=strcmp(topic,SubtopicRed); **int**?result_green=strcmp(topic,SubtopicGreen); if (result_yellow ==0 ){ Serial.print(payload[0]); if (payload[0]==48){ YellowStatus=0; Serial.print(YellowStatus); } if (payload[0]==49){ YellowStatus=1; Serial.print(YellowStatus); } } if (result_red ==0 ){ Serial.print(payload[0]); if (payload[0]==48){ RedStatus=0; } if (payload[0]==49){ RedStatus=1; } } if (result_green ==0 ){ Serial.print(payload[0]); if (payload[0]==48){ GreenStatus=0; } if (payload[0]==49){ GreenStatus=1; } } Serial.print(“] “); for (**int**?i = 0; i < length; i++) { Serial.print((**char**)payload[i]); } } **void**?setup(){ //If LAN Modules is used,Please use this operation mode M5.begin(true, true, true, false, kMBusModeInput); Serial2.begin(115200, SERIAL_8N1, 13, 14); // START M5.Lcd.fillScreen(BLACK); M5.Lcd.setCursor(10, 10); M5.Lcd.setTextColor(WHITE); M5.Lcd.setTextSize(3); M5.Lcd.printf(“Inited..n”); M5.Lcd.printf(“IP:192.168.1.211n”); M5.Lcd.printf(“Mac:90-A2-DA-x0F-08-E1n”); mqttClient.setServer(ENDPOINT, PORT); mqttClient.setCallback(callback); SPI.begin(18, 19, 23, -1); Ethernet.init(26); Ethernet.begin(mac, Local); connectMQTT(); } **void**?mqttLoop() { if (!mqttClient.connected()) { connectMQTT(); } mqttClient.loop(); } **void**?connectMQTT() { while (!mqttClient.connected()) { Serial.print(“Connection Loop..”); if (mqttClient.connect(MYID)) { Serial.println(“Connected.”); **int**?qos = 0; mqttClient.subscribe(SUBTOPIC, qos); Serial.println(“Subscribed.”); } else { Serial.print(“Failed. Error state=”); Serial.print(mqttClient.state()); // Wait 5 seconds before retrying delay(1000); } } } **long**?messageSentAt = 0; **int**?count = 0; **char**?pubMessage[128]; **int**?led,red,green,blue; **int**?status=0; **void**?loop() { mqttLoop(); **long**?now = millis(); if (now ? messageSentAt > 500) { M5.update(); M5.Lcd.fillScreen(BLACK); if (YellowStatus ==1){ M5.Lcd.fillCircle(160, 120, 30, YELLOW); } if (YellowStatus ==0){ M5.Lcd.drawCircle(160, 120, 30, YELLOW); } if (RedStatus ==1){ M5.Lcd.fillCircle(240, 120, 30, RED); } if (RedStatus ==0){ M5.Lcd.drawCircle(240, 120, 30, RED); } if (GreenStatus ==1){ M5.Lcd.fillCircle(80, 120, 30, GREEN); } if (GreenStatus ==0){ M5.Lcd.drawCircle(80, 120, 30, GREEN); } Serial.print(YellowStatus); messageSentAt = now; sprintf(pubMessage, “{”count”: %d}”, count++); Serial.print(“Publishing message to topic “); Serial.println(PUBLISHTOPIC); Serial.println(pubMessage); mqttClient.publish(PUBLISHTOPIC, pubMessage); Serial.println(“Published.”); } } | | --- | ## Result ## Result-M5Core2 Side ## Source Code Please access this link to download the source code; You can find the RoboDK Project,TwinCAT Project and the M5Stack Source Code. --- Source: https://maker.wiznet.io/praveen_k/projects/ProjectRoboDK-x-TwinCAT-x-MQTT-x-Opencv-Color-Dectector/