Code: Select all
#include <dummy.h>
/***************************************************
Adafruit MQTT Library ESP8266 Example
Must use ESP8266 Arduino from:
https://github.com/esp8266/Arduino
Works great with Adafruit's Huzzah ESP board & Feather
----> https://www.adafruit.com/product/2471
----> https://www.adafruit.com/products/2821
Adafruit invests time and resources providing this open source code,
please support Adafruit and open-source hardware by purchasing
products from Adafruit!
Written by Tony DiCola for Adafruit Industries.
MIT license, all text above must be included in any redistribution
****************************************************/
#include <WiFi.h>
#include "Adafruit_MQTT.h"
#include "Adafruit_MQTT_Client.h"
/************************* WiFi Access Point *********************************/
#define WLAN_SSID "NS"
#define WLAN_PASS "Ru11"
/************************* Adafruit.io Setup *********************************/
#define AIO_SERVER "io.adafruit.com"
#define AIO_SERVERPORT 1883 // use 8883 for SSL
#define AIO_USERNAME "Venp"
#define AIO_KEY "aio_AdHL25k"
#define RPWM1 4
#define LPWM1 0
#define L_EN1 2
#define R_EN1 15
#define trigPin1 23
#define echoPin1 34
#define trigPin2 22
#define echoPin2 35
#define trigPin3 19
#define echoPin3 18
#define trigPin4 5
#define echoPin4 17
#define SOUND_VELOCITY 0.034
#define CM_TO_INCH 0.393701
long duration1;
long duration2;
long duration3;
long duration4;
int scan = 0;
long distanceCm1;
long distanceCm2;
long distanceCm3;
long distanceCm4;
long distance = 0;
int y = 0;
int loops = 1;
int a = 0;
int b = 0;
int c = 0;
int d = 0;
/************ Global State (you don't need to change this!) ******************/
// Create an ESP8266 WiFiClient class to connect to the MQTT server.
WiFiClient client;
// or... use WiFiFlientSecure for SSL
//WiFiClientSecure client;
// Setup the MQTT client class by passing in the WiFi client and MQTT server and login details.
Adafruit_MQTT_Client mqtt(&client, AIO_SERVER, AIO_SERVERPORT, AIO_USERNAME, AIO_KEY);
/****************************** Feeds ***************************************/
// Setup a feed called 'motorkanan' for publishing.
// Notice MQTT paths for AIO follow the form: <username>/feeds/<feedname>
//-------------------------------------------------------------
//Adafruit_MQTT_Publish motorkanan = Adafruit_MQTT_Publish(&mqtt, AIO_USERNAME "/feeds/motorkanan");
// Setup a feed called 'kiri' for subscribing to changes.
Adafruit_MQTT_Subscribe motorkanan = Adafruit_MQTT_Subscribe(&mqtt, AIO_USERNAME "/feeds/motorkanan");
Adafruit_MQTT_Subscribe motorkiri = Adafruit_MQTT_Subscribe(&mqtt, AIO_USERNAME "/feeds/motorkiri");
/*************************** Sketch Code ************************************/
// Bug workaround for Arduino 1.6.6, it seems to need a function declaration
// for some reason (only affects ESP8266, likely an arduino-builder bug).
void MQTT_connect();
void setup() {
pinMode(trigPin1, OUTPUT); // Sets the trigPin as an Output
pinMode(trigPin2, OUTPUT);
pinMode(trigPin3, OUTPUT);
pinMode(trigPin4, OUTPUT);
pinMode(echoPin1, INPUT); // Sets the echoPin as an Input
pinMode(echoPin2, INPUT);
pinMode(echoPin3, INPUT);
pinMode(echoPin4, INPUT);
pinMode(RPWM1,OUTPUT);
pinMode(LPWM1,OUTPUT);
pinMode(L_EN1,OUTPUT);
pinMode(R_EN1,OUTPUT);
digitalWrite(L_EN1, LOW);
digitalWrite(R_EN1, LOW);
digitalWrite(RPWM1, LOW);
digitalWrite(LPWM1, LOW);
scan = 0;
distance = 0;
loops = 1;
Serial.begin(115200);
delay(10);
Serial.println(F("Adafruit MQTT demo"));
// Connect to WiFi access point.
Serial.println(); Serial.println();
Serial.print("Connecting to ");
Serial.println(WLAN_SSID);
WiFi.begin(WLAN_SSID, WLAN_PASS);
while (WiFi.status() != WL_CONNECTED) {
delay(500);
Serial.print(".");
}
Serial.println();
Serial.println("WiFi connected");
Serial.println("IP address: "); Serial.println(WiFi.localIP());
// Setup MQTT subscription for onoff feed.
mqtt.subscribe(&motorkanan);
mqtt.subscribe(&motorkiri);
}
uint32_t x=0;
void loop() {
// Ensure the connection to the MQTT server is alive (this will make the first
// connection and automatically reconnect when disconnected). See the MQTT_connect
// function definition further below.
MQTT_connect();
// this is our 'wait for incoming subscription packets' busy subloop
// try to spend your time here
Adafruit_MQTT_Subscribe *subscription;
while ((subscription = mqtt.readSubscription(5000)))
{
// digitalWrite(trigPin1, LOW);
// delayMicroseconds(2);
// digitalWrite(trigPin1, HIGH);
// delayMicroseconds(10);
// digitalWrite(trigPin1, LOW);
// duration1 = pulseIn(echoPin1, HIGH);
// delay(10);
//
// digitalWrite(trigPin2, LOW);
// delayMicroseconds(2);
// digitalWrite(trigPin2, HIGH);
// delayMicroseconds(10);
// digitalWrite(trigPin2, LOW);
// duration2 = pulseIn(echoPin2, HIGH);
// delay(10);
//
// digitalWrite(trigPin3, LOW);
// delayMicroseconds(2);
// digitalWrite(trigPin3, HIGH);
// delayMicroseconds(10);
// digitalWrite(trigPin3, LOW);
// duration3 = pulseIn(echoPin3, HIGH);
// delay(10);
//
// digitalWrite(trigPin4, LOW);
// delayMicroseconds(2);
// digitalWrite(trigPin4, HIGH);
// delayMicroseconds(10);
// digitalWrite(trigPin4, LOW);
// duration4 = pulseIn(echoPin4, HIGH);
// delay(10);
//
// // Calculate the distance
// distanceCm1 = duration1 * SOUND_VELOCITY/2;
// distanceCm2 = duration2 * SOUND_VELOCITY/2;
// distanceCm3 = duration3 * SOUND_VELOCITY/2;
// distanceCm4 = duration4 * SOUND_VELOCITY/2;
//
// if (distanceCm1 <= 10)
// {
// distance = 1;
// scan = 0;
// }
// else if (distanceCm2 <= 10)
// {
// distance = 1;
// scan = 0;
// }
// else if (distanceCm3 <= 10)
// {
// distance = 1;
// scan = 0;
// }
// else if (distanceCm4 <= 10)
// {
// distance = 1;
// scan = 0;
// }
// else
// {
// distance = 0;
// scan = 0;
// }
// Serial.print("Distance1 (cm): ");
// Serial.println(distanceCm1);
// delay(10);
//
// Serial.print("Distance2 (cm): ");
// Serial.println(distanceCm2);
// delay(10);
//
// Serial.print("Distance3 (cm): ");
// Serial.println(distanceCm3);
// delay(10);
//
// Serial.print("Distance4 (cm): ");
// Serial.println(distanceCm4);
// delay(10);
//
// Serial.println("");
// delay(10);
if (subscription == &motorkanan) {
Serial.print(F("Got: "));
Serial.println((char *)motorkanan.lastread);
Serial.print("distance: ");
Serial.println(distance);
if (strcmp((char *)motorkanan.lastread, "1") == 0)
{
digitalWrite(trigPin1, LOW);
delayMicroseconds(2);
digitalWrite(trigPin1, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin1, LOW);
duration1 = pulseIn(echoPin1, HIGH);
delay(10);
distanceCm1 = duration1 * SOUND_VELOCITY/2;
Serial.print("jarak maju: ");
Serial.println(distanceCm1);
if (distanceCm1 <= 10)
{
digitalWrite(L_EN1, LOW);
digitalWrite(R_EN1, LOW);
digitalWrite(RPWM1, LOW);
digitalWrite(LPWM1, LOW);
scan = 0;
}
else if (distanceCm1 >= 11)
{
scan = 1;
while (scan == 1)
{
digitalWrite(RPWM1, HIGH);
digitalWrite(R_EN1, HIGH);
digitalWrite(L_EN1, LOW);
digitalWrite(LPWM1, LOW);
loops = 1;
digitalWrite(trigPin1, LOW);
delayMicroseconds(2);
digitalWrite(trigPin1, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin1, LOW);
duration1 = pulseIn(echoPin1, HIGH);
delay(10);
digitalWrite(trigPin2, LOW);
delayMicroseconds(2);
digitalWrite(trigPin2, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin2, LOW);
duration2 = pulseIn(echoPin2, HIGH);
delay(10);
digitalWrite(trigPin3, LOW);
delayMicroseconds(2);
digitalWrite(trigPin3, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin3, LOW);
duration3 = pulseIn(echoPin3, HIGH);
delay(10);
digitalWrite(trigPin4, LOW);
delayMicroseconds(2);
digitalWrite(trigPin4, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin4, LOW);
duration4 = pulseIn(echoPin4, HIGH);
delay(10);
// Calculate the distance
distanceCm1 = duration1 * SOUND_VELOCITY/2;
distanceCm2 = duration2 * SOUND_VELOCITY/2;
distanceCm3 = duration3 * SOUND_VELOCITY/2;
distanceCm4 = duration4 * SOUND_VELOCITY/2;
if (distanceCm1 <= 10)
{
digitalWrite(L_EN1, LOW);
digitalWrite(R_EN1, LOW);
digitalWrite(RPWM1, LOW);
digitalWrite(LPWM1, LOW);
Serial.println("STOPED !!!");
scan = 0;
}
else if (distanceCm2 <= 10)
{
digitalWrite(L_EN1, LOW);
digitalWrite(R_EN1, LOW);
digitalWrite(RPWM1, LOW);
digitalWrite(LPWM1, LOW);
Serial.println("STOPED !!!");
scan = 0;
}
else if (distanceCm3 <= 10)
{
digitalWrite(L_EN1, LOW);
digitalWrite(R_EN1, LOW);
digitalWrite(RPWM1, LOW);
digitalWrite(LPWM1, LOW);
Serial.println("STOPED !!!");
scan = 0;
}
else if (distanceCm4 <= 10)
{
digitalWrite(L_EN1, LOW);
digitalWrite(R_EN1, LOW);
digitalWrite(RPWM1, LOW);
digitalWrite(LPWM1, LOW);
Serial.println("STOPED !!!");
scan = 0;
}
// else
// {
// digitalWrite(L_EN1, LOW);
// digitalWrite(R_EN1, LOW);
// digitalWrite(RPWM1, LOW);
// digitalWrite(LPWM1, LOW);
// }
}
}
else
{
digitalWrite(L_EN1, LOW);
digitalWrite(R_EN1, LOW);
digitalWrite(RPWM1, LOW);
digitalWrite(LPWM1, LOW);
}
}
if (strcmp((char *)motorkanan.lastread, "2") == 0)
{
digitalWrite(trigPin2, LOW);
delayMicroseconds(2);
digitalWrite(trigPin2, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin2, LOW);
duration1 = pulseIn(echoPin2, HIGH);
delay(10);
distanceCm2 = duration2 * SOUND_VELOCITY/2;
Serial.print("jarak mundur: ");
Serial.println(distanceCm2);
if (distanceCm1 <= 10)
{
digitalWrite(L_EN1, LOW);
digitalWrite(R_EN1, LOW);
digitalWrite(RPWM1, LOW);
digitalWrite(LPWM1, LOW);
scan = 0;
}
else if (distanceCm1 >= 11)
{
scan = 1;
for (int scan =1; scan == 1; y++)
{
digitalWrite(LPWM1, HIGH);
digitalWrite(L_EN1, HIGH);
digitalWrite(RPWM1, LOW);
digitalWrite(R_EN1, LOW);
digitalWrite(trigPin1, LOW);
delayMicroseconds(2);
digitalWrite(trigPin1, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin1, LOW);
duration1 = pulseIn(echoPin1, HIGH);
delay(10);
digitalWrite(trigPin2, LOW);
delayMicroseconds(2);
digitalWrite(trigPin2, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin2, LOW);
duration2 = pulseIn(echoPin2, HIGH);
delay(10);
digitalWrite(trigPin3, LOW);
delayMicroseconds(2);
digitalWrite(trigPin3, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin3, LOW);
duration3 = pulseIn(echoPin3, HIGH);
delay(10);
digitalWrite(trigPin4, LOW);
delayMicroseconds(2);
digitalWrite(trigPin4, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin4, LOW);
duration4 = pulseIn(echoPin4, HIGH);
delay(10);
// Calculate the distance
distanceCm1 = duration1 * SOUND_VELOCITY/2;
distanceCm2 = duration2 * SOUND_VELOCITY/2;
distanceCm3 = duration3 * SOUND_VELOCITY/2;
distanceCm4 = duration4 * SOUND_VELOCITY/2;
if (distanceCm1 <= 10)
{
digitalWrite(L_EN1, LOW);
digitalWrite(R_EN1, LOW);
digitalWrite(RPWM1, LOW);
digitalWrite(LPWM1, LOW);
scan = 0;
}
else if (distanceCm2 <= 10)
{
digitalWrite(L_EN1, LOW);
digitalWrite(R_EN1, LOW);
digitalWrite(RPWM1, LOW);
digitalWrite(LPWM1, LOW);
scan = 0;
}
else if (distanceCm3 <= 10)
{
digitalWrite(L_EN1, LOW);
digitalWrite(R_EN1, LOW);
digitalWrite(RPWM1, LOW);
digitalWrite(LPWM1, LOW);
scan = 0;
}
else if (distanceCm4 <= 10)
{
digitalWrite(L_EN1, LOW);
digitalWrite(R_EN1, LOW);
digitalWrite(RPWM1, LOW);
digitalWrite(LPWM1, LOW);
scan = 0;
}
}
}
else
{
digitalWrite(L_EN1, LOW);
digitalWrite(R_EN1, LOW);
digitalWrite(RPWM1, LOW);
digitalWrite(LPWM1, LOW);
}
}
if (strcmp((char *)motorkanan.lastread, "3") == 0)
{
digitalWrite(trigPin3, LOW);
delayMicroseconds(2);
digitalWrite(trigPin3, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin3, LOW);
duration1 = pulseIn(echoPin3, HIGH);
delay(10);
distanceCm3 = duration3 * SOUND_VELOCITY/2;
Serial.print("jarak kanan: ");
Serial.println(distanceCm3);
if (distanceCm1 <= 10)
{
digitalWrite(L_EN1, LOW);
digitalWrite(R_EN1, LOW);
digitalWrite(RPWM1, LOW);
digitalWrite(LPWM1, LOW);
scan = 0;
}
else if (distanceCm1 >= 11)
{
scan = 1;
for (int scan =1; scan == 1; y++)
{
digitalWrite(L_EN1, HIGH);
digitalWrite(RPWM1, HIGH);
digitalWrite(R_EN1, LOW);
digitalWrite(LPWM1, LOW);
digitalWrite(trigPin1, LOW);
delayMicroseconds(2);
digitalWrite(trigPin1, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin1, LOW);
duration1 = pulseIn(echoPin1, HIGH);
delay(10);
digitalWrite(trigPin2, LOW);
delayMicroseconds(2);
digitalWrite(trigPin2, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin2, LOW);
duration2 = pulseIn(echoPin2, HIGH);
delay(10);
digitalWrite(trigPin3, LOW);
delayMicroseconds(2);
digitalWrite(trigPin3, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin3, LOW);
duration3 = pulseIn(echoPin3, HIGH);
delay(10);
digitalWrite(trigPin4, LOW);
delayMicroseconds(2);
digitalWrite(trigPin4, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin4, LOW);
duration4 = pulseIn(echoPin4, HIGH);
delay(10);
// Calculate the distance
distanceCm1 = duration1 * SOUND_VELOCITY/2;
distanceCm2 = duration2 * SOUND_VELOCITY/2;
distanceCm3 = duration3 * SOUND_VELOCITY/2;
distanceCm4 = duration4 * SOUND_VELOCITY/2;
if (distanceCm1 <= 10)
{
digitalWrite(L_EN1, LOW);
digitalWrite(R_EN1, LOW);
digitalWrite(RPWM1, LOW);
digitalWrite(LPWM1, LOW);
scan = 0;
}
else if (distanceCm2 <= 10)
{
digitalWrite(L_EN1, LOW);
digitalWrite(R_EN1, LOW);
digitalWrite(RPWM1, LOW);
digitalWrite(LPWM1, LOW);
scan = 0;
}
else if (distanceCm3 <= 10)
{
digitalWrite(L_EN1, LOW);
digitalWrite(R_EN1, LOW);
digitalWrite(RPWM1, LOW);
digitalWrite(LPWM1, LOW);
scan = 0;
}
else if (distanceCm4 <= 10)
{
digitalWrite(L_EN1, LOW);
digitalWrite(R_EN1, LOW);
digitalWrite(RPWM1, LOW);
digitalWrite(LPWM1, LOW);
scan = 0;
}
}
}
else
{
digitalWrite(L_EN1, LOW);
digitalWrite(R_EN1, LOW);
digitalWrite(RPWM1, LOW);
digitalWrite(LPWM1, LOW);
}
}
if (strcmp((char *)motorkanan.lastread, "4") == 0)
{
digitalWrite(trigPin4, LOW);
delayMicroseconds(2);
digitalWrite(trigPin4, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin4, LOW);
duration1 = pulseIn(echoPin4, HIGH);
delay(10);
distanceCm4 = duration4 * SOUND_VELOCITY/2;
Serial.print("jarak kiri: ");
Serial.println(distanceCm4);
if (distanceCm1 <= 10)
{
digitalWrite(L_EN1, LOW);
digitalWrite(R_EN1, LOW);
digitalWrite(RPWM1, LOW);
digitalWrite(LPWM1, LOW);
scan = 0;
}
else if (distanceCm1 >= 11)
{
scan = 1;
for (int scan =1; scan == 1; y++)
{
digitalWrite(R_EN1, HIGH);
digitalWrite(LPWM1, HIGH);
digitalWrite(L_EN1, LOW);
digitalWrite(RPWM1, LOW);
digitalWrite(trigPin1, LOW);
delayMicroseconds(2);
digitalWrite(trigPin1, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin1, LOW);
duration1 = pulseIn(echoPin1, HIGH);
delay(10);
digitalWrite(trigPin2, LOW);
delayMicroseconds(2);
digitalWrite(trigPin2, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin2, LOW);
duration2 = pulseIn(echoPin2, HIGH);
delay(10);
digitalWrite(trigPin3, LOW);
delayMicroseconds(2);
digitalWrite(trigPin3, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin3, LOW);
duration3 = pulseIn(echoPin3, HIGH);
delay(10);
digitalWrite(trigPin4, LOW);
delayMicroseconds(2);
digitalWrite(trigPin4, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin4, LOW);
duration4 = pulseIn(echoPin4, HIGH);
delay(10);
// Calculate the distance
distanceCm1 = duration1 * SOUND_VELOCITY/2;
distanceCm2 = duration2 * SOUND_VELOCITY/2;
distanceCm3 = duration3 * SOUND_VELOCITY/2;
distanceCm4 = duration4 * SOUND_VELOCITY/2;
if (distanceCm1 <= 10)
{
digitalWrite(L_EN1, LOW);
digitalWrite(R_EN1, LOW);
digitalWrite(RPWM1, LOW);
digitalWrite(LPWM1, LOW);
scan = 0;
}
else if (distanceCm2 <= 10)
{
digitalWrite(L_EN1, LOW);
digitalWrite(R_EN1, LOW);
digitalWrite(RPWM1, LOW);
digitalWrite(LPWM1, LOW);
scan = 0;
}
else if (distanceCm3 <= 10)
{
digitalWrite(L_EN1, LOW);
digitalWrite(R_EN1, LOW);
digitalWrite(RPWM1, LOW);
digitalWrite(LPWM1, LOW);
scan = 0;
}
else if (distanceCm4 <= 10)
{
digitalWrite(L_EN1, LOW);
digitalWrite(R_EN1, LOW);
digitalWrite(RPWM1, LOW);
digitalWrite(LPWM1, LOW);
scan = 0;
}
}
}
else
{
digitalWrite(L_EN1, LOW);
digitalWrite(R_EN1, LOW);
digitalWrite(RPWM1, LOW);
digitalWrite(LPWM1, LOW);
}
}
if (strcmp((char *)motorkanan.lastread, "0") == 0)
{
digitalWrite(L_EN1, LOW);
digitalWrite(R_EN1, LOW);
digitalWrite(RPWM1, LOW);
digitalWrite(LPWM1, LOW);
}
}
}
}
void MQTT_connect() {
int8_t ret;
// Stop if already connected.
if (mqtt.connected()) {
return;
}
Serial.print("Connecting to MQTT... ");
uint8_t retries = 3;
while ((ret = mqtt.connect()) != 0) { // connect will return 0 for connected
Serial.println(mqtt.connectErrorString(ret));
Serial.println("Retrying MQTT connection in 5 seconds...");
mqtt.disconnect();
delay(5000); // wait 5 seconds
retries--;
if (retries == 0) {
// basically die and wait for WDT to reset me
while (1);
}
}
Serial.println("MQTT Connected!");
}