Olja Jakovljevic / Mbed OS mc_broker_V1

Student project, Faculty of Electrical Engineering, University of Belgrade

Dependencies:   SSD1308_128x64_I2C Adafruit_GFX 19E042PIM_MB_PINS

main.cpp@2:4c2c7c8d608b, 2022-07-13 (annotated)

Committer:
oljakovljevic
Date:
Wed Jul 13 15:07:00 2022 +0000
Revision:
2:4c2c7c8d608b
Parent:
1:c994530bdb3d 
Student project, Faculty of Electrical Engineering, University of Belgrade

Who changed what in which revision?

UserRevisionLine numberNew contents of line
tzwell 0:6380a1c94d6c 1 /*
oljakovljevic 2:4c2c7c8d608b 2
oljakovljevic 2:4c2c7c8d608b 3 This code implements MQTT broker for Smart Industry system.
oljakovljevic 2:4c2c7c8d608b 4 The system sorts plastic caps based on their color (red, green or blue).
oljakovljevic 2:4c2c7c8d608b 5 Proximity sensors detect the presence of the cup, color sensor determines the color and
oljakovljevic 2:4c2c7c8d608b 6 servo motors drive the sorting mechanism.
oljakovljevic 2:4c2c7c8d608b 7
oljakovljevic 2:4c2c7c8d608b 8 Faculty of Electrical Engineering, University of Belgrade.
oljakovljevic 2:4c2c7c8d608b 9 Version (1), July 2022.
oljakovljevic 2:4c2c7c8d608b 10
oljakovljevic 2:4c2c7c8d608b 11 */
oljakovljevic 2:4c2c7c8d608b 12
oljakovljevic 2:4c2c7c8d608b 13 // Include the libraries
tzwell 0:6380a1c94d6c 14
tzwell 0:6380a1c94d6c 15 #include "mbed.h"
tzwell 0:6380a1c94d6c 16 #include "mb_pins.h"
tzwell 0:6380a1c94d6c 17 #include "platform/mbed_thread.h"
tzwell 0:6380a1c94d6c 18 #include "MQTTClientMbedOs.h"
oljakovljevic 2:4c2c7c8d608b 19 #include "SSD1308.h"
oljakovljevic 2:4c2c7c8d608b 20 #include "Adafruit_GFX.h"
oljakovljevic 2:4c2c7c8d608b 21 #include "Adafruit_GFX_Config.h"
oljakovljevic 2:4c2c7c8d608b 22 #include "Adafruit_SSD1306.h"
tzwell 0:6380a1c94d6c 23
oljakovljevic 2:4c2c7c8d608b 24 // I2C bus pins
oljakovljevic 2:4c2c7c8d608b 25 #define D_SDA PB_14
oljakovljevic 2:4c2c7c8d608b 26 #define D_SCL PB_13
oljakovljevic 2:4c2c7c8d608b 27
oljakovljevic 2:4c2c7c8d608b 28 // I2C address, 60d or 0x3c
oljakovljevic 2:4c2c7c8d608b 29 #define I2C_REAL_ADD 0x3c
oljakovljevic 2:4c2c7c8d608b 30 #define I2C_ADDRESS I2C_REAL_ADD << 1
oljakovljevic 2:4c2c7c8d608b 31 #define I2C_FREQUENCY 400000
oljakovljevic 2:4c2c7c8d608b 32
oljakovljevic 2:4c2c7c8d608b 33 // Client yield timeout in miliseconds
tzwell 0:6380a1c94d6c 34 #define YIELD_TIMEOUT_MS 1000
oljakovljevic 2:4c2c7c8d608b 35
oljakovljevic 2:4c2c7c8d608b 36 // Maximum number of networks to scan for
oljakovljevic 2:4c2c7c8d608b 37 #define MAX_NETWORKS 10
oljakovljevic 2:4c2c7c8d608b 38
oljakovljevic 2:4c2c7c8d608b 39 // Small delay for network information printing
tzwell 0:6380a1c94d6c 40 #define PRINTF_DELAY_MS 10
tzwell 0:6380a1c94d6c 41
oljakovljevic 2:4c2c7c8d608b 42 // Set OLED width and heigth [pixel]
oljakovljevic 2:4c2c7c8d608b 43 #define OLED_WIDTH_PX 128
oljakovljevic 2:4c2c7c8d608b 44 #define OLED_HEIGHT_PX 64
tzwell 0:6380a1c94d6c 45
oljakovljevic 2:4c2c7c8d608b 46 // Initialize I2C for OLED display
oljakovljevic 2:4c2c7c8d608b 47 I2C i2c(D_SDA, D_SCL);
oljakovljevic 2:4c2c7c8d608b 48
oljakovljevic 2:4c2c7c8d608b 49 // Initialize OLED
oljakovljevic 2:4c2c7c8d608b 50 Adafruit_SSD1306_I2c myOled(i2c,PB_5,I2C_ADDRESS,OLED_HEIGHT_PX,OLED_WIDTH_PX);
oljakovljevic 2:4c2c7c8d608b 51
oljakovljevic 2:4c2c7c8d608b 52 // Variables that count the number of caps for every color
oljakovljevic 2:4c2c7c8d608b 53 int red;
oljakovljevic 2:4c2c7c8d608b 54 int green;
oljakovljevic 2:4c2c7c8d608b 55 int blue;
oljakovljevic 2:4c2c7c8d608b 56
oljakovljevic 2:4c2c7c8d608b 57 // Variables used in broker decision algorithm
oljakovljevic 2:4c2c7c8d608b 58 int flagColor = 0;
oljakovljevic 2:4c2c7c8d608b 59 int flagProx1 = 0;
oljakovljevic 2:4c2c7c8d608b 60 int flagProx2 = 0;
oljakovljevic 2:4c2c7c8d608b 61 int flagStage1 = 1;
oljakovljevic 2:4c2c7c8d608b 62 int flagStage2 = 1;
oljakovljevic 2:4c2c7c8d608b 63 int notAvailable = 0;
oljakovljevic 2:4c2c7c8d608b 64 int poximityTreshold1 = 300;
oljakovljevic 2:4c2c7c8d608b 65 int poximityTreshold2 = 280;
oljakovljevic 2:4c2c7c8d608b 66 int currentColor = 0;
oljakovljevic 2:4c2c7c8d608b 67
oljakovljevic 2:4c2c7c8d608b 68 // Define start button
oljakovljevic 2:4c2c7c8d608b 69 DigitalIn buttonStart(MB_SW1);
oljakovljevic 2:4c2c7c8d608b 70 int flagStart = 0;
oljakovljevic 2:4c2c7c8d608b 71
oljakovljevic 2:4c2c7c8d608b 72 // Pointer to a WiFi network object
tzwell 0:6380a1c94d6c 73 WiFiInterface *wifi;
oljakovljevic 2:4c2c7c8d608b 74
oljakovljevic 2:4c2c7c8d608b 75 // Creating TCP socket
tzwell 0:6380a1c94d6c 76 TCPSocket socket;
oljakovljevic 2:4c2c7c8d608b 77
oljakovljevic 2:4c2c7c8d608b 78 // Creating MQTT client using the TCP socket
tzwell 0:6380a1c94d6c 79 MQTTClient client(&socket);
oljakovljevic 2:4c2c7c8d608b 80
oljakovljevic 2:4c2c7c8d608b 81 // Message handler
tzwell 0:6380a1c94d6c 82 MQTT::Message message;
tzwell 0:6380a1c94d6c 83
oljakovljevic 2:4c2c7c8d608b 84 // MQTT topics
oljakovljevic 2:4c2c7c8d608b 85 char* topic_pub_servo1 = "PMK_industry/micro/servo1";
oljakovljevic 2:4c2c7c8d608b 86 char* topic_pub_servo2 = "PMK_industry/micro/servo2";
oljakovljevic 2:4c2c7c8d608b 87 char* topic_sub_color = "PMK_industry/micro/color";
oljakovljevic 2:4c2c7c8d608b 88 char* topic_sub_proximity1 = "PMK_industry/micro/proximity1";
oljakovljevic 2:4c2c7c8d608b 89 char* topic_sub_proximity2 = "PMK_industry/micro/proximity2";
oljakovljevic 2:4c2c7c8d608b 90
oljakovljevic 2:4c2c7c8d608b 91 char* receavedMessage;
oljakovljevic 2:4c2c7c8d608b 92
oljakovljevic 2:4c2c7c8d608b 93 // Counter of arrived messages
tzwell 0:6380a1c94d6c 94 int arrivedcount = 0;
oljakovljevic 2:4c2c7c8d608b 95
oljakovljevic 2:4c2c7c8d608b 96 // HiveMQ broker connectivity information
tzwell 1:c994530bdb3d 97 const char* hostname = "broker.hivemq.com";
tzwell 1:c994530bdb3d 98 int port = 1883;
oljakovljevic 2:4c2c7c8d608b 99
oljakovljevic 2:4c2c7c8d608b 100 // Returning a string for a provided network encryption
tzwell 0:6380a1c94d6c 101 const char *sec2str(nsapi_security_t sec)
tzwell 0:6380a1c94d6c 102 {
tzwell 0:6380a1c94d6c 103 switch (sec)
tzwell 0:6380a1c94d6c 104 {
tzwell 0:6380a1c94d6c 105 case NSAPI_SECURITY_NONE:
tzwell 0:6380a1c94d6c 106 return "None";
tzwell 0:6380a1c94d6c 107 case NSAPI_SECURITY_WEP:
tzwell 0:6380a1c94d6c 108 return "WEP";
tzwell 0:6380a1c94d6c 109 case NSAPI_SECURITY_WPA:
tzwell 0:6380a1c94d6c 110 return "WPA";
tzwell 0:6380a1c94d6c 111 case NSAPI_SECURITY_WPA2:
tzwell 0:6380a1c94d6c 112 return "WPA2";
tzwell 0:6380a1c94d6c 113 case NSAPI_SECURITY_WPA_WPA2:
tzwell 0:6380a1c94d6c 114 return "WPA/WPA2";
tzwell 0:6380a1c94d6c 115 case NSAPI_SECURITY_UNKNOWN:
tzwell 0:6380a1c94d6c 116 default:
tzwell 0:6380a1c94d6c 117 return "Unknown";
tzwell 0:6380a1c94d6c 118 }
tzwell 0:6380a1c94d6c 119 }
tzwell 0:6380a1c94d6c 120
oljakovljevic 2:4c2c7c8d608b 121 // Scan available WiFi networks
tzwell 1:c994530bdb3d 122 int scan_networks(WiFiInterface *wifi)
tzwell 0:6380a1c94d6c 123 {
tzwell 0:6380a1c94d6c 124 printf("Scan:\n");
tzwell 1:c994530bdb3d 125
oljakovljevic 2:4c2c7c8d608b 126 // Scan only for the number of networks, first parameter is NULL
tzwell 1:c994530bdb3d 127 int count = wifi->scan(NULL, 0);
oljakovljevic 2:4c2c7c8d608b 128
tzwell 1:c994530bdb3d 129 // If there are no networks, count == 0, if there is an error, counter < 0:
tzwell 0:6380a1c94d6c 130 if (count <= 0)
tzwell 0:6380a1c94d6c 131 {
tzwell 0:6380a1c94d6c 132 printf("scan() failed with return value: %d\n", count);
tzwell 0:6380a1c94d6c 133 return 0;
tzwell 0:6380a1c94d6c 134 }
tzwell 0:6380a1c94d6c 135
oljakovljevic 2:4c2c7c8d608b 136 // Limit number of network arbitrary to some reasonable number
tzwell 0:6380a1c94d6c 137 count = count < MAX_NETWORKS ? count : MAX_NETWORKS;
tzwell 0:6380a1c94d6c 138
oljakovljevic 2:4c2c7c8d608b 139 // Create a local pointer to an object, which is an array of WiFi APs
tzwell 1:c994530bdb3d 140 WiFiAccessPoint *ap = new WiFiAccessPoint[count];
oljakovljevic 2:4c2c7c8d608b 141
oljakovljevic 2:4c2c7c8d608b 142 // Now scan again for 'count' networks and populate the array of APs
tzwell 0:6380a1c94d6c 143 count = wifi->scan(ap, count);
tzwell 1:c994530bdb3d 144
oljakovljevic 2:4c2c7c8d608b 145 // This time, the number of entries to 'ap' is returned
tzwell 0:6380a1c94d6c 146 if (count <= 0)
tzwell 0:6380a1c94d6c 147 {
tzwell 0:6380a1c94d6c 148 printf("scan() failed with return value: %d\n", count);
tzwell 0:6380a1c94d6c 149 return 0;
tzwell 0:6380a1c94d6c 150 }
tzwell 0:6380a1c94d6c 151
oljakovljevic 2:4c2c7c8d608b 152 // Print out the parameters of each AP
tzwell 0:6380a1c94d6c 153 for (int i = 0; i < count; i++)
tzwell 0:6380a1c94d6c 154 {
tzwell 0:6380a1c94d6c 155 printf("Network: %s secured: %s BSSID: %hhX:%hhX:%hhX:%hhx:%hhx:%hhx RSSI: %hhd Ch: %hhd\n", ap[i].get_ssid(),
tzwell 0:6380a1c94d6c 156 sec2str(ap[i].get_security()), ap[i].get_bssid()[0], ap[i].get_bssid()[1], ap[i].get_bssid()[2],
tzwell 0:6380a1c94d6c 157 ap[i].get_bssid()[3], ap[i].get_bssid()[4], ap[i].get_bssid()[5], ap[i].get_rssi(), ap[i].get_channel());
tzwell 0:6380a1c94d6c 158 thread_sleep_for(PRINTF_DELAY_MS);
tzwell 0:6380a1c94d6c 159 }
tzwell 0:6380a1c94d6c 160 printf("%d networks available.\n", count);
tzwell 1:c994530bdb3d 161
tzwell 1:c994530bdb3d 162 // Since 'ap' is dynamically allocated pointer to the array of objects, it
tzwell 1:c994530bdb3d 163 // needs to be deleted:
tzwell 0:6380a1c94d6c 164 delete[] ap;
tzwell 0:6380a1c94d6c 165 return count;
tzwell 0:6380a1c94d6c 166 }
tzwell 0:6380a1c94d6c 167
oljakovljevic 2:4c2c7c8d608b 168 // MQTT message handlers for certain topics
oljakovljevic 2:4c2c7c8d608b 169 void messageArrivedColor(MQTT::MessageData& md)
oljakovljevic 2:4c2c7c8d608b 170 {
oljakovljevic 2:4c2c7c8d608b 171 MQTT::Message &message = md.message;
oljakovljevic 2:4c2c7c8d608b 172 receavedMessage = (char*)message.payload;
oljakovljevic 2:4c2c7c8d608b 173
oljakovljevic 2:4c2c7c8d608b 174 if (strcmp(receavedMessage,"Red") == 0 ){ flagColor = 1; }
oljakovljevic 2:4c2c7c8d608b 175 else if (strcmp(receavedMessage,"Green") == 0 ) { flagColor = 2; }
oljakovljevic 2:4c2c7c8d608b 176 else if (strcmp(receavedMessage, "Blue") == 0 ) { flagColor = 3; }
oljakovljevic 2:4c2c7c8d608b 177 printf("Color: %.*s\r\n", message.payloadlen, (char*)message.payload);
oljakovljevic 2:4c2c7c8d608b 178
oljakovljevic 2:4c2c7c8d608b 179 }
oljakovljevic 2:4c2c7c8d608b 180
oljakovljevic 2:4c2c7c8d608b 181 void messageArrivedProximity1(MQTT::MessageData& md)
tzwell 0:6380a1c94d6c 182 {
tzwell 0:6380a1c94d6c 183 MQTT::Message &message = md.message;
oljakovljevic 2:4c2c7c8d608b 184 int receavedMessage = atoi((char*)message.payload);
oljakovljevic 2:4c2c7c8d608b 185
oljakovljevic 2:4c2c7c8d608b 186 if (receavedMessage > poximityTreshold1){ flagProx1 = 1;}
oljakovljevic 2:4c2c7c8d608b 187 else { flagProx1 = 0;}
oljakovljevic 2:4c2c7c8d608b 188 printf("Prox1 = %d\t", receavedMessage);
oljakovljevic 2:4c2c7c8d608b 189 printf("flag = %d\r\n", flagProx1);
oljakovljevic 2:4c2c7c8d608b 190
tzwell 0:6380a1c94d6c 191 }
tzwell 0:6380a1c94d6c 192
oljakovljevic 2:4c2c7c8d608b 193 void messageArrivedProximity2(MQTT::MessageData& md)
oljakovljevic 2:4c2c7c8d608b 194 {
oljakovljevic 2:4c2c7c8d608b 195 MQTT::Message &message = md.message;
oljakovljevic 2:4c2c7c8d608b 196 int receavedMessage = atoi((char*)message.payload);
tzwell 0:6380a1c94d6c 197
oljakovljevic 2:4c2c7c8d608b 198 if (receavedMessage > poximityTreshold2){ flagProx2 = 1;}
oljakovljevic 2:4c2c7c8d608b 199 else { flagProx2 = 0;}
oljakovljevic 2:4c2c7c8d608b 200 printf("Prox2 = %d\t", receavedMessage);
oljakovljevic 2:4c2c7c8d608b 201 printf("flag = %d\r\n", flagProx2);
oljakovljevic 2:4c2c7c8d608b 202
tzwell 0:6380a1c94d6c 203 }
tzwell 0:6380a1c94d6c 204
tzwell 0:6380a1c94d6c 205 int main()
tzwell 0:6380a1c94d6c 206 {
tzwell 1:c994530bdb3d 207 // Create a default network interface:
tzwell 0:6380a1c94d6c 208 wifi = WiFiInterface::get_default_instance();
tzwell 0:6380a1c94d6c 209 if (!wifi) {
tzwell 0:6380a1c94d6c 210 printf("ERROR: No WiFiInterface found.\n");
tzwell 0:6380a1c94d6c 211 return -1;
tzwell 0:6380a1c94d6c 212 }
tzwell 1:c994530bdb3d 213
tzwell 1:c994530bdb3d 214 // Scan for available networks and aquire information about Access Points:
tzwell 1:c994530bdb3d 215 int count = scan_networks(wifi);
tzwell 0:6380a1c94d6c 216 if (count == 0) {
tzwell 0:6380a1c94d6c 217 printf("No WIFI APs found - can't continue further.\n");
tzwell 0:6380a1c94d6c 218 return -1;
tzwell 0:6380a1c94d6c 219 }
tzwell 1:c994530bdb3d 220
tzwell 1:c994530bdb3d 221 // Connect to the network with the parameters specified in 'mbed_app.json':
tzwell 0:6380a1c94d6c 222 printf("\nConnecting to %s...\n", MBED_CONF_APP_WIFI_SSID);
tzwell 0:6380a1c94d6c 223 int ret = wifi->connect(MBED_CONF_APP_WIFI_SSID, MBED_CONF_APP_WIFI_PASSWORD, NSAPI_SECURITY_WPA_WPA2);
tzwell 0:6380a1c94d6c 224 if (ret != 0) {
tzwell 0:6380a1c94d6c 225 printf("\nConnection error: %d\n", ret);
tzwell 0:6380a1c94d6c 226 return -1;
tzwell 0:6380a1c94d6c 227 }
tzwell 1:c994530bdb3d 228
tzwell 1:c994530bdb3d 229 // Print out the information aquired:
tzwell 0:6380a1c94d6c 230 printf("Success\n\n");
tzwell 0:6380a1c94d6c 231 printf("MAC: %s\n", wifi->get_mac_address());
tzwell 0:6380a1c94d6c 232 printf("IP: %s\n", wifi->get_ip_address());
tzwell 0:6380a1c94d6c 233 printf("Netmask: %s\n", wifi->get_netmask());
tzwell 0:6380a1c94d6c 234 printf("Gateway: %s\n", wifi->get_gateway());
tzwell 0:6380a1c94d6c 235 printf("RSSI: %d\n\n", wifi->get_rssi());
tzwell 1:c994530bdb3d 236
tzwell 1:c994530bdb3d 237 // Open TCP socket using WiFi network interface:
tzwell 0:6380a1c94d6c 238 socket.open(wifi);
oljakovljevic 2:4c2c7c8d608b 239
tzwell 1:c994530bdb3d 240 // Connect to the HiveMQ broker:
tzwell 0:6380a1c94d6c 241 socket.connect(hostname, port);
oljakovljevic 2:4c2c7c8d608b 242
tzwell 1:c994530bdb3d 243 // Fill connect data with default values:
tzwell 0:6380a1c94d6c 244 MQTTPacket_connectData data = MQTTPacket_connectData_initializer;
oljakovljevic 2:4c2c7c8d608b 245
tzwell 1:c994530bdb3d 246 // Change only ID and protocol version:
tzwell 0:6380a1c94d6c 247 data.MQTTVersion = 3;
oljakovljevic 2:4c2c7c8d608b 248 data.clientID.cstring = "Broker";
tzwell 0:6380a1c94d6c 249
oljakovljevic 2:4c2c7c8d608b 250 // Connect to the mqtt broker
tzwell 1:c994530bdb3d 251 int rc = 0;
tzwell 0:6380a1c94d6c 252 if ((rc = client.connect(data)) != 0)
tzwell 0:6380a1c94d6c 253 printf("rc from MQTT connect is %d\r\n", rc);
tzwell 0:6380a1c94d6c 254
oljakovljevic 2:4c2c7c8d608b 255 // Subscribe to topics
oljakovljevic 2:4c2c7c8d608b 256 if ((rc = client.subscribe(topic_sub_color, MQTT::QOS2, messageArrivedColor)) != 0)
oljakovljevic 2:4c2c7c8d608b 257 printf("rc from MQTT subscribe is %d\r\n", rc);
oljakovljevic 2:4c2c7c8d608b 258
oljakovljevic 2:4c2c7c8d608b 259 if ((rc = client.subscribe(topic_sub_proximity1, MQTT::QOS2, messageArrivedProximity1)) != 0)
oljakovljevic 2:4c2c7c8d608b 260 printf("rc from MQTT subscribe is %d\r\n", rc);
oljakovljevic 2:4c2c7c8d608b 261
oljakovljevic 2:4c2c7c8d608b 262 if ((rc = client.subscribe(topic_sub_proximity2, MQTT::QOS2, messageArrivedProximity2)) != 0)
tzwell 0:6380a1c94d6c 263 printf("rc from MQTT subscribe is %d\r\n", rc);
oljakovljevic 2:4c2c7c8d608b 264
oljakovljevic 2:4c2c7c8d608b 265 red = 0;
oljakovljevic 2:4c2c7c8d608b 266 green = 0;
oljakovljevic 2:4c2c7c8d608b 267 blue = 0;
oljakovljevic 2:4c2c7c8d608b 268
oljakovljevic 2:4c2c7c8d608b 269 char bufRed[15];
oljakovljevic 2:4c2c7c8d608b 270 char bufGreen[15];
oljakovljevic 2:4c2c7c8d608b 271 char bufBlue[15];
oljakovljevic 2:4c2c7c8d608b 272
oljakovljevic 2:4c2c7c8d608b 273 myOled.begin();
oljakovljevic 2:4c2c7c8d608b 274 myOled.setTextSize(2);
oljakovljevic 2:4c2c7c8d608b 275 myOled.setTextColor(WHITE);
oljakovljevic 2:4c2c7c8d608b 276 myOled.setTextCursor(25,0);
oljakovljevic 2:4c2c7c8d608b 277 myOled.printf("Press");
oljakovljevic 2:4c2c7c8d608b 278 myOled.setTextCursor(25,25);
oljakovljevic 2:4c2c7c8d608b 279 myOled.printf("START");
oljakovljevic 2:4c2c7c8d608b 280 myOled.display();
oljakovljevic 2:4c2c7c8d608b 281
tzwell 0:6380a1c94d6c 282 while (true) {
oljakovljevic 2:4c2c7c8d608b 283
oljakovljevic 2:4c2c7c8d608b 284 if (!flagStart){
oljakovljevic 2:4c2c7c8d608b 285 flagStart = !buttonStart;
oljakovljevic 2:4c2c7c8d608b 286 thread_sleep_for(50);
oljakovljevic 2:4c2c7c8d608b 287
tzwell 0:6380a1c94d6c 288 }
oljakovljevic 2:4c2c7c8d608b 289
oljakovljevic 2:4c2c7c8d608b 290 if (flagStart) {
oljakovljevic 2:4c2c7c8d608b 291
oljakovljevic 2:4c2c7c8d608b 292 sprintf(bufRed, "%s %d", "Red:", red);
oljakovljevic 2:4c2c7c8d608b 293 sprintf(bufGreen, "%s %d", "Green:", green);
oljakovljevic 2:4c2c7c8d608b 294 sprintf(bufBlue, "%s %d", "Blue:", blue);
oljakovljevic 2:4c2c7c8d608b 295
oljakovljevic 2:4c2c7c8d608b 296 myOled.clearDisplay();
oljakovljevic 2:4c2c7c8d608b 297 myOled.setTextSize(2);
oljakovljevic 2:4c2c7c8d608b 298 myOled.setTextColor(WHITE);
oljakovljevic 2:4c2c7c8d608b 299 myOled.setTextCursor(0,0);
oljakovljevic 2:4c2c7c8d608b 300 myOled.printf(bufRed);
oljakovljevic 2:4c2c7c8d608b 301 myOled.setTextCursor(0,25);
oljakovljevic 2:4c2c7c8d608b 302 myOled.printf(bufGreen);
oljakovljevic 2:4c2c7c8d608b 303 myOled.setTextCursor(0,50);
oljakovljevic 2:4c2c7c8d608b 304 myOled.printf(bufBlue);
oljakovljevic 2:4c2c7c8d608b 305 myOled.display();
oljakovljevic 2:4c2c7c8d608b 306
oljakovljevic 2:4c2c7c8d608b 307 // If a cap is detected at the first stage, send a command
oljakovljevic 2:4c2c7c8d608b 308 // for servo1 based on the color of the cap
oljakovljevic 2:4c2c7c8d608b 309 if(flagStage1 && flagProx1)
oljakovljevic 2:4c2c7c8d608b 310 {
oljakovljevic 2:4c2c7c8d608b 311
oljakovljevic 2:4c2c7c8d608b 312 if (flagColor == 1)
oljakovljevic 2:4c2c7c8d608b 313 {
oljakovljevic 2:4c2c7c8d608b 314 char buf[100];
oljakovljevic 2:4c2c7c8d608b 315 sprintf(buf, "right");
oljakovljevic 2:4c2c7c8d608b 316 message.qos = MQTT::QOS0;
oljakovljevic 2:4c2c7c8d608b 317 message.retained = false;
oljakovljevic 2:4c2c7c8d608b 318 message.dup = false;
oljakovljevic 2:4c2c7c8d608b 319 message.payload = (void*)buf;
oljakovljevic 2:4c2c7c8d608b 320 message.payloadlen = strlen(buf)+1;
oljakovljevic 2:4c2c7c8d608b 321 client.publish(topic_pub_servo1, message);
oljakovljevic 2:4c2c7c8d608b 322 flagStage1 = 0;
oljakovljevic 2:4c2c7c8d608b 323 red++;
oljakovljevic 2:4c2c7c8d608b 324
oljakovljevic 2:4c2c7c8d608b 325 }
oljakovljevic 2:4c2c7c8d608b 326
oljakovljevic 2:4c2c7c8d608b 327 else if (flagColor == 2)
oljakovljevic 2:4c2c7c8d608b 328 {
oljakovljevic 2:4c2c7c8d608b 329 char buf[100];
oljakovljevic 2:4c2c7c8d608b 330 sprintf(buf, "left");
oljakovljevic 2:4c2c7c8d608b 331 message.qos = MQTT::QOS0;
oljakovljevic 2:4c2c7c8d608b 332 message.retained = false;
oljakovljevic 2:4c2c7c8d608b 333 message.dup = false;
oljakovljevic 2:4c2c7c8d608b 334 message.payload = (void*)buf;
oljakovljevic 2:4c2c7c8d608b 335 message.payloadlen = strlen(buf)+1;
oljakovljevic 2:4c2c7c8d608b 336 client.publish(topic_pub_servo1, message);
oljakovljevic 2:4c2c7c8d608b 337 flagStage1 = 0;
oljakovljevic 2:4c2c7c8d608b 338 green++;
oljakovljevic 2:4c2c7c8d608b 339
oljakovljevic 2:4c2c7c8d608b 340 }
oljakovljevic 2:4c2c7c8d608b 341
oljakovljevic 2:4c2c7c8d608b 342 else if (flagColor == 3)
oljakovljevic 2:4c2c7c8d608b 343 {
oljakovljevic 2:4c2c7c8d608b 344 char buf[100];
oljakovljevic 2:4c2c7c8d608b 345 sprintf(buf, "left");
oljakovljevic 2:4c2c7c8d608b 346 message.qos = MQTT::QOS0;
oljakovljevic 2:4c2c7c8d608b 347 message.retained = false;
oljakovljevic 2:4c2c7c8d608b 348 message.dup = false;
oljakovljevic 2:4c2c7c8d608b 349 message.payload = (void*)buf;
oljakovljevic 2:4c2c7c8d608b 350 message.payloadlen = strlen(buf)+1;
oljakovljevic 2:4c2c7c8d608b 351 client.publish(topic_pub_servo1, message);
oljakovljevic 2:4c2c7c8d608b 352 flagStage1 = 0;
oljakovljevic 2:4c2c7c8d608b 353 blue++;
oljakovljevic 2:4c2c7c8d608b 354
oljakovljevic 2:4c2c7c8d608b 355 }
oljakovljevic 2:4c2c7c8d608b 356
oljakovljevic 2:4c2c7c8d608b 357 currentColor = flagColor;
oljakovljevic 2:4c2c7c8d608b 358 }
oljakovljevic 2:4c2c7c8d608b 359
oljakovljevic 2:4c2c7c8d608b 360 else if(!flagStage1 && !flagProx1)
oljakovljevic 2:4c2c7c8d608b 361 {
oljakovljevic 2:4c2c7c8d608b 362 char buf[100];
oljakovljevic 2:4c2c7c8d608b 363 sprintf(buf, "zero");
oljakovljevic 2:4c2c7c8d608b 364 message.qos = MQTT::QOS0;
oljakovljevic 2:4c2c7c8d608b 365 message.retained = false;
oljakovljevic 2:4c2c7c8d608b 366 message.dup = false;
oljakovljevic 2:4c2c7c8d608b 367 message.payload = (void*)buf;
oljakovljevic 2:4c2c7c8d608b 368 message.payloadlen = strlen(buf)+1;
oljakovljevic 2:4c2c7c8d608b 369 client.publish(topic_pub_servo1, message);
oljakovljevic 2:4c2c7c8d608b 370 flagStage1 = 1;
oljakovljevic 2:4c2c7c8d608b 371
oljakovljevic 2:4c2c7c8d608b 372 }
oljakovljevic 2:4c2c7c8d608b 373
oljakovljevic 2:4c2c7c8d608b 374 // If a cap is detected at the second stage, send a command
oljakovljevic 2:4c2c7c8d608b 375 // for servo2 based on the color of the cap
oljakovljevic 2:4c2c7c8d608b 376 if(flagStage2 && flagProx2)
oljakovljevic 2:4c2c7c8d608b 377 {
oljakovljevic 2:4c2c7c8d608b 378
oljakovljevic 2:4c2c7c8d608b 379 if (currentColor == 2)
oljakovljevic 2:4c2c7c8d608b 380 {
oljakovljevic 2:4c2c7c8d608b 381 char buf[100];
oljakovljevic 2:4c2c7c8d608b 382 sprintf(buf, "right");
oljakovljevic 2:4c2c7c8d608b 383 message.qos = MQTT::QOS0;
oljakovljevic 2:4c2c7c8d608b 384 message.retained = false;
oljakovljevic 2:4c2c7c8d608b 385 message.dup = false;
oljakovljevic 2:4c2c7c8d608b 386 message.payload = (void*)buf;
oljakovljevic 2:4c2c7c8d608b 387 message.payloadlen = strlen(buf)+1;
oljakovljevic 2:4c2c7c8d608b 388 client.publish(topic_pub_servo2, message);
oljakovljevic 2:4c2c7c8d608b 389 flagStage2 = 0;
oljakovljevic 2:4c2c7c8d608b 390
oljakovljevic 2:4c2c7c8d608b 391 }
oljakovljevic 2:4c2c7c8d608b 392
oljakovljevic 2:4c2c7c8d608b 393 else if (currentColor == 3)
oljakovljevic 2:4c2c7c8d608b 394 {
oljakovljevic 2:4c2c7c8d608b 395 char buf[100];
oljakovljevic 2:4c2c7c8d608b 396 sprintf(buf, "left");
oljakovljevic 2:4c2c7c8d608b 397 message.qos = MQTT::QOS0;
oljakovljevic 2:4c2c7c8d608b 398 message.retained = false;
oljakovljevic 2:4c2c7c8d608b 399 message.dup = false;
oljakovljevic 2:4c2c7c8d608b 400 message.payload = (void*)buf;
oljakovljevic 2:4c2c7c8d608b 401 message.payloadlen = strlen(buf)+1;
oljakovljevic 2:4c2c7c8d608b 402 client.publish(topic_pub_servo2, message);
oljakovljevic 2:4c2c7c8d608b 403 flagStage2 = 0;
oljakovljevic 2:4c2c7c8d608b 404
oljakovljevic 2:4c2c7c8d608b 405 }
oljakovljevic 2:4c2c7c8d608b 406
oljakovljevic 2:4c2c7c8d608b 407 }
oljakovljevic 2:4c2c7c8d608b 408
oljakovljevic 2:4c2c7c8d608b 409 else if(!flagStage2 && !flagProx2)
oljakovljevic 2:4c2c7c8d608b 410 {
oljakovljevic 2:4c2c7c8d608b 411 char buf[100];
oljakovljevic 2:4c2c7c8d608b 412 sprintf(buf, "zero");
oljakovljevic 2:4c2c7c8d608b 413 message.qos = MQTT::QOS0;
oljakovljevic 2:4c2c7c8d608b 414 message.retained = false;
oljakovljevic 2:4c2c7c8d608b 415 message.dup = false;
oljakovljevic 2:4c2c7c8d608b 416 message.payload = (void*)buf;
oljakovljevic 2:4c2c7c8d608b 417 message.payloadlen = strlen(buf)+1;
oljakovljevic 2:4c2c7c8d608b 418 client.publish(topic_pub_servo2, message);
oljakovljevic 2:4c2c7c8d608b 419 flagStage2 = 1;
oljakovljevic 2:4c2c7c8d608b 420 currentColor = 0;
oljakovljevic 2:4c2c7c8d608b 421
oljakovljevic 2:4c2c7c8d608b 422 }
oljakovljevic 2:4c2c7c8d608b 423
oljakovljevic 2:4c2c7c8d608b 424 client.yield(YIELD_TIMEOUT_MS); // Need to call yield API to maintain connection
oljakovljevic 2:4c2c7c8d608b 425 }
tzwell 0:6380a1c94d6c 426 }
tzwell 0:6380a1c94d6c 427 }