私のBlogへようこそ！

今回のProjectはRoboDKからロボットStationを作成し、ロボットが動作してる間となりにあるLED Towerが赤、緑、黄色を表示させます。RoboDK内でPythonとOpencvを使用しColor Dectectorを実装し、現在LED Towerの色を検知します。

もちろんそれだけではなく、RoboDK StationがPythonでMQTT Clientを実装しRaspberry-piのMQTT BrokerにMessageをPublishします。

TwinCAT側ではTF6701 Functionを使用しRaspberry-piのMQTT BrokerからRoboDKのTopicをSubscribeし、現在のLED Towner色を取得しTF1800表示させ、データも記録する。

最後に、 M5Stack Core2とW5500 Lan Moduleからも同じのTopicをSubscribeし、RoboDKのLED Towner現在の状態をLED Display表示します。

今回のProjectはいくつかのゴールがありますが、記事内でできるだけ丁寧に説明しますので、よろしくお願います。

RoboDK Stationを作成しLED Townerを作る
RoboDK 2D Camera simulation inteface使用
Opencvを理解する
- ImageをFile Systemから読み込む
- ImageをRoboDK の2D Camera Simunlation interfaceから読み込む
- RGB/HSV Modelはなにか
- Color Detectorの作り方
- ObjectにContoursを書く方法
PythonからMQTT Clientを実装する方法
Beckhoff TwinCAT TF6701 の使用方法
Beckhoff TwinCATからDataを記録するのに必要なLocal-time取得とLoggingプログラム作成
Raspberry-pi3からMQTT Brokerを立ち上げる
M5Stack Core2とW5500 Lan ModuleのMQTT Client実装方法

Other Project

こちらはこの間のProjectです。

興味ある方はみてください。

Beckhoff#TwinCAT x RoboDK x Pi4 x Coral USB ML – Image Classification

Color Detecor

Opencv installation

以下のコマンドでOpencvをインストールします。

pip install opencv-python

if Trouble

Python#Building wheel for opencv-python (PEP 517) … Never End

Python#ImportError: DLL load failed while importing cv2: The specified module could not be found

Concept

これからOpencvのいくつかのコンセプトと使用するテクニックを説明します。

Import images/Show

まずFile system内にあるImageをOpencvに読み込んで表示します。

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()

このようなPopupが表示されます。

RGB color model

RGB（またはRGBカラーモデル）とは、色の表現法の一種で、赤 (Red)、緑 (Green)、青 (Blue)の三つの原色を混ぜて幅広い色を再現する加法混合の一種である。RGBは三原色の頭文字である。

各パラメータは0-255になります。

もっと知りたい方はWikiを参考にしてください。

https://ja.wikipedia.org/wiki/RGB

Becareful!

Opencvでは順番はRGBではなくRGRになります。

RGB Calculator

インターネットにはRGBSimulatorがたくさんありますので練習してください。

https://www.w3schools.com/colors/colors_rgb.asp

HSV Model

Wikiからよりますと、

HSV色空間(英: HSV model)は色相(Hue)、彩度(Saturation・Chroma)、明度(Value・Brightness)の三つの成分からなる色空間。HSBモデル(Hue、Saturation、Brightness)とも言われる。

色相 – 色の種類（例えば赤、青、黄色）。0 – 360の範囲（アプリケーションによっては0 – 100 % に正規化されることもある）。
彩度 – 色の鮮やかさ。0 – 100 % の範囲。刺激純度と colorimetric purity の色彩的な量を比較して「純度」などともいう。色の彩度の低下につれて、灰色さが顕著になり、くすんだ色が現れる。また彩度の逆として「desaturation」を定義すると有益である。
明度 – 色の明るさ。0 – 100 % の範囲。

もっと知りたい方はWikiを参考にしてください。

https://ja.wikipedia.org/wiki/HSV%E8%89%B2%E7%A9%BA%E9%96%93

Becareful!

OpencvではHは0-179までSとVは0-255になります。

HSV Calculator

同じくネットではHSV Simulatorがたくさんあります。http://color.lukas-stratmann.com/color-systems/hsv.html

