Olja Jakovljevic / Mbed 2 deprecated mc_proximity_servo_V1

Student project, Faculty of Electrical Engineering, University of Belgrade

Dependencies:   mbed Servo

main.cpp@0:0093b6d26596, 2022-07-13 (annotated)

Committer:
oljakovljevic
Date:
Wed Jul 13 15:08:13 2022 +0000
Revision:
0:0093b6d26596
Student project, Faculty of Electrical Engineering, University of Belgrade

Who changed what in which revision?

UserRevisionLine numberNew contents of line
oljakovljevic 0:0093b6d26596 1 /*
oljakovljevic 0:0093b6d26596 2
oljakovljevic 0:0093b6d26596 3 This code implements MQTT client for Smart Industry system.
oljakovljevic 0:0093b6d26596 4 Proximity sensor detects the presence of the cup and
oljakovljevic 0:0093b6d26596 5 the information is sent to the broker.
oljakovljevic 0:0093b6d26596 6 If the cap is on the second stage, command for the servo motor, calculated by the broker,
oljakovljevic 0:0093b6d26596 7 is sent to the client.
oljakovljevic 0:0093b6d26596 8
oljakovljevic 0:0093b6d26596 9 Faculty of Electrical Engineering, University of Belgrade.
oljakovljevic 0:0093b6d26596 10 Version (1), July 2022.
oljakovljevic 0:0093b6d26596 11
oljakovljevic 0:0093b6d26596 12 */
oljakovljevic 0:0093b6d26596 13
oljakovljevic 0:0093b6d26596 14 // Include the libraries
oljakovljevic 0:0093b6d26596 15 #include "mbed.h"
oljakovljevic 0:0093b6d26596 16 #include "platform/mbed_thread.h"
oljakovljevic 0:0093b6d26596 17 #include <string>
oljakovljevic 0:0093b6d26596 18 #include <algorithm>
oljakovljevic 0:0093b6d26596 19 #include "Servo.h"
oljakovljevic 0:0093b6d26596 20 #include "MQTTClientMbedOs.h"
oljakovljevic 0:0093b6d26596 21
oljakovljevic 0:0093b6d26596 22 #define MAX_NETWORKS 10
oljakovljevic 0:0093b6d26596 23 #define PRINTF_DELAY_MS 10
oljakovljevic 0:0093b6d26596 24 #define YIELD_TIMEOUT_MS 1000
oljakovljevic 0:0093b6d26596 25
oljakovljevic 0:0093b6d26596 26 #define PSAdress (0x38<<1)
oljakovljevic 0:0093b6d26596 27
oljakovljevic 0:0093b6d26596 28 Servo myservo(D7); // servo motor control signal pin
oljakovljevic 0:0093b6d26596 29 Serial pc(USBTX,USBRX);
oljakovljevic 0:0093b6d26596 30
oljakovljevic 0:0093b6d26596 31 // Proximity sensor parameters
oljakovljevic 0:0093b6d26596 32 I2C i2c(PB_14,PB_13);
oljakovljevic 0:0093b6d26596 33
oljakovljevic 0:0093b6d26596 34 int PSresult = 0;
oljakovljevic 0:0093b6d26596 35 char modeControlRegister[2] = {0x41, 0xC6}; // Mode Control Register - {Adress, Value}
oljakovljevic 0:0093b6d26596 36 char PSregadRegister [2] = {0x44, 0x45}; // PS register values - {PSLSB Adress, PSMSB Adress}
oljakovljevic 0:0093b6d26596 37 char PSReading [2];
oljakovljevic 0:0093b6d26596 38 int poximityTreshold = 280;
oljakovljevic 0:0093b6d26596 39
oljakovljevic 0:0093b6d26596 40 TCPSocket socket;
oljakovljevic 0:0093b6d26596 41 MQTTClient client(&socket);
oljakovljevic 0:0093b6d26596 42 MQTT::Message message;
oljakovljevic 0:0093b6d26596 43
oljakovljevic 0:0093b6d26596 44 char* topic_pub_proximity2 = "PMK_industry/micro/proximity2";
oljakovljevic 0:0093b6d26596 45 char* topic_sub_servo2 = "PMK_industry/micro/servo2";
oljakovljevic 0:0093b6d26596 46
oljakovljevic 0:0093b6d26596 47 char* receavedMessage;
oljakovljevic 0:0093b6d26596 48
oljakovljevic 0:0093b6d26596 49 WiFiInterface *wifi;
oljakovljevic 0:0093b6d26596 50 volatile int mems_event = 0;
oljakovljevic 0:0093b6d26596 51 uint32_t previous_tick = 0;
oljakovljevic 0:0093b6d26596 52 uint32_t current_tick = 0;
oljakovljevic 0:0093b6d26596 53 uint8_t high = 0, low = 0;
oljakovljevic 0:0093b6d26596 54 float temperature = 0.0f;
oljakovljevic 0:0093b6d26596 55 char buffer[32];
oljakovljevic 0:0093b6d26596 56
oljakovljevic 0:0093b6d26596 57 int flag = 0;
oljakovljevic 0:0093b6d26596 58
oljakovljevic 0:0093b6d26596 59 const char *sec2str(nsapi_security_t sec)
oljakovljevic 0:0093b6d26596 60 {
oljakovljevic 0:0093b6d26596 61 switch (sec)
oljakovljevic 0:0093b6d26596 62 {
oljakovljevic 0:0093b6d26596 63 case NSAPI_SECURITY_NONE:
oljakovljevic 0:0093b6d26596 64 return "None";
oljakovljevic 0:0093b6d26596 65 case NSAPI_SECURITY_WEP:
oljakovljevic 0:0093b6d26596 66 return "WEP";
oljakovljevic 0:0093b6d26596 67 case NSAPI_SECURITY_WPA:
oljakovljevic 0:0093b6d26596 68 return "WPA";
oljakovljevic 0:0093b6d26596 69 case NSAPI_SECURITY_WPA2:
oljakovljevic 0:0093b6d26596 70 return "WPA2";
oljakovljevic 0:0093b6d26596 71 case NSAPI_SECURITY_WPA_WPA2:
oljakovljevic 0:0093b6d26596 72 return "WPA/WPA2";
oljakovljevic 0:0093b6d26596 73 case NSAPI_SECURITY_UNKNOWN:
oljakovljevic 0:0093b6d26596 74 default:
oljakovljevic 0:0093b6d26596 75 return "Unknown";
oljakovljevic 0:0093b6d26596 76 }
oljakovljevic 0:0093b6d26596 77 }
oljakovljevic 0:0093b6d26596 78
oljakovljevic 0:0093b6d26596 79 int scan_networks(WiFiInterface *wifi)
oljakovljevic 0:0093b6d26596 80 {
oljakovljevic 0:0093b6d26596 81 printf("Scan:\n");
oljakovljevic 0:0093b6d26596 82
oljakovljevic 0:0093b6d26596 83 // Scan only for the number of networks, first parameter is NULL
oljakovljevic 0:0093b6d26596 84 int count = wifi->scan(NULL, 0);
oljakovljevic 0:0093b6d26596 85
oljakovljevic 0:0093b6d26596 86 // If there are no networks, count == 0, if there is an error, counter < 0
oljakovljevic 0:0093b6d26596 87 if (count <= 0)
oljakovljevic 0:0093b6d26596 88 {
oljakovljevic 0:0093b6d26596 89 printf("scan() failed with return value: %d\n", count);
oljakovljevic 0:0093b6d26596 90 return 0;
oljakovljevic 0:0093b6d26596 91 }
oljakovljevic 0:0093b6d26596 92
oljakovljevic 0:0093b6d26596 93 // Limit number of network arbitrary to some reasonable number
oljakovljevic 0:0093b6d26596 94 count = count < MAX_NETWORKS ? count : MAX_NETWORKS;
oljakovljevic 0:0093b6d26596 95
oljakovljevic 0:0093b6d26596 96 // Create a local pointer to an object, which is an array of WiFi APs
oljakovljevic 0:0093b6d26596 97 WiFiAccessPoint *ap = new WiFiAccessPoint[count];
oljakovljevic 0:0093b6d26596 98
oljakovljevic 0:0093b6d26596 99 // Now scan again for 'count' networks and populate the array of APs
oljakovljevic 0:0093b6d26596 100 count = wifi->scan(ap, count);
oljakovljevic 0:0093b6d26596 101
oljakovljevic 0:0093b6d26596 102 // This time, the number of entries to 'ap' is returned
oljakovljevic 0:0093b6d26596 103 if (count <= 0)
oljakovljevic 0:0093b6d26596 104 {
oljakovljevic 0:0093b6d26596 105 printf("scan() failed with return value: %d\n", count);
oljakovljevic 0:0093b6d26596 106 return 0;
oljakovljevic 0:0093b6d26596 107 }
oljakovljevic 0:0093b6d26596 108
oljakovljevic 0:0093b6d26596 109 // Print out the parameters of each AP:
oljakovljevic 0:0093b6d26596 110 for (int i = 0; i < count; i++)
oljakovljevic 0:0093b6d26596 111 {
oljakovljevic 0:0093b6d26596 112 printf("Network: %s secured: %s BSSID: %hhX:%hhX:%hhX:%hhx:%hhx:%hhx RSSI: %hhd Ch: %hhd\n", ap[i].get_ssid(),
oljakovljevic 0:0093b6d26596 113 sec2str(ap[i].get_security()), ap[i].get_bssid()[0], ap[i].get_bssid()[1], ap[i].get_bssid()[2],
oljakovljevic 0:0093b6d26596 114 ap[i].get_bssid()[3], ap[i].get_bssid()[4], ap[i].get_bssid()[5], ap[i].get_rssi(), ap[i].get_channel());
oljakovljevic 0:0093b6d26596 115 thread_sleep_for(PRINTF_DELAY_MS);
oljakovljevic 0:0093b6d26596 116 }
oljakovljevic 0:0093b6d26596 117 printf("%d networks available.\n", count);
oljakovljevic 0:0093b6d26596 118
oljakovljevic 0:0093b6d26596 119 // Since 'ap' is dynamically allocated pointer to the array of objects, it
oljakovljevic 0:0093b6d26596 120 // needs to be deleted:
oljakovljevic 0:0093b6d26596 121 delete[] ap;
oljakovljevic 0:0093b6d26596 122 return count;
oljakovljevic 0:0093b6d26596 123 }
oljakovljevic 0:0093b6d26596 124
oljakovljevic 0:0093b6d26596 125 void messageArrivedServo2(MQTT::MessageData& md)
oljakovljevic 0:0093b6d26596 126 {
oljakovljevic 0:0093b6d26596 127 MQTT::Message &message = md.message;
oljakovljevic 0:0093b6d26596 128 receavedMessage = (char*)message.payload;
oljakovljevic 0:0093b6d26596 129
oljakovljevic 0:0093b6d26596 130 if (strcmp(receavedMessage,"zero") == 0){ flag = 0;}
oljakovljevic 0:0093b6d26596 131 else if (strcmp(receavedMessage,"left") == 0){ flag = 1;}
oljakovljevic 0:0093b6d26596 132 else if (strcmp(receavedMessage,"right") == 0) { flag = 2;}
oljakovljevic 0:0093b6d26596 133
oljakovljevic 0:0093b6d26596 134 }
oljakovljevic 0:0093b6d26596 135
oljakovljevic 0:0093b6d26596 136
oljakovljevic 0:0093b6d26596 137 int main()
oljakovljevic 0:0093b6d26596 138
oljakovljevic 0:0093b6d26596 139 {
oljakovljevic 0:0093b6d26596 140 const char* hostname = "broker.hivemq.com";
oljakovljevic 0:0093b6d26596 141 int port = 1883;
oljakovljevic 0:0093b6d26596 142
oljakovljevic 0:0093b6d26596 143 // Create a default network interface
oljakovljevic 0:0093b6d26596 144 wifi = WiFiInterface::get_default_instance();
oljakovljevic 0:0093b6d26596 145 if (!wifi) {
oljakovljevic 0:0093b6d26596 146 printf("ERROR: No WiFiInterface found.\n");
oljakovljevic 0:0093b6d26596 147 return -1;
oljakovljevic 0:0093b6d26596 148 }
oljakovljevic 0:0093b6d26596 149
oljakovljevic 0:0093b6d26596 150 // Scan for available networks and aquire information about Access Points
oljakovljevic 0:0093b6d26596 151 int count = scan_networks(wifi);
oljakovljevic 0:0093b6d26596 152 if (count == 0) {
oljakovljevic 0:0093b6d26596 153 printf("No WIFI APs found - can't continue further.\n");
oljakovljevic 0:0093b6d26596 154 return -1;
oljakovljevic 0:0093b6d26596 155 }
oljakovljevic 0:0093b6d26596 156
oljakovljevic 0:0093b6d26596 157 // Connect to the network with the parameters specified in 'mbed_app.json'
oljakovljevic 0:0093b6d26596 158 printf("\nConnecting to %s...\n", MBED_CONF_APP_WIFI_SSID);
oljakovljevic 0:0093b6d26596 159 int ret = wifi->connect(MBED_CONF_APP_WIFI_SSID, MBED_CONF_APP_WIFI_PASSWORD, NSAPI_SECURITY_WPA_WPA2);
oljakovljevic 0:0093b6d26596 160 if (ret != 0) {
oljakovljevic 0:0093b6d26596 161 printf("\nConnection error: %d\n", ret);
oljakovljevic 0:0093b6d26596 162 return -1;
oljakovljevic 0:0093b6d26596 163 }
oljakovljevic 0:0093b6d26596 164
oljakovljevic 0:0093b6d26596 165 // Print out the information aquired:
oljakovljevic 0:0093b6d26596 166 printf("Success\n\n");
oljakovljevic 0:0093b6d26596 167 printf("MAC: %s\n", wifi->get_mac_address());
oljakovljevic 0:0093b6d26596 168 printf("IP: %s\n", wifi->get_ip_address());
oljakovljevic 0:0093b6d26596 169 printf("Netmask: %s\n", wifi->get_netmask());
oljakovljevic 0:0093b6d26596 170 printf("Gateway: %s\n", wifi->get_gateway());
oljakovljevic 0:0093b6d26596 171 printf("RSSI: %d\n\n", wifi->get_rssi());
oljakovljevic 0:0093b6d26596 172
oljakovljevic 0:0093b6d26596 173 socket.open(wifi);
oljakovljevic 0:0093b6d26596 174 socket.connect(hostname, port);
oljakovljevic 0:0093b6d26596 175
oljakovljevic 0:0093b6d26596 176 int rc=0;
oljakovljevic 0:0093b6d26596 177
oljakovljevic 0:0093b6d26596 178 MQTTPacket_connectData data = MQTTPacket_connectData_initializer;
oljakovljevic 0:0093b6d26596 179 data.MQTTVersion = 3;
oljakovljevic 0:0093b6d26596 180 data.clientID.cstring = "Client2";
oljakovljevic 0:0093b6d26596 181
oljakovljevic 0:0093b6d26596 182 if ((rc = client.connect(data)) != 0)
oljakovljevic 0:0093b6d26596 183 printf("rc from MQTT connect is %d\r\n", rc);
oljakovljevic 0:0093b6d26596 184
oljakovljevic 0:0093b6d26596 185
oljakovljevic 0:0093b6d26596 186 if ((rc = client.subscribe(topic_sub_servo2, MQTT::QOS2, messageArrivedServo2)) != 0)
oljakovljevic 0:0093b6d26596 187 printf("rc from MQTT subscribe is %d\r\n", rc);
oljakovljevic 0:0093b6d26596 188
oljakovljevic 0:0093b6d26596 189 // Start I2C communication
oljakovljevic 0:0093b6d26596 190 i2c.start();
oljakovljevic 0:0093b6d26596 191
oljakovljevic 0:0093b6d26596 192 // Set the Mode Control Register, so the sensor is waken up from the standby mode (default)
oljakovljevic 0:0093b6d26596 193 i2c.write(PSAdress, modeControlRegister, 2, 1);
oljakovljevic 0:0093b6d26596 194
oljakovljevic 0:0093b6d26596 195 myservo.position(0);
oljakovljevic 0:0093b6d26596 196
oljakovljevic 0:0093b6d26596 197 while (true) {
oljakovljevic 0:0093b6d26596 198
oljakovljevic 0:0093b6d26596 199 i2c.write(PSAdress, PSregadRegister, 2, 1); // Select the registers to read from, no I2C Stop
oljakovljevic 0:0093b6d26596 200 i2c.read(PSAdress, PSReading, 2, 0); // Read data from register
oljakovljevic 0:0093b6d26596 201 PSresult = (int) ((int)PSReading[1] << 8) | PSReading[0];
oljakovljevic 0:0093b6d26596 202
oljakovljevic 0:0093b6d26596 203 char buf[100];
oljakovljevic 0:0093b6d26596 204 sprintf(buf, "%d", PSresult);
oljakovljevic 0:0093b6d26596 205 message.qos = MQTT::QOS0;
oljakovljevic 0:0093b6d26596 206 message.retained = false;
oljakovljevic 0:0093b6d26596 207 message.dup = false;
oljakovljevic 0:0093b6d26596 208 message.payload = (void*)buf;
oljakovljevic 0:0093b6d26596 209 message.payloadlen = strlen(buf)+1;
oljakovljevic 0:0093b6d26596 210 client.publish(topic_pub_proximity2, message);
oljakovljevic 0:0093b6d26596 211
oljakovljevic 0:0093b6d26596 212 // Servo commands
oljakovljevic 0:0093b6d26596 213
oljakovljevic 0:0093b6d26596 214 if (flag == 0)
oljakovljevic 0:0093b6d26596 215 {
oljakovljevic 0:0093b6d26596 216 myservo.position(0);
oljakovljevic 0:0093b6d26596 217
oljakovljevic 0:0093b6d26596 218 }
oljakovljevic 0:0093b6d26596 219
oljakovljevic 0:0093b6d26596 220 else if (flag == 1)
oljakovljevic 0:0093b6d26596 221 {
oljakovljevic 0:0093b6d26596 222 myservo.position(25);
oljakovljevic 0:0093b6d26596 223
oljakovljevic 0:0093b6d26596 224 }
oljakovljevic 0:0093b6d26596 225
oljakovljevic 0:0093b6d26596 226 else if (flag == 2)
oljakovljevic 0:0093b6d26596 227 {
oljakovljevic 0:0093b6d26596 228 myservo.position(-25);
oljakovljevic 0:0093b6d26596 229
oljakovljevic 0:0093b6d26596 230 }
oljakovljevic 0:0093b6d26596 231
oljakovljevic 0:0093b6d26596 232 client.yield(YIELD_TIMEOUT_MS); // Need to call yield API to maintain connection
oljakovljevic 0:0093b6d26596 233 }
oljakovljevic 0:0093b6d26596 234 }
oljakovljevic 0:0093b6d26596 235