File content as of revision 1:e73b7979a1a3:

/* mbed Microcontroller Library
 * Copyright (c) 2019 ARM Limited
 * SPDX-License-Identifier: Apache-2.0
 */

#include "mbed.h"
#include "platform/mbed_thread.h"
#include "Servo.h"
#include "color.h"
#include <string>
#include <MQTTClientMbedOs.h>
#include <algorithm>

#include "rohm-sensor-hal/rohm-sensor-hal/rohm_hal.h"       //mbed.h, types, DEBUG_print*
#include "rohm-sensor-hal/rohm-sensor-hal/I2CCommon.h"

#include "rohm-rpr0521/rohm-rpr0521/rpr0521_driver.h"
#include "rohm-rpr0521/rohm-rpr0521/rpr0521.h"

// Blinking rate in milliseconds
#define BLINKING_RATE_MS                                                    500
Serial pc(USBTX,USBRX);

Timeout T2;
void interruptT2(){
    printf("error2");
    }

//varijable boje
float r1[3]={1 ,109.0/399.0,133.0/399.0};
float g1[3]={206.0/411.0,399.0/411.0,1};
float b1[3]={120.0/490.0 ,209.0/490.0, 1};
DigitalOut colorLed(D4);
DigitalOut colorVout(D5);
int salji_boju=0;
int fleg_start=0;
//varijable za servo
Servo servo2(D3);
int fleg_proxy1=0;
InterruptIn button(USER_BUTTON);
int arrivedcount = 0;
TCPSocket socket;
MQTTClient client(&socket);
MQTT::Message message;
int button_pressed=0;
char* topic_pub1 = "PMK_industrija/micro1/proxy1";
char* topic_pub4 = "PMK_industrija/micro1/proxy2";
char* topic_pub3 = "PMK_industrija/micro1/color2";
char* topic_pub2 = "PMK_industrija/micro1/echo/3";
char* topic_sub = "PMK_industrija/micro3/#";
int fleg_proxy2=0;

WiFiInterface *wifi;
volatile int mems_event = 0;
uint32_t previous_tick = 0;
uint32_t current_tick = 0;
uint8_t high = 0, low = 0;
float temperature = 0.0f;
char buffer[32];
static char *print_double(char *str, double v, int decimalDigits = 2)
{
    int i = 1;
    int intPart, fractPart;
    int len;
    char *ptr;
 
    /* prepare decimal digits multiplicator */
    for (; decimalDigits != 0; i *= 10, decimalDigits--);
 
    /* calculate integer & fractinal parts */
    intPart = (int)v;
    fractPart = (int)((v - (double)(int)v) * i);
 
    /* fill in integer part */
    sprintf(str, "%i.", intPart);
 
    /* prepare fill in of fractional part */
    len = strlen(str);
    ptr = &str[len];
 
    /* fill in leading fractional zeros */
    for (i /= 10; i > 1; i /= 10, ptr++) {
        if (fractPart >= i) {
            break;
        }
        *ptr = '0';
    }
 
    /* fill in (rest of) fractional part */
    sprintf(ptr, "%i", fractPart);
 
    return str;
}

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";
    }
}

int scan_demo(WiFiInterface *wifi)
{
    WiFiAccessPoint *ap;
    printf("Scan:\n");
    int count = wifi->scan(NULL,0);
    if (count <= 0) {
        printf("scan() failed with return value: %d\n", count);
        return 0;
    }
    /* Limit number of network arbitrary to 15 */
    count = count < 15 ? count : 15;
    ap = new WiFiAccessPoint[count];
    count = wifi->scan(ap, count);
    if (count <= 0) {
        printf("scan() failed with return value: %d\n", count);
        return 0;
    }
    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());
    }
    printf("%d networks available.\n", count);
    delete[] ap;
    return count;
}

