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

{

}