5.2 MQTT Robot CiRA

//////// ผนวกไลบรารี่ ////////////

#include <Arduino.h>

#include<ArduinoJson.h>

#include <WiFi.h>

#include <PubSubClient.h>

#include <ESP32Servo.h>

#include <Wire.h>

#include <SPI.h>

#include <Adafruit_GFX.h>

#include <Adafruit_SSD1306.h>

Adafruit_SSD1306 OLED(-1);

// Update these with values suitable for your network.

const char* ssid = "BJHome2G";

const char* password = "123456789";

// Config MQTT Server

#define mqtt_server "192.168.1.15"

#define mqtt_port 1883

#define mqtt_user "user"

#define mqtt_password "123456"

WiFiClient espClient;

PubSubClient client(espClient);

//////////Sound buzzer์///////////

int snd = 18;

//////// set io ///////////////////

int button = 27; ///button

int k_nob = 35 ; // K-nob

#define LED_PIN_A 5

#define LED_PIN_B 19

#define LED_PIN_C 23

int ss1 = 32; // servo ch1

int ss2 = 33; // servo ch2

int ss3 = 34; // servo ch3

Servo sv1;

Servo sv2;

Servo sv3;

///////////////////////////////////////////

////////// variable motor port/////////////

#define DR1 16

#define DR2 17

#define PWMR 4

/////////////////////////////////

#define DL1 2

#define DL2 15

#define PWML 13

/////////////////////////////////

void setup() {

OLED.begin(SSD1306_SWITCHCAPVCC, 0x3C); // address i2c 0x3C (for the 128x64)

pinMode(button , INPUT);

//////// Motor OUTPUT /////////

pinMode(DL1, OUTPUT);

pinMode(DL2, OUTPUT);

pinMode(PWML, OUTPUT);

pinMode(DR1, OUTPUT);

pinMode(DR2, OUTPUT);

pinMode(PWMR, OUTPUT);

////// Servo set ////////////

sv1.attach(ss1);

sv2.attach(ss2);

sv3.attach(ss3);

sv_set();

tone(snd, 800, 100); tone(snd, 1000, 100); tone(snd, 800, 100);

}

void run(int spl, int spr) // ประกาศฟังก์ชัน run(กำลังมอเตอร์ซ้าาย,กำลังมอเตอร์ขวา);

{

if (spl > 0)

{

digitalWrite(DL1, LOW);

digitalWrite(DL2, HIGH);

analogWrite(PWML, spl);

}

else if (spl < 0)

{

digitalWrite(DL1, HIGH);

digitalWrite(DL2, LOW);

analogWrite(PWML, -spl);

}

else

{

digitalWrite(DL1, HIGH);

digitalWrite(DL2, HIGH);

}

//////////////////////////////////////

if (spr > 0)

{

digitalWrite(DR1, LOW);

digitalWrite(DR2, HIGH);

analogWrite(PWMR, spr);

}

else if (spr < 0)

{

digitalWrite(DR1, HIGH);

digitalWrite(DR2, LOW);

analogWrite(PWMR, -spr);

}

else

{

digitalWrite(DR1, HIGH);

digitalWrite(DR2, HIGH);

}

}

