File content as of revision 2:4c2c7c8d608b:

/*

This code implements MQTT broker for Smart Industry system.
The system sorts plastic caps based on their color (red, green or blue). 
Proximity sensors detect the presence of the cup, color sensor determines the color and
servo motors drive the sorting mechanism.

Faculty of Electrical Engineering, University of Belgrade.
Version (1), July 2022.

*/

// Include the libraries

#include "mbed.h"
#include "mb_pins.h"
#include "platform/mbed_thread.h"
#include "MQTTClientMbedOs.h"
#include "SSD1308.h"
#include "Adafruit_GFX.h"
#include "Adafruit_GFX_Config.h"
#include "Adafruit_SSD1306.h"

// I2C bus pins
#define D_SDA                  PB_14 
#define D_SCL                  PB_13
     
// I2C address, 60d or 0x3c
#define I2C_REAL_ADD                                                        0x3c
#define I2C_ADDRESS                                            I2C_REAL_ADD << 1
#define I2C_FREQUENCY                                                     400000

// Client yield timeout in miliseconds
#define YIELD_TIMEOUT_MS                                                    1000

// Maximum number of networks to scan for
#define MAX_NETWORKS                                                          10

// Small delay for network information printing
#define PRINTF_DELAY_MS                                                       10

// Set OLED width and heigth [pixel]
#define OLED_WIDTH_PX                                                        128
#define OLED_HEIGHT_PX                                                        64

// Initialize I2C for OLED display
I2C i2c(D_SDA, D_SCL);

// Initialize OLED
Adafruit_SSD1306_I2c myOled(i2c,PB_5,I2C_ADDRESS,OLED_HEIGHT_PX,OLED_WIDTH_PX);

// Variables that count the number of caps for every color
int red;
int green;
int blue;

// Variables used in broker decision algorithm
int flagColor = 0;
int flagProx1 = 0;
int flagProx2 = 0;
int flagStage1 = 1;
int flagStage2 = 1;
int notAvailable = 0;
int poximityTreshold1 = 300;
int poximityTreshold2 = 280;
int currentColor = 0;

// Define start button
DigitalIn buttonStart(MB_SW1); 
int flagStart = 0;

// Pointer to a WiFi network object
WiFiInterface *wifi;

// Creating TCP socket
TCPSocket socket;

// Creating MQTT client using the TCP socket
MQTTClient client(&socket);

// Message handler
MQTT::Message message;

// MQTT topics
char* topic_pub_servo1 = "PMK_industry/micro/servo1";
char* topic_pub_servo2 = "PMK_industry/micro/servo2";
char* topic_sub_color = "PMK_industry/micro/color";
char* topic_sub_proximity1 = "PMK_industry/micro/proximity1";
char* topic_sub_proximity2 = "PMK_industry/micro/proximity2";

char* receavedMessage;

// Counter of arrived messages
int arrivedcount = 0;

// HiveMQ broker connectivity information
const char* hostname = "broker.hivemq.com";
int port = 1883;

// Returning a string for a provided network encryption
const char *sec2str(nsapi_security_t sec)
{
    switch (sec) 
    {
        case NSAPI_SECURITY_NONE:
            return "None";
        case NSAPI_SECURITY_WEP:
            return "WEP";
        case NSAPI_SECURITY_WPA:
            return "WPA";
        case NSAPI_SECURITY_WPA2:
            return "WPA2";
        case NSAPI_SECURITY_WPA_WPA2:
            return "WPA/WPA2";
        case NSAPI_SECURITY_UNKNOWN:
        default:
            return "Unknown";
    }
}

// Scan available WiFi networks
int scan_networks(WiFiInterface *wifi)
{
    printf("Scan:\n");
    
    // Scan only for the number of networks, first parameter is NULL
    int count = wifi->scan(NULL, 0);
    
    // If there are no networks, count == 0, if there is an error, counter < 0:
    if (count <= 0)
    {
        printf("scan() failed with return value: %d\n", count);
        return 0;
    }

    // Limit number of network arbitrary to some reasonable number
    count = count < MAX_NETWORKS ? count : MAX_NETWORKS;
    
    // Create a local pointer to an object, which is an array of WiFi APs
    WiFiAccessPoint *ap = new WiFiAccessPoint[count];
    
    // Now scan again for 'count' networks and populate the array of APs
    count = wifi->scan(ap, count);
    
    // This time, the number of entries to 'ap' is returned
    if (count <= 0) 
    {
        printf("scan() failed with return value: %d\n", count);
        return 0;
    }
    
    // Print out the parameters of each AP
    for (int i = 0; i < count; i++) 
    {
        printf("Network: %s secured: %s BSSID: %hhX:%hhX:%hhX:%hhx:%hhx:%hhx RSSI: %hhd Ch: %hhd\n", ap[i].get_ssid(),
               sec2str(ap[i].get_security()), ap[i].get_bssid()[0], ap[i].get_bssid()[1], ap[i].get_bssid()[2],
               ap[i].get_bssid()[3], ap[i].get_bssid()[4], ap[i].get_bssid()[5], ap[i].get_rssi(), ap[i].get_channel());
        thread_sleep_for(PRINTF_DELAY_MS);
    }
    printf("%d networks available.\n", count);
    
    // Since 'ap' is dynamically allocated pointer to the array of objects, it
    // needs to be deleted:
    delete[] ap;
    return count;
}