Mask

私たちはRGBモデルではなくHSVモデルで色を感知します。

こちらのTableは3色のHSVのRangeです。

	Rnage	Range in Opencv
Red	0-60,300-360	0-30,150-179
Green	60-189	30-90
Blue	180-300	90-150

Sample

まず最初にバナナのImageから黄色を検出してみます。

lower_bound とupper_boundのNumpy配列からMaskを作成し、黄色をFilter Outします。

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()

よしバナナ見ましたね！

Contours

Contours はObjectの枠などを示すもので、以前BeckhoffのTc3_Vision記事で説明しましたので、よかったら参考にしてください。

Beckhoff#TwinCAT TF7000を使用しBarCode読み込んでみよう

Sample

前の例ではバナナImageから黄色を検知しましたので、今度はバナナのContoursを描き出してみます。

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が描きされました！

WorkFlow

最後に、私は以下のFlowでColor Detecorを作ります。

RoboDK Side

RoboDK Stationでは3色のLED TownerとRobotを配置し2D Camera InterfaceでLED Townerを撮影し色を検知し、そしてMQTT を使用しBrokerにMessageをPublishします。

今回は手動でRobotプログラムをStartしましたが、もちろんPLCなどから経由でもOKです。

Reference Link

Beckhoff#TwinCAT TF7000を使用しBarCode読み込んでみよう

RoboDK# 2D Camera Simulation Interfaceを使ってみよう

RoboDK#TwinCATとOPCUA連携し異なるプログラム起動する

MQTT#using Python Library as a Subscriber and Publisher

Light Tower Signal

次はどうやってLED TownerのON/OFF設定するかを説明します。

Ideaは簡単でLED を2つ重ねて一つを明るい色でもう一つは暗い色にします。

最後はEventを使用しLEDを表示・非表示します。

RoboDKの公式Youtubeでは詳しい説明あります。

Red

これは赤色です。

Yellow

これは黄色です。

Green

これは緑色です。

Signal Flow

Color TowerはRobotの状態により違う色が表示されます。

Robotプログラムが起動されたら、緑=ON、赤と黄色=OFF。

Robotプログラムが起動されIO_5待ち状態なら、緑と黄色=ON、赤=OFF。

Robotプログラムが起動されIO_5待ち状態過ぎたら、緑=ON、赤と黄色=OFF。

Robotプログラムが完了したら、赤=ON、緑と黄色=OFF。

WorkFlow

こちらはColor Detecorと連動したWork Flowです。あまり変わらずMQTT Clientのプログラムが追加されただけです。

MQTTからPublishされたTopicはLED TowerのON./OFFです。

–

Code

以下はColor DetectorとMQTT Clientを実装したCodeです。

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=True
if len(contours_r) >0:
Red=True
if len(contours_g) >0:
Green=True
cv2.waitKey(500)

cv2.destroyAllWindows()

Install Broker

Raspberryの便利Link

RaspberryPI#OSを入れずにSSHをEnableする方法

RaspberryPI#初期でcmdline.txt編集しStatic IP設定

Raspberrypi#Using VNC Viewer

Brokerインストール

MQTT#Brokerのセットアップ

もしConnectionが拒否されたら

MQTT#Connection is refuesd solution

TwinCAT Side

Reference Link

以下のLinkでTwinCATからMQTTを操作する記事があります。

Beckhoff#TwinCAT TF6701 TopicをSubscribeする

Beckhoff#TwinCAT TF6701 AWS IOT CoreにMessage Publishする

WorkFlow

TwinCAT側ではすごくシンプルです。

Local Timeを取ります>MQTT Connectionパラメタ設定>TopicをSubscribeする>新しいMessageがくる>そのMessageのTopicを判断する>Loggingする

のような流れです。

Program

DUT_Log

Data loggingのDUTです。StringはTimestampで、StatusはLEDの状態です。

TYPE DUT_Log :
STRUCT
timeStamp:STRING;
Status :DINT;
END_STRUCT
END_TYPE

GVL_Logger

DUT_Logの配列をGVLで定義します。

