Diff: main.cpp

Revision:

1:d3bb978c1ff7

Parent:

0:51001d8fdeff

Child:

2:5918b8bad3a7

--- a/main.cpp	Thu Jun 24 15:11:30 2021 +0000
+++ b/main.cpp	Wed Jun 30 20:50:16 2021 +0000
@@ -9,20 +9,29 @@
 #include "color.h"
 #include <string>
 #include <MQTTClientMbedOs.h>
+#include <algorithm>
 
 // Blinking rate in milliseconds
 #define BLINKING_RATE_MS                                                    500
 
+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};
+
 Servo servo0(D3);
-Servo servo1(D4);
+Servo servo1(D6);
+DigitalOut colorLed(D4);
+DigitalOut Servo1Napajanje(D7);
+DigitalOut colorVout(D5);
 InterruptIn button(USER_BUTTON);
 int arrivedcount = 0;
 TCPSocket socket;
 MQTTClient client(&socket);
 MQTT::Message message;
 int button_pressed=0;
-char* topic_pub = "PMK-client-temperature";
+char* topic_pub = "PMK_industrija/micro1/color";
 char* topic_sub = "PMK_industrija/micro2/#";
+int salji_boju=0;
 
 WiFiInterface *wifi;
 volatile int mems_event = 0;
@@ -122,7 +131,7 @@
     printf("Payload %.*s\r\n", message.payloadlen, (char*)message.payload);
     char* poruka = (char*)message.payload;
     if(topic_name.find("servo0")!=std::string::npos) {
-         printf("blblabla\n"); 
+         printf("AKTIVIRAN SERVO0\n"); 
          for(float p=0; p<1.0; p += 0.1) {
         servo0 = p;
         
@@ -133,10 +142,29 @@
         
         }
     }
+    else if(topic_name.find("proxy1")!=std::string::npos){
+    salji_boju=1;
+    }
+    else if(topic_name.find("servo1")!=std::string::npos){
+        printf("AKTIVIRAN SERVO1\n"); 
+         for(float p=0; p<1.0; p += 0.1) {
+        servo1 = p;
+        
+        }
+        wait(2);
+        for(float p=1.0; p>0.0; p -= 0.1) {
+        servo1 = p;
+        
+        }
+        
+        
+        
+        }
     ++arrivedcount;
 }
 
 
+
 void buttonFunction() {    
     button_pressed=1;   
 }
@@ -144,10 +172,13 @@
 int main()
 {
     // 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);
     servo0.calibrate(0.0005,90);
+    servo1.calibrate(0.0005,180);
     button.rise(&buttonFunction);
     const char* hostname = "broker.mqttdashboard.com";
     int port = 1883;    
@@ -201,27 +232,57 @@
     while (true) {
         led = !led;
         thread_sleep_for(BLINKING_RATE_MS);
-        if (button_pressed==1) {
-            button_pressed=0;    
+        if (salji_boju==1) {
+            salji_boju=0;    
             printf("Publishing data\r\n");    
             // QoS 0
             char buf[100];
-            printf("r: %d", cs.getRed());
+            float treshold=0.1;
+            
+            int Boja[3]={cs.getRed(), cs.getGreen(),cs.getBlue()};
             
-            printf("g: %d", cs.getGreen());
-            printf("b: %d", 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, "RGB: CRVENA \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, "RGB: ZELENA \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, "RGB: PLAVA \r\n");}
+                else { sprintf(buf, "RGB: NEKA DRUGA BOJA \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_pub, message);
+            message.qos = MQTT::QOS0;
+            message.retained = false;
+            message.dup = false;
+            message.payload = (void*)buf;
+            message.payloadlen = strlen(buf)+1;
+            client.publish(topic_pub, message);
             
         }
-        printf("Yielding"); 
+        //printf("Yielding"); 
         client.yield(1000);
-        printf(" -> Yielded\r\n"); 
+        //printf(" -> Yielded\r\n"); 
     }
 }