// MQTT message handlers for certain topics
void messageArrivedColor(MQTT::MessageData& md)
{
    MQTT::Message &message = md.message;
    receavedMessage = (char*)message.payload;
    
    if (strcmp(receavedMessage,"Red") == 0 ){ flagColor = 1; }
    else if (strcmp(receavedMessage,"Green") == 0 ) { flagColor = 2; }
    else if (strcmp(receavedMessage, "Blue") == 0 ) { flagColor = 3; }
    printf("Color: %.*s\r\n", message.payloadlen, (char*)message.payload);
    
}

void messageArrivedProximity1(MQTT::MessageData& md)
{
    MQTT::Message &message = md.message;
    int receavedMessage = atoi((char*)message.payload);

    if (receavedMessage > poximityTreshold1){ flagProx1 = 1;}
    else { flagProx1 = 0;}
    printf("Prox1 = %d\t", receavedMessage);
    printf("flag = %d\r\n", flagProx1);
    
}

void messageArrivedProximity2(MQTT::MessageData& md)
{
    MQTT::Message &message = md.message;
    int receavedMessage = atoi((char*)message.payload);
    
    if (receavedMessage > poximityTreshold2){ flagProx2 = 1;}
    else { flagProx2 = 0;}
    printf("Prox2 = %d\t", receavedMessage);
    printf("flag = %d\r\n", flagProx2);
    
}