void messageArrived(MQTT::MessageData& md)
{   char buf[100];
    MQTTString &topic = md.topicName;
    string topic_name = topic.lenstring.data;
    printf("Topic name %d : %s\r\n",topic.lenstring.len,topic_name.c_str() );
    MQTT::Message &message = md.message;
    printf("Message arrived: qos %d, retained %d, dup %d, packetid %d\r\n", message.qos, message.retained, message.dup, message.id);
    printf("Payload %.*s\r\n", message.payloadlen, (char*)message.payload);
    string poruka = (char*)message.payload;
    if(topic_name.find("servo2")!=std::string::npos) {
         printf("AKTIVIRAN SERVO2\n"); 
         T2.detach();
         for(float p=0; p<1.0; p += 0.1) {
        servo2 = p;
        
        }
        wait(2);
        for(float p=1.0; p>0.0; p -= 0.1) {
        servo2 = p;
        
        }
        sprintf(buf, "servo2-AKTIVIRAN \r\n");
            message.qos = MQTT::QOS0;
            message.retained = false;
            message.dup = false;
            message.payload = (void*)buf;
            message.payloadlen = strlen(buf)+1;
            client.publish(topic_pub2, message); 
    }
    else if(topic_name.find("color2")!=std::string::npos){
        salji_boju=1;
        }
    else if(topic_name.find("echo")!=std::string::npos){
            if(poruka.find("connect")!=std::string::npos){
            fleg_start=1;
            sprintf(buf, "echo3 \r\n");
            message.qos = MQTT::QOS0;
            message.retained = false;
            message.dup = false;
            message.payload = (void*)buf;
            message.payloadlen = strlen(buf)+1;
            client.publish(topic_pub2, message); 
            }
            else if(poruka.find("echo")!=std::string::npos){
                T2.detach();
            }
            }
    ++arrivedcount;
}



void buttonFunction() {    
    button_pressed=1;   
}

void rpr0521_print_one_value(){
    bool error;
    uint16_t data[3];
    
    error = rpr0521_read_data(&data[0],0);
    if (!error) {
        pc.printf("PS[%4u], Als0[%4u], Als1[%4u]\n\r", data[0], data[1], data[2]);
        }
    else {
        pc.printf("\n\r");
        }
}