VAR_GLOBAL
Red,Green,Yellow:ARRAY[0..99]OF DUT_Log;
END_VAR

Function Block FB_GetLocalTime

次はTwinCATからLocalTimwを取ります。

下記のLinkでTwinCATからLocal-Timeを取る方法を書いてます。

Beckhoff#TwinCAT3 Local Timeを取得する

VAR_INPUT

True=Local Time取得開始

VAR_INPUT
i_bEnable:BOOL:=TRUE;
END_VAR

VAR

内部使用する変数などです。

VAR
bTimeSyncError :BOOL;
TimeStruct :TIMESTRUCT;
InitTimeStruct :TIMESTRUCT;
LocalSystemTime :FB_LocalSystemTime;
dwCycle:INT:=1;
TON:TON;
END_VAR

PROGRAM

プログラムはシンプルで周期FB_LocalSystemTime() を呼び出し常にエラーあるかどうかをCheckするだけです。

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

時間をリセットするMethodです。

TimeStruct:=InitTimeStruct;

METHOD Reset : BOOL

エラーをリセットするMethodです。

bTimeSyncError:=FALSE;

METHOD TimeInString : String

こちらは時間FormatをStringに変換し、CONCAT関数を使用し各部分を繋がります。

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

現在時間のproperty です。

LocalTimeStruct:=LocalSystemTime.systemTime;

PROPERTY LocalTimeValid : Bool

GET

現在時間が有効かどうかを確認できるproperty です。

LocalTimeValid:=LocalSystemTime.bValid;

PROPERTY TimeSyncError : Bool

GET

現在Function Blockがエラーであるかどうか確認できるpropertyです。

TimeSyncError:=bTimeSyncError;

FUNCTION_BLOCK FB_Logger

LED Towerの状態を記録するFunction Blockです。

0=Off、1=On.

VAR_INPUT

VAR_INPUT
i_TimeStamp:STRING;
i_Value :DINT;
END_VAR

VAR_IN_OUT

VAR_IN_OUT
io_logData:ARRAY[*]OF DUT_Log;
END_VAR

VAR

VAR
index:INT:=0;
END_VAR

METHOD ClearLog : BOOL

LogをクリアするMethodです。

VAR_INPUT

VAR_INPUT
bStart:BOOL;
END_VAR

VAR_INST

VAR_INST
R_TRIG:R_TRIG;
END_VAR

VAR

VAR
i:DINT;
END_VAR

PRGORAM

R_TRIG(CLK:=bStart);
IF R_TRIG.Q THEN
FOR i:=ArrayLower TO ArrayHigher DO
io_logData[i].Status:=0;
io_logData[i].timeStamp:=’ ‘;
END_FOR
END_IF

METHOD datalog : BOOL

状態を保存とシフトするMethodです。0番目は常に最新Logです。

VAR_INPUT

VAR_INPUT
bStart:BOOL;
END_VAR

VAR_INST

VAR_INST
R_TRIG:R_TRIG;
END_VAR

VAR

VAR
i:DINT:=0;
END_VAR

PROGRAM

こちらのプログラムは自動でDUTのサイズとシフトするByte数を計算します。その結果は次はのMEMCPY のパラメタnになります。MEMCPYはつまりMemory Copyそのもので、三菱のBMOVEのような関数です。

0番目は常に最新Logです。

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

配列のUpperを取得するproperty です。

GET

ArrayHigher:=UPPER_BOUND(io_logData,1);

PROPERTY ArrayLower : DINT

配列のLowerを取得するproperty です。

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

この画面ではBeckhoffのTE1800使用しLogとLED現在状態を表示できます。

M5Core2 Side

最後はM5Core2側ですね。以下のLinkでM5StackとW5500 Lan ModuleでMQTT通信実装するTutorialを書いています。

M5Stack#LAN W5500 with MQTT

Code

こちらのSample CodeがM5StackとW5500 LanモジュールでMQTT Topic Subscribeし、RoboDK StationのLED Townerと同じ状態が表示されるようにします。

#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.211\n”);
M5.Lcd.printf(“Mac:90-A2-DA-x0F-08-E1\n”);

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.”);
}
}