void loop() {

int sw = digitalRead(button); // set sw = button

int nob = analogRead(k_nob); // variable nob = k_nob

int menu = map(nob, 0, 4095, 0, 12); // mab nob 0-4095 to 0-12

OLED.clearDisplay(); //clear oled

OLED.setTextColor(WHITE, BLACK); //set color

OLED.setCursor(0, 0); // set cursor position

OLED.setTextSize(2); // set text size 2 point

OLED.print(" "); // space text

OLED.println(menu); // display menu 0 to 12

OLED.setTextSize(1); // set text size 1 point

OLED.println(" LOTUS ARDUIBOT"); // display LOTUS ARDUIBOT

OLED.print(" "); // space text

OLED.print(nob); // display nob

OLED.println(" TEST"); // display TEST

OLED.setTextSize(2); // set text size 2 point

///////////////////////////////////////////////////////////////

if ((menu == 0))

{

}

if ((menu == 1))

{ OLED.println(" SERVO123");

}

if ((menu == 2))

{ OLED.println(" MOTOR");

}

if ((menu == 3))

{

OLED.println(" SOUND");

}

if ((menu == 4))

{

OLED.println(" MQTT");

}

if ((menu == 5))

{

}

if ((sw == 0) and (menu == 6))

{

}

if ((sw == 0) and (menu == 7))

{

}

if ((sw == 0) and (menu == 8))

{

}

if ((sw == 0) and (menu == 9))

{

}

if ((sw == 0) and (menu == 10))

{

}

if ((sw == 0) and (menu == 11))

{

}

if ((sw == 0) and (menu == 12))

{

}

//////////////////////////////////////////////////////////////

OLED.display();

if ((sw == 0) and (menu == 0))

{ tone(snd, 800, 100);

}

if ((sw == 0) and (menu == 1))

{

tone(snd, 800, 100); menu1();

}

if ((sw == 0) and (menu == 2))

{

tone(snd, 800, 100); menu2();

}

if ((sw == 0) and (menu == 3))

{

tone(snd, 800, 100); menu3();

}

if ((sw == 0) and (menu == 4))

{

tone(snd, 800, 100); menu4();

}

if ((sw == 0) and (menu == 5))

{

tone(snd, 800, 100); menu5();

}

if ((sw == 0) and (menu == 6))

{

tone(snd, 800, 100); menu6();

}

if ((sw == 0) and (menu == 7))

{

tone(snd, 800, 100); menu7();

}

if ((sw == 0) and (menu == 8))

{

tone(snd, 800, 100); menu8();

}

if ((sw == 0) and (menu == 9))

{

tone(snd, 800, 100); menu9();

}

if ((sw == 0) and (menu == 10))

{

tone(snd, 800, 100); menu10();

}

if ((sw == 0) and (menu == 11))

{

tone(snd, 800, 100); menu11();

}

if ((sw == 0) and (menu == 12))

{

tone(snd, 800, 100); menu12();

}

delay(100);

}

void motor()

{ //-------------- forward-------------- //

OLED.clearDisplay();

OLED.setTextColor(WHITE, BLACK);

OLED.setCursor(0, 0);

OLED.setTextSize(2);

OLED.println(" ML MR");

OLED.println(" F F");

OLED.setTextSize(1);

OLED.println(" -------------------");

OLED.setTextSize(2);

OLED.println(" FORWARD");

OLED.display();

run(100, 100); delay(1000); tone(3, 1000, 500);

//-------------- back ward-------------- //

OLED.clearDisplay();

OLED.setTextColor(WHITE, BLACK);

OLED.setCursor(0, 0);

OLED.setTextSize(2);

OLED.println(" ML MR");

OLED.println(" B B");

OLED.setTextSize(1);

OLED.println(" -------------------");

OLED.setTextSize(2);

OLED.println(" BACK");

OLED.display();

run(-100, -100); delay(2000); tone(3, 1000, 500);

//------------------ trun left-----------------------------//

OLED.clearDisplay();

OLED.setTextColor(WHITE, BLACK);

OLED.setCursor(0, 0);

OLED.setTextSize(2);

OLED.println(" ML MR");

OLED.println(" B F");

OLED.setTextSize(1);

OLED.println(" -------------------");

OLED.setTextSize(2);

OLED.println(" LEFT");

OLED.display();

run(-100, 100); delay(2000); tone(3, 1000, 500);

//------------------turn right-----------------------------//

OLED.clearDisplay();

OLED.setTextColor(WHITE, BLACK);

OLED.setCursor(0, 0);

OLED.setTextSize(2);

OLED.println(" ML MR");

OLED.println(" F B");

OLED.setTextSize(1);

OLED.println(" -------------------");

OLED.setTextSize(2);

OLED.println(" RIGHT");

OLED.display();

run(100, -100); delay(2000); tone(3, 1000, 500);

//------------------- stop motor----------------------------//

OLED.clearDisplay();

OLED.setTextColor(WHITE, BLACK);

OLED.setCursor(0, 0);

OLED.setTextSize(2);

OLED.println(" ML MR");

OLED.println(" 0 0");

OLED.setTextSize(1);

OLED.println(" -------------------");

OLED.setTextSize(2);

OLED.println(" STOP");

OLED.display();

run(0, 0); delay(2000); tone(3, 1000, 500);

}

void sv_knob() {

while (true) {

int vr = analogRead(A7);

int nob = map(vr, 0, 4095, 0, 180);

OLED.clearDisplay();

OLED.setTextColor(WHITE, BLACK);

OLED.setCursor(0, 0);

OLED.setTextSize(2);

OLED.print("SV1 = ");

OLED.println(nob);

OLED.print("SV2 = ");

OLED.println(nob);

OLED.print("SV3 = ");

OLED.println(nob);

OLED.display();

sv1.write(nob);

sv2.write(nob);

sv3.write(nob);

delay(50);

}

}