int main()
{   bool error;
    uint16_t dataProx[3];
    // Initialise the digital pin LED1 as an output
    colorLed=1;
    colorVout=1;
    //Servo1Napajanje=1;
    ColorSensor cs(PA_5,PA_6,PA_7,PB_6,PC_7);
    DigitalOut led(LED1);
    servo2.calibrate(0.0005,90);
    //servo1.calibrate(0.0005,180);
    button.rise(&buttonFunction);
    const char* hostname = "broker.mqttdashboard.com";
    int port = 1883;    
    
    pc.printf("\nRPR0521 library test program.\n\r");
    I2CCommonBegin();

   rpr0521_wait_until_found(0);
    pc.printf("\nSensor found.\n\r");
    rpr0521_initial_setup(0);
    wait(1);
    
    rpr0521_wait_until_found(1);
    pc.printf("\nSensor found.\n\r");
    rpr0521_initial_setup(1);
    wait(1);    
   
    
    
    wifi = WiFiInterface::get_default_instance();
    if (!wifi) {
        printf("ERROR: No WiFiInterface found.\n");
        return -1;
    }

    int count = scan_demo(wifi);
    if (count == 0) {
        printf("No WIFI APs found - can't continue further.\n");
        return -1;
    }

    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;
    }

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

    socket.open(wifi);
    socket.connect(hostname, port);
    
    int rc=0;
    
    MQTTPacket_connectData data = MQTTPacket_connectData_initializer;
    data.MQTTVersion = 3;
    data.clientID.cstring = "PMK-client3";
    //data.username.cstring = "testuser";
    //data.password.cstring = "testpassword";
    
    if ((rc = client.connect(data)) != 0)
        printf("rc from MQTT connect is %d\r\n", rc);

    
    if ((rc = client.subscribe(topic_sub, MQTT::QOS2, messageArrived)) != 0)
        printf("rc from MQTT subscribe is %d\r\n", rc);    
    
    
    
    while (true) {
        if(fleg_start==1){
        led = !led;
        thread_sleep_for(BLINKING_RATE_MS);
        if (salji_boju==1) {
            T2.detach();
            salji_boju=0;    
            printf("Publishing data\r\n");    
            // QoS 0
            char buf[100];
            float treshold=0.1;
            
            int Boja[3]={cs.getRed(), cs.getGreen(),cs.getBlue()};
            
            while(Boja[0]<30 && Boja[1]<30 && Boja[2]<30){
                wait(1);
                Boja[0]=cs.getRed();
                Boja[1]=cs.getGreen();
                Boja[2]=cs.getBlue();
                printf("LOSE MERENJE \r\n");
                printf("R:%d , G:%d , B:%d",Boja[0],Boja[1],Boja[2]);
                }
            int strongCol=max(Boja[0],max(Boja[1],Boja[2]));
            float BojaScal[3];
            BojaScal[0]=Boja[0]/(strongCol*1.0);
            BojaScal[1]=Boja[1]/(strongCol*1.0);
            BojaScal[2]=Boja[2]/(strongCol*1.0);        
            
            if(BojaScal[1]<r1[1]+treshold && BojaScal[1]>r1[1]-treshold && BojaScal[2]<r1[2]+treshold && BojaScal[0]>r1[0]-treshold && BojaScal[0]<r1[0]+treshold && BojaScal[2]>r1[2]-treshold ) {
                sprintf(buf, "Nije \r\n");}
            else if(BojaScal[1]<g1[1]+treshold && BojaScal[1]>g1[1]-treshold && BojaScal[2]<g1[2]+treshold && BojaScal[0]>g1[0]-treshold && BojaScal[0]<g1[0]+treshold && BojaScal[2]>g1[2]-treshold ) {
                sprintf(buf, "Jeste \r\n");}
                else if(BojaScal[1]<b1[1]+treshold && BojaScal[1]>b1[1]-treshold && BojaScal[2]<b1[2]+treshold && BojaScal[0]>b1[0]-treshold && BojaScal[0]<b1[0]+treshold && BojaScal[2]>b1[2]-treshold ) {
                sprintf(buf, "Nije \r\n");}
                else { sprintf(buf, "Nije \r\n");}
            
            printf("R: %.3f", BojaScal[0]);
            printf("G: %.3f", BojaScal[1]);
            printf("B: %.3f\r\n", BojaScal[2]);
            
            printf(" Crvena R: %.3f", r1[0]);
            printf("G: %.3f", r1[1]);
            printf("B: %.3f\r\n", r1[2]);
            
            //sprintf(buf, "RGB: %7s C\r\n", print_double(buffer, cs.getRed()*100000000+cs.getGreen()*10000+cs.getBlue()));
            
            message.qos = MQTT::QOS0;
            message.retained = false;
            message.dup = false;
            message.payload = (void*)buf;
            message.payloadlen = strlen(buf)+1;
            client.publish(topic_pub3, message);
            T2.attach(&interruptT2,5);
        }
        
        
        //printf("Yielding"); 
        client.yield(1000);
        //printf(" -> Yielded\r\n"); 
    //}
    
    error = rpr0521_read_data(&dataProx[0],0);
        if (!error){
            if(dataProx[0] > 50 && !fleg_proxy1) {
                fleg_proxy1=1;
                printf("Pristigao je cep na lokaciju O.O O:'( \r\n ");
                char buf[100];
                sprintf(buf, "proxy1-AKTIV PS = %d",dataProx[0]);
                message.qos = MQTT::QOS0;
                message.retained = false;
                message.dup = false;
                message.payload = (void*)buf;
                message.payloadlen = strlen(buf)+1;
                client.publish(topic_pub1, message);
                T2.attach(&interruptT2,5);    
                
                }
                else if(dataProx[0]<50){fleg_proxy1=0;}
                client.yield(1000);
                }
                error = rpr0521_read_data(&dataProx[0],1);
        if (!error){
            if(dataProx[0] > 50 && !fleg_proxy2) {
                fleg_proxy2=1;
                printf("Pristigao je cep na lokaciju O.O O:'( \r\n ");
                char buf[100];
                sprintf(buf, "proxy1-AKTIV PS = %d",dataProx[0]);
                message.qos = MQTT::QOS0;
                message.retained = false;
                message.dup = false;
                message.payload = (void*)buf;
                message.payloadlen = strlen(buf)+1;
                client.publish(topic_pub4, message);
                T2.attach(&interruptT2,5);    
                
                }
                else if(dataProx[0]<50){fleg_proxy2=0;}
                client.yield(1000);
                }
                
    
    }
    }
}