main.cpp

00001 /* mbed Microcontroller Library
00002  * Copyright (c) 2019 ARM Limited
00003  * SPDX-License-Identifier: Apache-2.0
00004  */
00005 
00006 #include "mbed.h"
00007 #include "platform/mbed_thread.h"
00008 #include "Servo.h"
00009 #include "color.h"
00010 #include <string>
00011 #include <MQTTClientMbedOs.h>
00012 #include <algorithm>
00013 
00014 // Blinking rate in milliseconds
00015 #define BLINKING_RATE_MS                                                    500
00016 
00017 // varijable za boju
00018 
00019 float r1[3]={1 ,109.0/399.0,133.0/399.0};
00020 float g1[3]={206.0/411.0,399.0/411.0,1};
00021 float b1[3]={120.0/490.0 ,209.0/490.0, 1};
00022 DigitalOut colorLed(D4);
00023 DigitalOut colorVout(D5);
00024 int salji_boju=0;
00025 int fleg_start=0;
00026 Serial pc(USBTX,USBRX);
00027 // varijable za servo
00028 Servo servo0(D3);
00029 Servo servo1(D6);
00030 DigitalOut Servo1Napajanje(D7);
00031 
00032 InterruptIn button(USER_BUTTON);
00033 int arrivedcount = 0;
00034 TCPSocket socket;
00035 MQTTClient client(&socket);
00036 MQTT::Message message;
00037 int button_pressed=0;
00038 
00039 //Timeout T1;
00040 //void interruptT1(){
00041   //  printf("error1");
00042    // }
00043 
00044 char* topic_pub = "PMK_industrija/micro1/color1";
00045 char* topic_pub1 = "PMK_industrija/micro1/echo/2";
00046 char* topic_sub = "PMK_industrija/micro2/#";
00047 
00048 
00049 WiFiInterface *wifi;
00050 volatile int mems_event = 0;
00051 uint32_t previous_tick = 0;
00052 uint32_t current_tick = 0;
00053 uint8_t high = 0, low = 0;
00054 float temperature = 0.0f;
00055 char buffer[32];
00056 
00057 static char *print_double(char *str, double v, int decimalDigits = 2)
00058 {
00059     int i = 1;
00060     int intPart, fractPart;
00061     int len;
00062     char *ptr;
00063  
00064     /* prepare decimal digits multiplicator */
00065     for (; decimalDigits != 0; i *= 10, decimalDigits--);
00066  
00067     /* calculate integer & fractinal parts */
00068     intPart = (int)v;
00069     fractPart = (int)((v - (double)(int)v) * i);
00070  
00071     /* fill in integer part */
00072     sprintf(str, "%i.", intPart);
00073  
00074     /* prepare fill in of fractional part */
00075     len = strlen(str);
00076     ptr = &str[len];
00077  
00078     /* fill in leading fractional zeros */
00079     for (i /= 10; i > 1; i /= 10, ptr++) {
00080         if (fractPart >= i) {
00081             break;
00082         }
00083         *ptr = '0';
00084     }
00085  
00086     /* fill in (rest of) fractional part */
00087     sprintf(ptr, "%i", fractPart);
00088  
00089     return str;
00090 }
00091 
00092 const char *sec2str(nsapi_security_t sec)
00093 {
00094     switch (sec) {
00095         case NSAPI_SECURITY_NONE:
00096             return "None";
00097         case NSAPI_SECURITY_WEP:
00098             return "WEP";
00099         case NSAPI_SECURITY_WPA:
00100             return "WPA";
00101         case NSAPI_SECURITY_WPA2:
00102             return "WPA2";
00103         case NSAPI_SECURITY_WPA_WPA2:
00104             return "WPA/WPA2";
00105         case NSAPI_SECURITY_UNKNOWN:
00106         default:
00107             return "Unknown";
00108     }
00109 }
00110 
00111 int scan_demo(WiFiInterface *wifi)
00112 {
00113     WiFiAccessPoint *ap;
00114     printf("Scan:\n");
00115     int count = wifi->scan(NULL,0);
00116     if (count <= 0) {
00117         printf("scan() failed with return value: %d\n", count);
00118         return 0;
00119     }
00120     /* Limit number of network arbitrary to 15 */
00121     count = count < 15 ? count : 15;
00122     ap = new WiFiAccessPoint[count];
00123     count = wifi->scan(ap, count);
00124     if (count <= 0) {
00125         printf("scan() failed with return value: %d\n", count);
00126         return 0;
00127     }
00128     for (int i = 0; i < count; i++) {
00129         printf("Network: %s secured: %s BSSID: %hhX:%hhX:%hhX:%hhx:%hhx:%hhx RSSI: %hhd Ch: %hhd\n", ap[i].get_ssid(),
00130                sec2str(ap[i].get_security()), ap[i].get_bssid()[0], ap[i].get_bssid()[1], ap[i].get_bssid()[2],
00131                ap[i].get_bssid()[3], ap[i].get_bssid()[4], ap[i].get_bssid()[5], ap[i].get_rssi(), ap[i].get_channel());
00132     }
00133     printf("%d networks available.\n", count);
00134     delete[] ap;
00135     return count;
00136 }
00137 
00138 void messageArrived(MQTT::MessageData& md)
00139 {  char buf[100];
00140     MQTTString &topic = md.topicName;
00141     string topic_name = topic.lenstring.data;
00142     printf("Topic name %d : %s\r\n",topic.lenstring.len,topic_name.c_str() );
00143     MQTT::Message &message = md.message;
00144     printf("Message arrived: qos %d, retained %d, dup %d, packetid %d\r\n", message.qos, message.retained, message.dup, message.id);
00145     printf("Payload %.*s\r\n", message.payloadlen, (char*)message.payload);
00146     string poruka = (char*)message.payload;
00147     
00148     //if(topic_name.find("servo0")!=std::string::npos) {
00149       //   printf("AKTIVIRAN SERVO0\n"); 
00150         // for(float p=0; p<1.0; p += 0.1) {
00151        // servo0 = p;
00152         
00153        // }
00154        // wait(2);
00155        // for(float p=1.0; p>0.0; p -= 0.1) {
00156        // servo0 = p;
00157         
00158         //}
00159         
00160         //sprintf(buf, "servo0-AKTIVIRAN \r\n");
00161           //  message.qos = MQTT::QOS0;
00162             //message.retained = false;
00163             //message.dup = false;
00164             //message.payload = (void*)buf;
00165             //message.payloadlen = strlen(buf)+1;
00166             //client.publish(topic_pub1, message); 
00167     //}
00168     //else if(topic_name.find("color1")!=std::string::npos){
00169     //salji_boju=1;
00170     //}
00171     //else if(topic_name.find("servo1")!=std::string::npos){
00172      //   T1.detach();
00173       //  printf("AKTIVIRAN SERVO1\n"); 
00174         // for(float p=0; p<1.0; p += 0.1) {
00175         //servo1 = p;
00176       // da li na 45        
00177         //}
00178         //wait(2);
00179         //for(float p=1.0; p>0.0; p -= 0.1) {
00180         //servo1 = p;
00181         
00182         //}  
00183         
00184           //  sprintf(buf, "servo1-AKTIVIRAN \r\n");
00185             //message.qos = MQTT::QOS0;
00186             //message.retained = false;
00187             //message.dup = false;
00188             //message.payload = (void*)buf;
00189             //message.payloadlen = strlen(buf)+1;
00190             //client.publish(topic_pub1, message); 
00191 
00192         
00193         //}
00194         //else 
00195         if(topic_name.find("echo")!=std::string::npos){
00196             if(poruka.find("connect")!=std::string::npos){
00197             fleg_start=1;
00198             sprintf(buf, "echo2 \r\n");
00199             message.qos = MQTT::QOS0;
00200             message.retained = false;
00201             message.dup = false;
00202             message.payload = (void*)buf;
00203             message.payloadlen = strlen(buf)+1;
00204             client.publish(topic_pub1, message); 
00205             }
00206             else if(poruka.find("echo")!=std::string::npos){
00207        //         T1.detach();
00208             }
00209             }
00210     ++arrivedcount;
00211 
00212 }
00213 
00214 
00215 void buttonFunction() {    
00216     button_pressed=1;   
00217 }
00218 
00219 int main()
00220 {   
00221     // Initialise the digital pin LED1 as an output
00222     colorLed=1;
00223     colorVout=1;
00224     
00225     ColorSensor cs(PA_5,PA_6,PA_7,PB_6,PC_7);
00226     DigitalOut led(LED1);
00227     Servo1Napajanje=1;
00228     servo0.calibrate(0.0005,90);
00229     servo1.calibrate(0.0005,180);
00230     button.rise(&buttonFunction);
00231     
00232     const char* hostname = "broker.mqttdashboard.com";
00233     int port = 1883;    
00234     wifi = WiFiInterface::get_default_instance();
00235     if (!wifi) {
00236         printf("ERROR: No WiFiInterface found.\n");
00237         return -1;
00238     }
00239 
00240     int count = scan_demo(wifi);
00241     if (count == 0) {
00242         printf("No WIFI APs found - can't continue further.\n");
00243         return -1;
00244     }
00245 
00246     printf("\nConnecting to %s...\n", MBED_CONF_APP_WIFI_SSID);
00247     int ret = wifi->connect(MBED_CONF_APP_WIFI_SSID, MBED_CONF_APP_WIFI_PASSWORD, NSAPI_SECURITY_WPA_WPA2);
00248     if (ret != 0) {
00249         printf("\nConnection error: %d\n", ret);
00250         return -1;
00251     }
00252 
00253     printf("Success\n\n");
00254     printf("MAC: %s\n", wifi->get_mac_address());
00255     printf("IP: %s\n", wifi->get_ip_address());
00256     printf("Netmask: %s\n", wifi->get_netmask());
00257     printf("Gateway: %s\n", wifi->get_gateway());
00258     printf("RSSI: %d\n\n", wifi->get_rssi());
00259 
00260     socket.open(wifi);
00261     socket.connect(hostname, port);
00262     
00263     int rc=0;
00264     
00265     MQTTPacket_connectData data = MQTTPacket_connectData_initializer;
00266     data.MQTTVersion = 3;
00267     data.clientID.cstring = "PMK-client2";
00268     //data.username.cstring = "testuser";
00269     //data.password.cstring = "testpassword";
00270     
00271     if ((rc = client.connect(data)) != 0)
00272         printf("rc from MQTT connect is %d\r\n", rc);
00273 
00274     
00275     if ((rc = client.subscribe(topic_sub, MQTT::QOS2, messageArrived)) != 0)
00276         printf("rc from MQTT subscribe is %d\r\n", rc);    
00277     
00278     
00279      char buf[100];
00280             float treshold=0.1;  
00281     while (true) {
00282         client.yield(1000);
00283         if (fleg_start==1){
00284         led = !led;
00285         thread_sleep_for(BLINKING_RATE_MS);
00286         if (salji_boju==1) {
00287             salji_boju=0;    
00288             printf("Publishing data\r\n");    
00289             // QoS 0
00290            
00291             
00292             int Boja[3]={cs.getRed(), cs.getGreen(),cs.getBlue()};
00293             
00294             while(Boja[0]<30 && Boja[1]<30 && Boja[2]<30){
00295                 wait(1);
00296                 Boja[0]=cs.getRed();
00297                 Boja[1]=cs.getGreen();
00298                 Boja[2]=cs.getBlue();
00299                 printf("LOSE MERENJE \r\n");
00300                 printf("R:%d , G:%d , B:%d",Boja[0],Boja[1],Boja[2]);
00301                 }
00302             int strongCol=max(Boja[0],max(Boja[1],Boja[2]));
00303             float BojaScal[3];
00304             BojaScal[0]=Boja[0]/(strongCol*1.0);
00305             BojaScal[1]=Boja[1]/(strongCol*1.0);
00306             BojaScal[2]=Boja[2]/(strongCol*1.0);        
00307             
00308             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 ) {
00309                 sprintf(buf, "Jeste \r\n");}
00310             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 ) {
00311                 sprintf(buf, "Nije \r\n");}
00312                 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 ) {
00313                 sprintf(buf, "Nije \r\n");}
00314                 else { sprintf(buf, "Nije \r\n");}
00315             
00316             printf("R: %.3f", BojaScal[0]);
00317             printf("G: %.3f", BojaScal[1]);
00318             printf("B: %.3f\r\n", BojaScal[2]);
00319             
00320             printf(" Crvena R: %.3f", r1[0]);
00321             printf("G: %.3f", r1[1]);
00322             printf("B: %.3f\r\n", r1[2]);
00323             
00324             //sprintf(buf, "RGB: %7s C\r\n", print_double(buffer, cs.getRed()*100000000+cs.getGreen()*10000+cs.getBlue()));
00325             
00326             message.qos = MQTT::QOS0;
00327             message.retained = false;
00328             message.dup = false;
00329             message.payload = (void*)buf;
00330             message.payloadlen = strlen(buf)+1;
00331             client.publish(topic_pub, message);
00332          //   T1.attach(&interruptT1,5);
00333         }
00334         //printf("Yielding"); 
00335         client.yield(1000);
00336         //printf(" -> Yielded\r\n"); 
00337     }
00338     }
00339     
00340 }