/////////////////////////////////////////////////////

void sv_set(){ sv1.write(40);}

////////////////////////////////////////////////////

void sound() {

tone(snd, 660, 100);

delay(150);

tone(snd, 660, 100);

delay(300);

tone(snd, 660, 100);

delay(300);

tone(snd, 510, 100);

delay(100);

tone(snd, 660, 100);

delay(300);

tone(snd, 770, 100);

delay(550);

tone(snd, 380, 100);

delay(575);

tone(snd, 510, 100);

delay(450);

tone(snd, 380, 100);

delay(400);

tone(snd, 320, 100);

delay(500);

tone(snd, 440, 100);

delay(300);

tone(snd, 480, 80);

delay(330);

tone(snd, 450, 100);

delay(150);

tone(snd, 430, 100);

delay(300);

tone(snd, 380, 100);

delay(200);

tone(snd, 660, 80);

delay(200);

tone(snd, 760, 50);

delay(150);

tone(snd, 860, 100);

delay(300);

tone(snd, 700, 80);

delay(150);

tone(snd, 760, 50);

delay(350);

tone(snd, 660, 80);

delay(300);

tone(snd, 520, 80);

delay(150);

tone(snd, 580, 80);

delay(150);

tone(snd, 480, 80);

delay(500);

tone(snd, 510, 100);

delay(450);

tone(snd, 380, 100);

delay(400);

tone(snd, 320, 100);

delay(500);

tone(snd, 440, 100);

delay(300);

tone(snd, 480, 80);

delay(330);

tone(snd, 450, 100);

delay(150);

tone(snd, 430, 100);

delay(300);

tone(snd, 380, 100);

delay(200);

tone(snd, 660, 80);

delay(200);

tone(snd, 760, 50);

delay(150);

tone(snd, 860, 100);

delay(300);

tone(snd, 700, 80);

delay(150);

tone(snd, 760, 50);

delay(350);

tone(snd, 660, 80);

delay(300);

tone(snd, 520, 80);

delay(150);

tone(snd, 580, 80);

delay(150);

tone(snd, 480, 80);

delay(500);

tone(snd, 500, 100);

delay(300);

tone(snd, 760, 100);

delay(100);

tone(snd, 720, 100);

delay(150);

tone(snd, 680, 100);

delay(150);

tone(snd, 620, 150);

delay(300);

tone(snd, 650, 150);

delay(300);

tone(snd, 380, 100);

delay(150);

tone(snd, 430, 100);

delay(150);

tone(snd, 500, 100);

delay(300);

tone(snd, 430, 100);

delay(150);

tone(snd, 500, 100);

delay(100);

tone(snd, 570, 100);

delay(220);

tone(snd, 500, 100);

delay(300);

tone(snd, 760, 100);

delay(100);

tone(snd, 720, 100);

delay(150);

tone(snd, 680, 100);

delay(150);

tone(snd, 620, 150);

delay(300);

tone(snd, 650, 200);

delay(300);

tone(snd, 1020, 80);

delay(300);

tone(snd, 1020, 80);

delay(150);

tone(snd, 1020, 80);

delay(300);

tone(snd, 380, 100);

delay(300);

tone(snd, 500, 100);

delay(300);

tone(snd, 760, 100);

delay(100);

tone(snd, 720, 100);

delay(150);

tone(snd, 680, 100);

delay(150);

tone(snd, 620, 150);

delay(300);

tone(snd, 650, 150);

delay(300);

tone(snd, 380, 100);

delay(150);

tone(snd, 430, 100);

delay(150);

tone(snd, 500, 100);

delay(300);

tone(snd, 430, 100);

delay(150);

tone(snd, 500, 100);

delay(100);

tone(snd, 570, 100);

delay(420);

tone(snd, 585, 100);

delay(450);

tone(snd, 550, 100);

delay(420);

tone(snd, 500, 100);

delay(360);

tone(snd, 380, 100);

delay(300);

tone(snd, 500, 100);

delay(300);

tone(snd, 500, 100);

delay(150);

tone(snd, 500, 100);

delay(300);

tone(snd, 500, 100);

delay(300);

tone(snd, 760, 100);

delay(100);

tone(snd, 720, 100);

delay(150);

tone(snd, 680, 100);

delay(150);

tone(snd, 620, 150);

delay(300);

tone(snd, 650, 150);

delay(300);

tone(snd, 380, 100);

delay(150);

tone(snd, 430, 100);

delay(150);

tone(snd, 500, 100);

delay(300);

tone(snd, 430, 100);

delay(150);

tone(snd, 500, 100);

delay(100);

tone(snd, 570, 100);

delay(220);

tone(snd, 500, 100);

delay(300);

tone(snd, 760, 100);

delay(100);

tone(snd, 720, 100);

delay(150);

tone(snd, 680, 100);

delay(150);

tone(snd, 620, 150);

delay(300);

tone(snd, 650, 200);

delay(300);

tone(snd, 1020, 80);

delay(300);

tone(snd, 1020, 80);

delay(150);

tone(snd, 1020, 80);

delay(300);

tone(snd, 380, 100);

delay(300);

tone(snd, 500, 100);

delay(300);

tone(snd, 760, 100);

delay(100);

tone(snd, 720, 100);

delay(150);

tone(snd, 680, 100);

delay(150);

tone(snd, 620, 150);

delay(300);

tone(snd, 650, 150);

delay(300);

tone(snd, 380, 100);

delay(150);

tone(snd, 430, 100);

delay(150);

tone(snd, 500, 100);

delay(300);

tone(snd, 430, 100);

delay(150);

tone(snd, 500, 100);

delay(100);

tone(snd, 570, 100);

delay(420);

tone(snd, 585, 100);

delay(450);

tone(snd, 550, 100);

delay(420);

tone(9, 500, 100);

delay(360);

tone(snd, 380, 100);

delay(300);

tone(snd, 500, 100);

delay(300);

tone(snd, 500, 100);

delay(150);

tone(snd, 500, 100);

delay(300);

tone(snd, 500, 60);

delay(150);

tone(snd, 500, 80);

delay(300);

tone(snd, 500, 60);

delay(350);

tone(snd, 500, 80);

delay(150);

tone(snd, 580, 80);

delay(350);

tone(snd, 660, 80);

delay(150);

tone(snd, 500, 80);

delay(300);

tone(snd, 430, 80);

delay(150);

tone(snd, 380, 80);

delay(600);

tone(snd, 500, 60);

delay(150);

tone(snd, 500, 80);

delay(300);

tone(snd, 500, 60);

delay(350);

tone(snd, 500, 80);

delay(150);

tone(snd, 580, 80);

delay(150);

tone(snd, 660, 80);

delay(550);

tone(snd, 870, 80);

delay(325);

tone(snd, 760, 80);

delay(600);

tone(snd, 500, 60);

delay(150);

tone(snd, 500, 80);

delay(300);

tone(snd, 500, 60);

delay(350);

tone(snd, 500, 80);

delay(150);

tone(snd, 580, 80);

delay(350);

tone(snd, 660, 80);

delay(150);

tone(snd, 500, 80);

delay(300);

tone(snd, 430, 80);

delay(150);

tone(snd, 380, 80);

delay(600);

tone(snd, 660, 100);

delay(150);

tone(snd, 660, 100);

delay(300);

tone(snd, 660, 100);

delay(300);

tone(snd, 510, 100);

delay(100);

tone(snd, 660, 100);

delay(300);

tone(snd, 770, 100);

delay(550);

tone(snd, 380, 100);

delay(575);

}

