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?
User | Revision | Line number | New 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 | } |