Emilija Stankovic / Mbed OS PMK_industrija_mikro2

PMKIND

Dependencies:   Servo TextLCD

main.cpp@1:d3bb978c1ff7, 2021-06-30 (annotated)

Committer:
emilija
Date:
Wed Jun 30 20:50:16 2021 +0000
Revision:
1:d3bb978c1ff7
Parent:
0:51001d8fdeff
Child:
2:5918b8bad3a7
PMK_industrija_mikro2

Who changed what in which revision?

UserRevisionLine numberNew contents of line
emilija 0:51001d8fdeff 1 /* mbed Microcontroller Library
emilija 0:51001d8fdeff 2 * Copyright (c) 2019 ARM Limited
emilija 0:51001d8fdeff 3 * SPDX-License-Identifier: Apache-2.0
emilija 0:51001d8fdeff 4 */
emilija 0:51001d8fdeff 5
emilija 0:51001d8fdeff 6 #include "mbed.h"
emilija 0:51001d8fdeff 7 #include "platform/mbed_thread.h"
emilija 0:51001d8fdeff 8 #include "Servo.h"
emilija 0:51001d8fdeff 9 #include "color.h"
emilija 0:51001d8fdeff 10 #include <string>
emilija 0:51001d8fdeff 11 #include <MQTTClientMbedOs.h>
emilija 1:d3bb978c1ff7 12 #include <algorithm>
emilija 0:51001d8fdeff 13
emilija 0:51001d8fdeff 14 // Blinking rate in milliseconds
emilija 0:51001d8fdeff 15 #define BLINKING_RATE_MS 500
emilija 0:51001d8fdeff 16
emilija 1:d3bb978c1ff7 17 float r1[3]={1 ,109.0/399.0,133.0/399.0};
emilija 1:d3bb978c1ff7 18 float g1[3]={206.0/411.0,399.0/411.0,1};
emilija 1:d3bb978c1ff7 19 float b1[3]={120.0/490.0 ,209.0/490.0, 1};
emilija 1:d3bb978c1ff7 20
emilija 0:51001d8fdeff 21 Servo servo0(D3);
emilija 1:d3bb978c1ff7 22 Servo servo1(D6);
emilija 1:d3bb978c1ff7 23 DigitalOut colorLed(D4);
emilija 1:d3bb978c1ff7 24 DigitalOut Servo1Napajanje(D7);
emilija 1:d3bb978c1ff7 25 DigitalOut colorVout(D5);
emilija 0:51001d8fdeff 26 InterruptIn button(USER_BUTTON);
emilija 0:51001d8fdeff 27 int arrivedcount = 0;
emilija 0:51001d8fdeff 28 TCPSocket socket;
emilija 0:51001d8fdeff 29 MQTTClient client(&socket);
emilija 0:51001d8fdeff 30 MQTT::Message message;
emilija 0:51001d8fdeff 31 int button_pressed=0;
emilija 1:d3bb978c1ff7 32 char* topic_pub = "PMK_industrija/micro1/color";
emilija 0:51001d8fdeff 33 char* topic_sub = "PMK_industrija/micro2/#";
emilija 1:d3bb978c1ff7 34 int salji_boju=0;
emilija 0:51001d8fdeff 35
emilija 0:51001d8fdeff 36 WiFiInterface *wifi;
emilija 0:51001d8fdeff 37 volatile int mems_event = 0;
emilija 0:51001d8fdeff 38 uint32_t previous_tick = 0;
emilija 0:51001d8fdeff 39 uint32_t current_tick = 0;
emilija 0:51001d8fdeff 40 uint8_t high = 0, low = 0;
emilija 0:51001d8fdeff 41 float temperature = 0.0f;
emilija 0:51001d8fdeff 42 char buffer[32];
emilija 0:51001d8fdeff 43 static char *print_double(char *str, double v, int decimalDigits = 2)
emilija 0:51001d8fdeff 44 {
emilija 0:51001d8fdeff 45 int i = 1;
emilija 0:51001d8fdeff 46 int intPart, fractPart;
emilija 0:51001d8fdeff 47 int len;
emilija 0:51001d8fdeff 48 char *ptr;
emilija 0:51001d8fdeff 49
emilija 0:51001d8fdeff 50 /* prepare decimal digits multiplicator */
emilija 0:51001d8fdeff 51 for (; decimalDigits != 0; i *= 10, decimalDigits--);
emilija 0:51001d8fdeff 52
emilija 0:51001d8fdeff 53 /* calculate integer & fractinal parts */
emilija 0:51001d8fdeff 54 intPart = (int)v;
emilija 0:51001d8fdeff 55 fractPart = (int)((v - (double)(int)v) * i);
emilija 0:51001d8fdeff 56
emilija 0:51001d8fdeff 57 /* fill in integer part */
emilija 0:51001d8fdeff 58 sprintf(str, "%i.", intPart);
emilija 0:51001d8fdeff 59
emilija 0:51001d8fdeff 60 /* prepare fill in of fractional part */
emilija 0:51001d8fdeff 61 len = strlen(str);
emilija 0:51001d8fdeff 62 ptr = &str[len];
emilija 0:51001d8fdeff 63
emilija 0:51001d8fdeff 64 /* fill in leading fractional zeros */
emilija 0:51001d8fdeff 65 for (i /= 10; i > 1; i /= 10, ptr++) {
emilija 0:51001d8fdeff 66 if (fractPart >= i) {
emilija 0:51001d8fdeff 67 break;
emilija 0:51001d8fdeff 68 }
emilija 0:51001d8fdeff 69 *ptr = '0';
emilija 0:51001d8fdeff 70 }
emilija 0:51001d8fdeff 71
emilija 0:51001d8fdeff 72 /* fill in (rest of) fractional part */
emilija 0:51001d8fdeff 73 sprintf(ptr, "%i", fractPart);
emilija 0:51001d8fdeff 74
emilija 0:51001d8fdeff 75 return str;
emilija 0:51001d8fdeff 76 }
emilija 0:51001d8fdeff 77
emilija 0:51001d8fdeff 78 const char *sec2str(nsapi_security_t sec)
emilija 0:51001d8fdeff 79 {
emilija 0:51001d8fdeff 80 switch (sec) {
emilija 0:51001d8fdeff 81 case NSAPI_SECURITY_NONE:
emilija 0:51001d8fdeff 82 return "None";
emilija 0:51001d8fdeff 83 case NSAPI_SECURITY_WEP:
emilija 0:51001d8fdeff 84 return "WEP";
emilija 0:51001d8fdeff 85 case NSAPI_SECURITY_WPA:
emilija 0:51001d8fdeff 86 return "WPA";
emilija 0:51001d8fdeff 87 case NSAPI_SECURITY_WPA2:
emilija 0:51001d8fdeff 88 return "WPA2";
emilija 0:51001d8fdeff 89 case NSAPI_SECURITY_WPA_WPA2:
emilija 0:51001d8fdeff 90 return "WPA/WPA2";
emilija 0:51001d8fdeff 91 case NSAPI_SECURITY_UNKNOWN:
emilija 0:51001d8fdeff 92 default:
emilija 0:51001d8fdeff 93 return "Unknown";
emilija 0:51001d8fdeff 94 }
emilija 0:51001d8fdeff 95 }
emilija 0:51001d8fdeff 96
emilija 0:51001d8fdeff 97 int scan_demo(WiFiInterface *wifi)
emilija 0:51001d8fdeff 98 {
emilija 0:51001d8fdeff 99 WiFiAccessPoint *ap;
emilija 0:51001d8fdeff 100 printf("Scan:\n");
emilija 0:51001d8fdeff 101 int count = wifi->scan(NULL,0);
emilija 0:51001d8fdeff 102 if (count <= 0) {
emilija 0:51001d8fdeff 103 printf("scan() failed with return value: %d\n", count);
emilija 0:51001d8fdeff 104 return 0;
emilija 0:51001d8fdeff 105 }
emilija 0:51001d8fdeff 106 /* Limit number of network arbitrary to 15 */
emilija 0:51001d8fdeff 107 count = count < 15 ? count : 15;
emilija 0:51001d8fdeff 108 ap = new WiFiAccessPoint[count];
emilija 0:51001d8fdeff 109 count = wifi->scan(ap, count);
emilija 0:51001d8fdeff 110 if (count <= 0) {
emilija 0:51001d8fdeff 111 printf("scan() failed with return value: %d\n", count);
emilija 0:51001d8fdeff 112 return 0;
emilija 0:51001d8fdeff 113 }
emilija 0:51001d8fdeff 114 for (int i = 0; i < count; i++) {
emilija 0:51001d8fdeff 115 printf("Network: %s secured: %s BSSID: %hhX:%hhX:%hhX:%hhx:%hhx:%hhx RSSI: %hhd Ch: %hhd\n", ap[i].get_ssid(),
emilija 0:51001d8fdeff 116 sec2str(ap[i].get_security()), ap[i].get_bssid()[0], ap[i].get_bssid()[1], ap[i].get_bssid()[2],
emilija 0:51001d8fdeff 117 ap[i].get_bssid()[3], ap[i].get_bssid()[4], ap[i].get_bssid()[5], ap[i].get_rssi(), ap[i].get_channel());
emilija 0:51001d8fdeff 118 }
emilija 0:51001d8fdeff 119 printf("%d networks available.\n", count);
emilija 0:51001d8fdeff 120 delete[] ap;
emilija 0:51001d8fdeff 121 return count;
emilija 0:51001d8fdeff 122 }
emilija 0:51001d8fdeff 123
emilija 0:51001d8fdeff 124 void messageArrived(MQTT::MessageData& md)
emilija 0:51001d8fdeff 125 {
emilija 0:51001d8fdeff 126 MQTTString &topic = md.topicName;
emilija 0:51001d8fdeff 127 string topic_name = topic.lenstring.data;
emilija 0:51001d8fdeff 128 printf("Topic name %d : %s\r\n",topic.lenstring.len,topic_name.c_str() );
emilija 0:51001d8fdeff 129 MQTT::Message &message = md.message;
emilija 0:51001d8fdeff 130 printf("Message arrived: qos %d, retained %d, dup %d, packetid %d\r\n", message.qos, message.retained, message.dup, message.id);
emilija 0:51001d8fdeff 131 printf("Payload %.*s\r\n", message.payloadlen, (char*)message.payload);
emilija 0:51001d8fdeff 132 char* poruka = (char*)message.payload;
emilija 0:51001d8fdeff 133 if(topic_name.find("servo0")!=std::string::npos) {
emilija 1:d3bb978c1ff7 134 printf("AKTIVIRAN SERVO0\n");
emilija 0:51001d8fdeff 135 for(float p=0; p<1.0; p += 0.1) {
emilija 0:51001d8fdeff 136 servo0 = p;
emilija 0:51001d8fdeff 137
emilija 0:51001d8fdeff 138 }
emilija 0:51001d8fdeff 139 wait(2);
emilija 0:51001d8fdeff 140 for(float p=1.0; p>0.0; p -= 0.1) {
emilija 0:51001d8fdeff 141 servo0 = p;
emilija 0:51001d8fdeff 142
emilija 0:51001d8fdeff 143 }
emilija 0:51001d8fdeff 144 }
emilija 1:d3bb978c1ff7 145 else if(topic_name.find("proxy1")!=std::string::npos){
emilija 1:d3bb978c1ff7 146 salji_boju=1;
emilija 1:d3bb978c1ff7 147 }
emilija 1:d3bb978c1ff7 148 else if(topic_name.find("servo1")!=std::string::npos){
emilija 1:d3bb978c1ff7 149 printf("AKTIVIRAN SERVO1\n");
emilija 1:d3bb978c1ff7 150 for(float p=0; p<1.0; p += 0.1) {
emilija 1:d3bb978c1ff7 151 servo1 = p;
emilija 1:d3bb978c1ff7 152
emilija 1:d3bb978c1ff7 153 }
emilija 1:d3bb978c1ff7 154 wait(2);
emilija 1:d3bb978c1ff7 155 for(float p=1.0; p>0.0; p -= 0.1) {
emilija 1:d3bb978c1ff7 156 servo1 = p;
emilija 1:d3bb978c1ff7 157
emilija 1:d3bb978c1ff7 158 }
emilija 1:d3bb978c1ff7 159
emilija 1:d3bb978c1ff7 160
emilija 1:d3bb978c1ff7 161
emilija 1:d3bb978c1ff7 162 }
emilija 0:51001d8fdeff 163 ++arrivedcount;
emilija 0:51001d8fdeff 164 }
emilija 0:51001d8fdeff 165
emilija 0:51001d8fdeff 166
emilija 1:d3bb978c1ff7 167
emilija 0:51001d8fdeff 168 void buttonFunction() {
emilija 0:51001d8fdeff 169 button_pressed=1;
emilija 0:51001d8fdeff 170 }
emilija 0:51001d8fdeff 171
emilija 0:51001d8fdeff 172 int main()
emilija 0:51001d8fdeff 173 {
emilija 0:51001d8fdeff 174 // Initialise the digital pin LED1 as an output
emilija 1:d3bb978c1ff7 175 colorLed=1;
emilija 1:d3bb978c1ff7 176 colorVout=1;
emilija 1:d3bb978c1ff7 177 Servo1Napajanje=1;
emilija 0:51001d8fdeff 178 ColorSensor cs(PA_5,PA_6,PA_7,PB_6,PC_7);
emilija 0:51001d8fdeff 179 DigitalOut led(LED1);
emilija 0:51001d8fdeff 180 servo0.calibrate(0.0005,90);
emilija 1:d3bb978c1ff7 181 servo1.calibrate(0.0005,180);
emilija 0:51001d8fdeff 182 button.rise(&buttonFunction);
emilija 0:51001d8fdeff 183 const char* hostname = "broker.mqttdashboard.com";
emilija 0:51001d8fdeff 184 int port = 1883;
emilija 0:51001d8fdeff 185
emilija 0:51001d8fdeff 186 wifi = WiFiInterface::get_default_instance();
emilija 0:51001d8fdeff 187 if (!wifi) {
emilija 0:51001d8fdeff 188 printf("ERROR: No WiFiInterface found.\n");
emilija 0:51001d8fdeff 189 return -1;
emilija 0:51001d8fdeff 190 }
emilija 0:51001d8fdeff 191
emilija 0:51001d8fdeff 192 int count = scan_demo(wifi);
emilija 0:51001d8fdeff 193 if (count == 0) {
emilija 0:51001d8fdeff 194 printf("No WIFI APs found - can't continue further.\n");
emilija 0:51001d8fdeff 195 return -1;
emilija 0:51001d8fdeff 196 }
emilija 0:51001d8fdeff 197
emilija 0:51001d8fdeff 198 printf("\nConnecting to %s...\n", MBED_CONF_APP_WIFI_SSID);
emilija 0:51001d8fdeff 199 int ret = wifi->connect(MBED_CONF_APP_WIFI_SSID, MBED_CONF_APP_WIFI_PASSWORD, NSAPI_SECURITY_WPA_WPA2);
emilija 0:51001d8fdeff 200 if (ret != 0) {
emilija 0:51001d8fdeff 201 printf("\nConnection error: %d\n", ret);
emilija 0:51001d8fdeff 202 return -1;
emilija 0:51001d8fdeff 203 }
emilija 0:51001d8fdeff 204
emilija 0:51001d8fdeff 205 printf("Success\n\n");
emilija 0:51001d8fdeff 206 printf("MAC: %s\n", wifi->get_mac_address());
emilija 0:51001d8fdeff 207 printf("IP: %s\n", wifi->get_ip_address());
emilija 0:51001d8fdeff 208 printf("Netmask: %s\n", wifi->get_netmask());
emilija 0:51001d8fdeff 209 printf("Gateway: %s\n", wifi->get_gateway());
emilija 0:51001d8fdeff 210 printf("RSSI: %d\n\n", wifi->get_rssi());
emilija 0:51001d8fdeff 211
emilija 0:51001d8fdeff 212 socket.open(wifi);
emilija 0:51001d8fdeff 213 socket.connect(hostname, port);
emilija 0:51001d8fdeff 214
emilija 0:51001d8fdeff 215 int rc=0;
emilija 0:51001d8fdeff 216
emilija 0:51001d8fdeff 217 MQTTPacket_connectData data = MQTTPacket_connectData_initializer;
emilija 0:51001d8fdeff 218 data.MQTTVersion = 3;
emilija 0:51001d8fdeff 219 data.clientID.cstring = "PMK-client2";
emilija 0:51001d8fdeff 220 //data.username.cstring = "testuser";
emilija 0:51001d8fdeff 221 //data.password.cstring = "testpassword";
emilija 0:51001d8fdeff 222
emilija 0:51001d8fdeff 223 if ((rc = client.connect(data)) != 0)
emilija 0:51001d8fdeff 224 printf("rc from MQTT connect is %d\r\n", rc);
emilija 0:51001d8fdeff 225
emilija 0:51001d8fdeff 226
emilija 0:51001d8fdeff 227 if ((rc = client.subscribe(topic_sub, MQTT::QOS2, messageArrived)) != 0)
emilija 0:51001d8fdeff 228 printf("rc from MQTT subscribe is %d\r\n", rc);
emilija 0:51001d8fdeff 229
emilija 0:51001d8fdeff 230
emilija 0:51001d8fdeff 231
emilija 0:51001d8fdeff 232 while (true) {
emilija 0:51001d8fdeff 233 led = !led;
emilija 0:51001d8fdeff 234 thread_sleep_for(BLINKING_RATE_MS);
emilija 1:d3bb978c1ff7 235 if (salji_boju==1) {
emilija 1:d3bb978c1ff7 236 salji_boju=0;
emilija 0:51001d8fdeff 237 printf("Publishing data\r\n");
emilija 0:51001d8fdeff 238 // QoS 0
emilija 0:51001d8fdeff 239 char buf[100];
emilija 1:d3bb978c1ff7 240 float treshold=0.1;
emilija 1:d3bb978c1ff7 241
emilija 1:d3bb978c1ff7 242 int Boja[3]={cs.getRed(), cs.getGreen(),cs.getBlue()};
emilija 0:51001d8fdeff 243
emilija 1:d3bb978c1ff7 244 while(Boja[0]<30 && Boja[1]<30 && Boja[2]<30){
emilija 1:d3bb978c1ff7 245 wait(1);
emilija 1:d3bb978c1ff7 246 Boja[0]=cs.getRed();
emilija 1:d3bb978c1ff7 247 Boja[1]=cs.getGreen();
emilija 1:d3bb978c1ff7 248 Boja[2]=cs.getBlue();
emilija 1:d3bb978c1ff7 249 printf("LOSE MERENJE \r\n");
emilija 1:d3bb978c1ff7 250 printf("R:%d , G:%d , B:%d",Boja[0],Boja[1],Boja[2]);
emilija 1:d3bb978c1ff7 251 }
emilija 1:d3bb978c1ff7 252 int strongCol=max(Boja[0],max(Boja[1],Boja[2]));
emilija 1:d3bb978c1ff7 253 float BojaScal[3];
emilija 1:d3bb978c1ff7 254 BojaScal[0]=Boja[0]/(strongCol*1.0);
emilija 1:d3bb978c1ff7 255 BojaScal[1]=Boja[1]/(strongCol*1.0);
emilija 1:d3bb978c1ff7 256 BojaScal[2]=Boja[2]/(strongCol*1.0);
emilija 1:d3bb978c1ff7 257
emilija 1:d3bb978c1ff7 258 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 ) {
emilija 1:d3bb978c1ff7 259 sprintf(buf, "RGB: CRVENA \r\n");}
emilija 1:d3bb978c1ff7 260 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 ) {
emilija 1:d3bb978c1ff7 261 sprintf(buf, "RGB: ZELENA \r\n");}
emilija 1:d3bb978c1ff7 262 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 ) {
emilija 1:d3bb978c1ff7 263 sprintf(buf, "RGB: PLAVA \r\n");}
emilija 1:d3bb978c1ff7 264 else { sprintf(buf, "RGB: NEKA DRUGA BOJA \r\n");}
emilija 1:d3bb978c1ff7 265
emilija 1:d3bb978c1ff7 266 printf("R: %.3f", BojaScal[0]);
emilija 1:d3bb978c1ff7 267 printf("G: %.3f", BojaScal[1]);
emilija 1:d3bb978c1ff7 268 printf("B: %.3f\r\n", BojaScal[2]);
emilija 1:d3bb978c1ff7 269
emilija 1:d3bb978c1ff7 270 printf(" Crvena R: %.3f", r1[0]);
emilija 1:d3bb978c1ff7 271 printf("G: %.3f", r1[1]);
emilija 1:d3bb978c1ff7 272 printf("B: %.3f\r\n", r1[2]);
emilija 1:d3bb978c1ff7 273
emilija 0:51001d8fdeff 274 //sprintf(buf, "RGB: %7s C\r\n", print_double(buffer, cs.getRed()*100000000+cs.getGreen()*10000+cs.getBlue()));
emilija 0:51001d8fdeff 275
emilija 1:d3bb978c1ff7 276 message.qos = MQTT::QOS0;
emilija 1:d3bb978c1ff7 277 message.retained = false;
emilija 1:d3bb978c1ff7 278 message.dup = false;
emilija 1:d3bb978c1ff7 279 message.payload = (void*)buf;
emilija 1:d3bb978c1ff7 280 message.payloadlen = strlen(buf)+1;
emilija 1:d3bb978c1ff7 281 client.publish(topic_pub, message);
emilija 0:51001d8fdeff 282
emilija 0:51001d8fdeff 283 }
emilija 1:d3bb978c1ff7 284 //printf("Yielding");
emilija 0:51001d8fdeff 285 client.yield(1000);
emilija 1:d3bb978c1ff7 286 //printf(" -> Yielded\r\n");
emilija 0:51001d8fdeff 287 }
emilija 0:51001d8fdeff 288 }