void mqtt_set(){

tone(snd, 800, 100); tone(snd, 900, 100); tone(snd, 800, 100);tone(snd, 900, 100);delay(50);tone(snd, 1000, 1000);

OLED.begin(SSD1306_SWITCHCAPVCC, 0x3C); // กำหนดแอดเดรสของพอร์ตจอเป็น 0x3C (for the 128x64)

pinMode(LED_PIN_A, OUTPUT);

pinMode(LED_PIN_B, OUTPUT);

pinMode(LED_PIN_C, OUTPUT);

Serial.begin(115200);

delay(10);

Serial.println();

Serial.print("Connecting to ");

Serial.println(ssid);

WiFi.begin(ssid, password);

digitalWrite(LED_PIN_A,HIGH);

digitalWrite(LED_PIN_B, HIGH);

digitalWrite(LED_PIN_C, HIGH);

delay(2000);

digitalWrite(LED_PIN_A, LOW);

digitalWrite(LED_PIN_B, LOW);

digitalWrite(LED_PIN_C, LOW);

OLED.clearDisplay();

OLED.setCursor(0, 0);

OLED.setTextColor(WHITE, BLACK);

OLED.setTextSize(2);

OLED.println(" WiFi");

OLED.println("Conecting");

OLED.display();

while (WiFi.status() != WL_CONNECTED) {

delay(500);

Serial.print(".");

}

Serial.println("");

Serial.println("WiFi connected");

Serial.println("IP address: ");

Serial.println(WiFi.localIP());

client.setServer(mqtt_server, mqtt_port);

client.setCallback(callback);

tone(snd, 1000, 100); delay(150); tone(snd, 1000, 100); delay(150); tone(snd, 1000, 100);

OLED.clearDisplay();

OLED.setTextColor(WHITE, BLACK);

OLED.setCursor(0, 0);

OLED.setTextSize(1);

OLED.println("WiFi connected");

OLED.print("IP : "); OLED.println( WiFi.localIP());

OLED.print("*******************");

OLED.display();

}

