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