int main()
{
    // Create a default network interface:
    wifi = WiFiInterface::get_default_instance();
    if (!wifi) {
        printf("ERROR: No WiFiInterface found.\n");
        return -1;
    }
    
    // Scan for available networks and aquire information about Access Points:
    int count = scan_networks(wifi);
    if (count == 0) {
        printf("No WIFI APs found - can't continue further.\n");
        return -1;
    }
    
    // Connect to the network with the parameters specified in 'mbed_app.json':
    printf("\nConnecting to %s...\n", MBED_CONF_APP_WIFI_SSID);
    int ret = wifi->connect(MBED_CONF_APP_WIFI_SSID, MBED_CONF_APP_WIFI_PASSWORD, NSAPI_SECURITY_WPA_WPA2);
    if (ret != 0) {
        printf("\nConnection error: %d\n", ret);
        return -1;
    }
    
    // Print out the information aquired:
    printf("Success\n\n");
    printf("MAC: %s\n", wifi->get_mac_address());
    printf("IP: %s\n", wifi->get_ip_address());
    printf("Netmask: %s\n", wifi->get_netmask());
    printf("Gateway: %s\n", wifi->get_gateway());
    printf("RSSI: %d\n\n", wifi->get_rssi());   
    
    // Open TCP socket using WiFi network interface:
    socket.open(wifi);
    
    // Connect to the HiveMQ broker:
    socket.connect(hostname, port);
    
    // Fill connect data with default values:
    MQTTPacket_connectData data = MQTTPacket_connectData_initializer;
    
    // Change only ID and protocol version:
    data.MQTTVersion = 3;
    data.clientID.cstring = "Broker";
    
    // Connect to the mqtt broker
    int rc = 0;
    if ((rc = client.connect(data)) != 0)
        printf("rc from MQTT connect is %d\r\n", rc);

    // Subscribe to topics
    if ((rc = client.subscribe(topic_sub_color, MQTT::QOS2, messageArrivedColor)) != 0)
        printf("rc from MQTT subscribe is %d\r\n", rc);
        
    if ((rc = client.subscribe(topic_sub_proximity1, MQTT::QOS2, messageArrivedProximity1)) != 0)
        printf("rc from MQTT subscribe is %d\r\n", rc);
        
    if ((rc = client.subscribe(topic_sub_proximity2, MQTT::QOS2, messageArrivedProximity2)) != 0)
        printf("rc from MQTT subscribe is %d\r\n", rc);
    
    red = 0;
    green = 0;
    blue = 0;
    
    char bufRed[15];   
    char bufGreen[15];
    char bufBlue[15];
    
    myOled.begin(); 
    myOled.setTextSize(2);
    myOled.setTextColor(WHITE);
    myOled.setTextCursor(25,0);
    myOled.printf("Press");
    myOled.setTextCursor(25,25);
    myOled.printf("START");
    myOled.display();
 
    while (true) {
        
        if (!flagStart){
        flagStart = !buttonStart;
        thread_sleep_for(50);
        
        }
        
        if (flagStart) {
        
        sprintf(bufRed, "%s %d", "Red:", red);
        sprintf(bufGreen, "%s %d", "Green:", green);
        sprintf(bufBlue, "%s %d", "Blue:", blue);
        
        myOled.clearDisplay();
        myOled.setTextSize(2);
        myOled.setTextColor(WHITE);
        myOled.setTextCursor(0,0);
        myOled.printf(bufRed);
        myOled.setTextCursor(0,25);
        myOled.printf(bufGreen);
        myOled.setTextCursor(0,50);
        myOled.printf(bufBlue);
        myOled.display();
        
        // If a cap is detected at the first stage, send a command
        // for servo1 based on the color of the cap
        if(flagStage1 && flagProx1)
        {
            
            if (flagColor == 1)
            {
                char buf[100];
                sprintf(buf, "right");
                message.qos = MQTT::QOS0;
                message.retained = false;
                message.dup = false;
                message.payload = (void*)buf;
                message.payloadlen = strlen(buf)+1;
                client.publish(topic_pub_servo1, message);
                flagStage1 = 0;
                red++;
                
                }
                
                else if (flagColor == 2)
            {
                char buf[100];
                sprintf(buf, "left");
                message.qos = MQTT::QOS0;
                message.retained = false;
                message.dup = false;
                message.payload = (void*)buf;
                message.payloadlen = strlen(buf)+1;
                client.publish(topic_pub_servo1, message);
                flagStage1 = 0;
                green++;
                
                }
                
                else if (flagColor == 3)
            {
                char buf[100];
                sprintf(buf, "left");
                message.qos = MQTT::QOS0;
                message.retained = false;
                message.dup = false;
                message.payload = (void*)buf;
                message.payloadlen = strlen(buf)+1;
                client.publish(topic_pub_servo1, message);
                flagStage1 = 0;
                blue++;
                
                }
                
                currentColor = flagColor;
            }
        
            else if(!flagStage1 && !flagProx1)
            {
                char buf[100];
                sprintf(buf, "zero");
                message.qos = MQTT::QOS0;
                message.retained = false;
                message.dup = false;
                message.payload = (void*)buf;
                message.payloadlen = strlen(buf)+1;
                client.publish(topic_pub_servo1, message);
                flagStage1 = 1;
                
                }
        
        // If a cap is detected at the second stage, send a command
        // for servo2 based on the color of the cap
        if(flagStage2 && flagProx2)
        {
            
            if (currentColor == 2)
            {
                char buf[100];
                sprintf(buf, "right");
                message.qos = MQTT::QOS0;
                message.retained = false;
                message.dup = false;
                message.payload = (void*)buf;
                message.payloadlen = strlen(buf)+1;
                client.publish(topic_pub_servo2, message);
                flagStage2 = 0;
                
                }
                
                else if (currentColor  == 3)
            {
                char buf[100];
                sprintf(buf, "left");
                message.qos = MQTT::QOS0;
                message.retained = false;
                message.dup = false;
                message.payload = (void*)buf;
                message.payloadlen = strlen(buf)+1;
                client.publish(topic_pub_servo2, message);
                flagStage2 = 0;
                
                }
                
            }
        
            else if(!flagStage2 && !flagProx2)
            {
                char buf[100];
                sprintf(buf, "zero");
                message.qos = MQTT::QOS0;
                message.retained = false;
                message.dup = false;
                message.payload = (void*)buf;
                message.payloadlen = strlen(buf)+1;
                client.publish(topic_pub_servo2, message);
                flagStage2 = 1;
                currentColor = 0;
                
                }
                
        client.yield(YIELD_TIMEOUT_MS); // Need to call yield API to maintain connection
    }
    }
}