void mqtt_loop() {while(true){

if (!client.connected()) {

Serial.print("Attempting MQTT connection...");

if (client.connect("ESP32Client", mqtt_user, mqtt_password)) {

Serial.println("connected");

client.subscribe("ESP32"); // ESP32 Topic

} else {

Serial.print("failed, rc=");

Serial.print(client.state());

Serial.println(" try again in 5 seconds");

delay(5000);

return;

}

}

client.loop();

}}

void callback(char* topic, byte* payload, unsigned int length) {

Serial.print("Message arrived [");

Serial.print(topic);

Serial.print("] ");

String msg = "";

int i = 0;

while (i < length) msg += (char)payload[i++];

if (msg == "GET") {

client.publish("ESP32", (digitalRead(LED_PIN_A) ? "LEDON" : "LEDOFF"));

Serial.println("Send !");

return;

}

// CiRA CORE Mqtt //

const size_t capacity = JSON_OBJECT_SIZE(1); //? Object Size

DynamicJsonDocument doc(capacity); //? Declare variable for store json

deserializeJson(doc, payload, length); //? deserialize JSON

String msg_cira = doc["a"]; //? Store key title to tittle

////////////////////// command mqtt control LED A -C /////////////////////////////////

if((msg=="AON") or (msg_cira =="AON")) {digitalWrite(LED_PIN_A,HIGH);}

if((msg=="AOFF") or (msg_cira =="AOFF")){digitalWrite(LED_PIN_A,LOW);}

if((msg=="BON") or (msg_cira =="BON")){digitalWrite(LED_PIN_B,HIGH);}

if((msg=="BOFF") or (msg_cira =="BOFF")){digitalWrite(LED_PIN_B,LOW);}

if((msg=="CON") or (msg_cira =="CON")){digitalWrite(LED_PIN_C,HIGH);}

if((msg=="COFF") or (msg_cira =="COFF")){digitalWrite(LED_PIN_C,LOW);}

////////////////////// command mqtt control Motor /////////////////////////////////

if((msg=="fd") or (msg_cira =="fd")) {run(100,100);}

if((msg=="tl") or (msg_cira =="tl")) {run(0,100);}

if((msg=="tr") or (msg_cira =="tr")) {run(100,0);}

if((msg=="bk") or (msg_cira =="bk")) {run(-100,-100);}

if((msg=="ao") or (msg_cira =="ao")) {run(0,0);}

// digitalWrite(LED_PIN_A, (msg_cira == "LEDON" ? HIGH : LOW));

////////// Display OLED and Serial ////////////////

Serial.print("msg = "); Serial.println(msg);

Serial.print("msg_cira = "); Serial.println( msg_cira);

OLED.clearDisplay();

OLED.setTextColor(WHITE, BLACK);

OLED.setCursor(0, 0);

OLED.setTextSize(1);

OLED.println("WiFi connected");

OLED.print("IP : "); OLED.println( WiFi.localIP());

OLED.print("Topic : "); OLED.println( topic);

OLED.println("-------------------");

OLED.print("msg : "); OLED.println(msg);

OLED.println("-------------------");

OLED.print("msg_cira: "); OLED.println(msg_cira);

OLED.println("-------------------");

OLED.display();

}

//////////////write and use code menu ////////////

void menu1() /// write and use code menu 1

{

sv_knob(); // servo test

}

void menu2() /// write and use code menu

{ motor(); // dc motor test

}

void menu3() /// write and use code menu 3

{ sound(); // sound test

}

void menu4() /// write and use code menu 4

{mqtt_set();mqtt_loop();

}

void menu5() /// write and use code menu 5

{

}

void menu6() /// write and use code menu 6

{

}

void menu7() /// write and use code menu 7

{

}

void menu8() /// write and use code menu 8

{

}

void menu9() /// write and use code menu 9

{

}

void menu10() /// write and use code menu 10

{

}

void menu11() /// write and use code menu 11

{

}

void menu12() /// write and use code menu 12

{

}