Revision 0:3b4906b8a747, committed 2019-12-10

Comitter:

hyan99

Date:

Tue Dec 10 22:29:09 2019 +0000

Child:

1:19c3299ea83a

Commit message:

error

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/AccCar.cpp	Tue Dec 10 22:29:09 2019 +0000
@@ -0,0 +1,102 @@
+#include "AccCar.h"
+#include <stdlib.h>
+#include <algorithm>
+
+#define TICK 1000
+#define MONITOR_DIST 17
+#define SAFETY_GAP 2
+#define STOP 54
+#define MIN_SPEED 5
+#define MAX_SPEED 15
+
+Serial pc_acc(USBTX, USBRX);
+
+AccCar::AccCar(int id, Road* road, int flag, char* t1, char* t2) {
+    this->id = id;
+    this->road = road;
+    this->flag = flag;
+
+    this->thread = NULL;
+    this->forward_car = NULL;
+
+    this->cycle = 0;
+    this->comm = Communication::getInstance();
+    this->topic_position = t1;
+    this->topic_control = t2;
+//    communication->subscribe_to_position(topic_position);
+    comm->subscribe_to_topic_control(topic_control);
+}
+
+void AccCar::set_forward_car(AccCar* car) {
+    this->forward_car = car;
+}
+
+void AccCar::update() {
+    bool crossed = false;
+    int wait = 0;
+    while (true) {
+        ThisThread::sleep_for(TICK);
+        road->go_flags.wait_all(flag);
+
+        position = position + speed;
+
+        if (forward_car != NULL && forward_car->position - position < MONITOR_DIST) {
+            road->done_flags.wait_all(forward_car->flag, osWaitForever, false);
+
+            int diff = forward_car->position - position;
+            int maxSafeSpeed = diff - SAFETY_GAP;
+
+            speed = std::min(maxSafeSpeed, target_speed);
+        } else {
+            speed = target_speed;
+        }
+        // TODO: publish Position and Acc-speed
+        comm->publish_car(topic_position, speed, position);
+        Communication::control_flags.wait_all(flag); // wait for speed instruction from controller
+        speed = Communication::speeds[id - 1];
+//        // Crossing
+//        if (position < STOP) {
+//            speed = std::min(speed, STOP - position);
+//        } else if (!crossed && position == STOP) {
+//            speed = 0;
+//            if(wait == 0){
+//              road->intersection->add_to_q(this);
+//            }
+//            wait++;
+//            if(road->intersection->can_cross(this)) {
+//                crossed = true;
+//            }
+//        } else if (position < STOP + 2) {
+//            speed = 1;
+//        } else if(position == STOP + 2) {
+//            road->intersection->remove_from_q();
+//        }
+
+        cycle = (cycle + 1) % 5;
+        if (cycle == 0) {
+            target_speed = rand() % (MAX_SPEED - MIN_SPEED + 1) + MIN_SPEED;
+        }
+        road->done_flags.set(flag);
+    }
+}
+
+void AccCar::reset(int speed) {
+    road->done_flags.clear(flag);
+
+    if (thread != NULL) {
+        thread->terminate();
+    }
+
+    thread = new Thread();
+    thread->start( callback(this, &AccCar::update) );
+
+    this->position = 0;
+    this->speed = speed;
+    this->target_speed = speed;
+}
+
+void AccCar::stop() {
+    if (thread != NULL) {
+        thread->terminate();
+    }
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/AccCar.h	Tue Dec 10 22:29:09 2019 +0000
@@ -0,0 +1,40 @@
+#ifndef _ACC_CAR_H_
+#define _ACC_CAR_H_
+
+#include "mbed.h"
+#include "Car.h"
+#include "Road.h"
+#include "Communication.h"
+
+class Road;
+class Car;
+
+class AccCar {
+public:
+// Inherited from class Car
+    int position;
+    int speed;
+    int flag;
+    
+    AccCar(int id, Road* road, int flag, char* t1, char* t2);
+    void set_forward_car(AccCar* car);
+    void update();
+    void reset(int speed);
+    void stop();
+    
+public:
+    int id;
+    int target_speed;
+    AccCar* forward_car;
+    
+    Road* road;
+    Thread* thread;
+    Communication *comm;
+    
+    char* topic_position;
+    char* topic_control;
+     
+protected:
+    int cycle;
+};
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Car.cpp	Tue Dec 10 22:29:09 2019 +0000
@@ -0,0 +1,61 @@
+#include "Car.h"
+#include <stdlib.h>
+
+#define TICK 1000
+
+Car::Car(int id, Road* road, int flag) {
+    this->id = id;   
+    this->road = road;
+    this->flag = flag;
+    
+    cycle = 0;
+    
+    this->thread = NULL;
+}
+
+void Car::update() {
+    bool crossed = false;
+    while (true) {
+        ThisThread::sleep_for(TICK);
+        road->go_flags.wait_all(flag);
+        cycle++;
+        
+        position = position + speed;
+        
+        if (cycle % 5 == 0) {
+            speed = rand() % 11 + 5;
+        }
+        
+        if (position < 54) {
+            speed = std::min(speed, 54 - position);
+        } else if (!crossed && position == 54) {
+            speed = 0;
+            crossed = true;
+        } else if (position < 56) {
+            speed = 1;
+        }
+        
+        road->done_flags.set(flag);   
+    }
+}
+
+void Car::reset(int position, int speed) {
+    road->done_flags.clear(flag);
+    
+    if (thread != NULL) {
+        thread->terminate();   
+    }
+    
+    thread = new Thread();
+    thread->start( callback(this, &Car::update) );
+    
+    cycle = 0;
+    this->position = position;
+    this->speed = speed;
+}
+
+void Car::stop() {
+    if (thread != NULL) {
+        thread->terminate();   
+    } 
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Car.h	Tue Dec 10 22:29:09 2019 +0000
@@ -0,0 +1,26 @@
+#ifndef _CAR_H_
+#define _CAR_H_
+
+#include "Road.h"
+#include "mbed.h"
+
+class Road;
+
+class Car {
+public:
+    int position;
+    int speed;
+    int flag;
+    
+    Car(int id, Road* road, int flag);
+    void update();
+    void reset(int position, int speed);
+    void stop();
+    
+protected:
+    int id;
+    int cycle;
+    Road* road;
+    Thread* thread; 
+};
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Communication.cpp	Tue Dec 10 22:29:09 2019 +0000
@@ -0,0 +1,235 @@
+#include "Communication.h"
+
+Communication* Communication::singleton = NULL;
+int Communication::speeds[5] = {0, 0, 0, 0, 0};
+int Communication::sync = 0;
+int Communication::flags[5] = {0x01, 0x02, 0x04, 0x08, 0x10};
+EventFlags Communication::control_flags;
+EventFlags Communication::sync_flags;
+
+Serial pc_comm(USBTX, USBRX);
+
+Communication* Communication::getInstance() {
+    if (singleton == NULL) {
+        Communication();
+    }
+    return singleton;
+}
+
+Communication::Communication() 
+{
+    singleton = this;
+    setup_wifi();
+    if (wifi == NULL) {
+        pc_comm.printf("Failed to set up Wifi.");
+    }
+    MQTTNetwork network(wifi);
+    setup_mqtt(network);
+    if (client == NULL) {
+        pc_comm.printf("Failed to set up Client.");
+    }
+}
+
+void Communication::setup_wifi() {
+    // Get a handle to the WiFi module
+    wifi = WiFiInterface::get_default_instance();
+    
+    // Connect the module to the wifi, based on the SSID and password 
+    // specified in the mbed_app.json configuration file
+    // If you are using AirPennNet-Device, this will not succeed until the MAC
+    // address (printed shortly after this) is registered
+    pc_comm.printf("Connecting to wifi\r\n");
+    int rc = wifi->connect(MBED_CONF_APP_WIFI_SSID, MBED_CONF_APP_WIFI_PASSWORD, NSAPI_SECURITY_WPA_WPA2);
+    
+    // Print out the MAC address of the wifi module. The MAC address is 
+    // needed to register the device with AirPennNet-Device, so that you
+    // can use the campus wifi
+    pc_comm.printf("MAC Address: ");
+    pc_comm.printf(wifi->get_mac_address());
+    pc_comm.printf("\r\n");
+    
+    if (rc != 0) {
+        pc_comm.printf("Problem connecting to wifi\r\n");   
+        return;
+    } else {
+        pc_comm.printf("Wifi connected\r\n");  
+    }
+}
+
+
+void Communication::setup_mqtt(MQTTNetwork &network) {
+    // the hostname and port point to a Google Cloud MQTT server we setup for
+    // this project
+    const char* hostname = "34.68.206.11";
+    int port = 1883;
+    
+    // Create the underlying network connection to the MQTT server
+    pc_comm.printf("Connecting to %s:%d\r\n", hostname, port);
+    int rc = network.connect(hostname, port);
+    if (rc != 0) {
+        pc_comm.printf("There was an error with the TCP connect: %d\r\n", rc);
+        return;
+    }
+    
+    pc_comm.printf("Connected to %s:%d\r\n", hostname, port);
+        
+    // Connect the MQTT client to the server
+    client = new MQTT::Client<MQTTNetwork, Countdown>(network);
+    rc = client->connect();
+    if (rc != 0) {
+        pc_comm.printf("There was an error with the MQTT connect: %d\r\n", rc);
+        return;
+    }
+    
+    pc_comm.printf("MQTT connect successful!\r\n");
+}
+
+void Communication::message_arrived(MQTT::MessageData& md)
+{
+    MQTT::Message &message = md.message;
+    pc_comm.printf("Message arrived: qos %d, retained %d, dup %d, packetid %d\r\n", message.qos, message.retained, message.dup, message.id);
+    pc_comm.printf("Payload %.*s\r\n", message.payloadlen, (char*)message.payload);
+}
+
+void Communication::control_arrived(MQTT::MessageData& md) {
+    MQTT::Message &message = md.message;
+    
+    char* payload = (char*) message.payload;
+    
+    int id = (int) payload[0];
+    int speed = (int) payload[1];
+    
+    speeds[id-1] = speed;
+    control_flags.set(flags[id-1]);
+}
+
+void Communication::sync_arrived(MQTT::MessageData& md) {
+    MQTT::Message &message = md.message;
+    
+    char* payload = (char*) message.payload;
+    sync = (int) payload[0];
+    sync_flags.set(0x01);
+}
+
+int Communication::send_message(char* topic) {
+    pc_comm.printf("Sending a message!\r\n");
+    MQTT::Message message;
+ 
+    char buf[100];
+    sprintf(buf, "Hello World!  This is a test message!\r\n");
+    int rc = client->publish(topic, (char*) buf, strlen(buf)+1, MQTT::QOS1);
+    
+    if (rc != 0) {
+        return -1;   
+    } else {
+        pc_comm.printf("Message sent!\r\n");
+        return 0;
+    }
+}
+
+int Communication::subscribe_to_topic_position(char* topic) {
+    int rc = client->subscribe(topic, MQTT::QOS1, message_arrived);
+    if (rc != 0) {
+        pc_comm.printf("There was a problem subscribing: %d\r\n", rc);
+        
+//        client->disconnect();
+        return -1;
+    } else {
+        pc_comm.printf("Subscribed!\r\n");
+    }
+    return rc;
+}
+
+int Communication::subscribe_to_topic_control(char* topic) {
+    int rc = client->subscribe(topic, MQTT::QOS1, control_arrived);
+    if (rc != 0) {
+        pc_comm.printf("There was a problem subscribing: %d\r\n", rc);
+        
+//        client->disconnect();
+        return -1;
+    } else {
+        pc_comm.printf("Subscribed!\r\n");
+    }
+    return rc;
+}
+
+int Communication::subscribe_to_topic_sync(char* topic) {
+    int rc = client->subscribe(topic, MQTT::QOS1, message_arrived);
+    if (rc != 0) {
+        pc_comm.printf("There was a problem subscribing: %d\r\n", rc);
+        
+//        client->disconnect();
+        return -1;
+    } else {
+        pc_comm.printf("Subscribed!\r\n");
+    }
+    return rc;
+}
+    
+void Communication::publish_car(char* topic, int speed, int position) {
+    message_t *message = mpool.alloc();
+    message->topic = topic;
+    message->speed = speed;
+    message->position = position;
+    q_car.put(message);
+}
+
+void Communication::publish_road(char* topic, int num) {
+    message_t *message = mpool.alloc();
+    message->topic = topic;
+    message->speed = -1; // indicate this is for sync
+    message->position = num;
+    q_car.put(message);
+}
+
+void Communication::start() {
+    while(1) {
+        osEvent evt = q_car.get();
+        if (evt.status == osEventMessage) {
+            message_t *message = (message_t*)evt.value.p;
+            char* topic = message->topic;
+            
+            if (message->speed == -1) {
+                char buf[1];
+                buf[0] = (char) message->position; // update number
+                
+                int rc = client->publish(topic, (char*) buf, strlen(buf)+1, MQTT::QOS1);
+                if (rc != 0) {
+                    pc_comm.printf("Failed to publish Road info: %s", topic);
+                } else {
+                    pc_comm.printf("Message sent!\r\n");
+                }
+            } else {
+                char buf[2];
+                buf[0] = (char) message->position;
+                buf[1] = (char) message->speed;
+                
+                int rc = client->publish(topic, (char*) buf, strlen(buf)+1, MQTT::QOS1);
+                if (rc != 0) {
+                    pc_comm.printf("Failed to publish AccCar info: %s", topic);
+                } else {
+                    pc_comm.printf("Message sent!\r\n");
+                }
+            }
+            mpool.free(message);
+            client->yield(100); // wait for 100ms
+        }
+    }
+}
+
+void Communication::reset() {
+    control_flags.clear();
+    
+    if (thread != NULL) {
+        thread->terminate();
+    }
+
+    thread = new Thread();
+    thread->start( callback(this, &Communication::start) );
+}
+
+void Communication::stop() {
+    if (thread != NULL) {
+        thread->terminate();
+    }
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Communication.h	Tue Dec 10 22:29:09 2019 +0000
@@ -0,0 +1,57 @@
+#ifndef _COMMUNICATION_H_
+#define _COMMUNICATION_H_
+
+#include "mbed.h"
+#include "MQTTNetwork.h"
+#include "MQTTClient.h"
+#include "MQTTmbed.h"
+
+
+typedef struct {
+    int    speed; // speed for AccCar, -1 for Road
+    int    position; // position for AccCar, update number for Road
+    char*  topic;
+} message_t;
+
+class Communication {
+public:
+    static EventFlags control_flags; // For cars waiting on speed control
+    static EventFlags sync_flags;
+    static int speeds[5];
+    static int sync;
+    static int flags[5];
+    
+    // Singleton
+    static Communication *getInstance();
+    
+    // MQTT Setup
+    void setup_wifi();
+    void setup_mqtt(MQTTNetwork &network);
+    static void message_arrived(MQTT::MessageData& md);
+    static void control_arrived(MQTT::MessageData& md);
+    static void sync_arrived(MQTT::MessageData& md);
+    int send_message(char* topic);
+    int subscribe_to_topic_position(char* topic);
+    int subscribe_to_topic_control(char* topic);
+    int subscribe_to_topic_sync(char* topic);
+    void publish_car(char* topic, int speed, int position);
+    void publish_road(char* topic, int num);
+    void start(); // function for serving requests (single thread)
+    void reset(); // function to create and start the singleton thread
+    void stop();
+    
+private:
+    static Communication *singleton;
+    
+    WiFiInterface *wifi;
+    MQTT::Client<MQTTNetwork, Countdown> *client;
+    Thread* thread;
+    
+    MemoryPool<message_t, 10> mpool;
+    Queue<message_t, 10> q_car;
+    
+    Communication();
+    
+};
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Intersection.cpp	Tue Dec 10 22:29:09 2019 +0000
@@ -0,0 +1,24 @@
+#include "Intersection.h"
+
+void Intersection::add_to_q(AccCar* car) {
+  qmutex.lock();
+  crossQ.push(car);
+  qmutex.unlock();
+}
+
+void Intersection::remove_from_q() {
+  qmutex.lock();
+  crossQ.pop();
+  qmutex.unlock();
+}
+
+bool Intersection::can_cross(AccCar* car) {
+  // FCFS
+  qmutex.lock();
+  if(crossQ.front() == car) {
+    qmutex.unlock();
+    return true;
+  }
+  qmutex.unlock();
+  return false;
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Intersection.h	Tue Dec 10 22:29:09 2019 +0000
@@ -0,0 +1,25 @@
+#ifndef _INTERSECTION_H_
+#define _INTERSECTION_H_
+
+#include "mbed.h"
+#include "Car.h"
+#include "AccCar.h"
+
+#include <queue>
+
+class Road;
+class AccCar;
+
+class Intersection {
+public:
+  Road* road1;
+  Road* road2;
+  std::queue<AccCar*>crossQ;
+  Mutex qmutex;
+
+  void add_to_q(AccCar* car);
+  void remove_from_q();
+  bool can_cross(AccCar* car);
+};
+
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MQTT.lib	Tue Dec 10 22:29:09 2019 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/teams/mqtt/code/MQTT/#9cff7b6bbd01

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MQTTNetwork.h	Tue Dec 10 22:29:09 2019 +0000
@@ -0,0 +1,44 @@
+#ifndef _MQTTNETWORK_H_
+#define _MQTTNETWORK_H_
+
+#include "NetworkInterface.h"
+
+class MQTTNetwork {
+public:
+    MQTTNetwork(NetworkInterface* aNetwork) : network(aNetwork) {
+        socket = new TCPSocket();
+    }
+
+    ~MQTTNetwork() {
+        delete socket;
+    }
+
+    int read(unsigned char* buffer, int len, int timeout) {
+        socket->set_blocking(false);
+        socket->set_timeout(timeout);
+        int ret = socket->recv(buffer, len);
+        if (NSAPI_ERROR_WOULD_BLOCK == ret)
+            return 0;
+        else
+            return ret;
+}
+
+    int write(unsigned char* buffer, int len, int timeout) {
+        return socket->send(buffer, len);
+    }
+
+    int connect(const char* hostname, int port) {
+        socket->open(network);
+        return socket->connect(hostname, port);
+    }
+
+    int disconnect() {
+        return socket->close();
+    }
+
+private:
+    NetworkInterface* network;
+    TCPSocket* socket;
+};
+
+#endif // _MQTTNETWORK_H_

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Road.cpp	Tue Dec 10 22:29:09 2019 +0000
@@ -0,0 +1,57 @@
+#include "Road.h"
+
+Serial pc_road(USBTX, USBRX);
+
+Road::Road(char* t1, char* t2) {
+    active_cars = 0x00;
+    topic_send = t1; // road publish, controller receive
+    topic_receive = t2; // road receive, controller publish
+    comm = Communication::getInstance();
+    comm->subscribe_to_topic_sync(topic_receive); // for receiving sync messages
+    n = 0;
+}
+
+void Road::add_car(Car* car) {
+    // this->car1 = car;
+
+    active_cars = active_cars | car->flag;
+}
+
+void Road::add_acc_car(AccCar* car, int id) {
+    active_cars = active_cars | car->flag;
+
+    switch(id) {
+      case 1: {
+        this->car1 = car;
+        break;
+      }
+      case 2: {
+        this->car2 = car;
+        break;
+      }
+      case 3: {
+        this->car3 = car;
+        break;
+      }
+      case 4: {
+        this->car4 = car;
+        break;
+      }
+      case 5: {
+        this->car5 = car;
+        break;
+      }
+    }
+}
+
+void Road::let_cars_update() {
+    go_flags.set(active_cars);
+}
+
+void Road::wait_for_car_update() {
+    done_flags.wait_all(active_cars);
+    n = n + 1;
+    comm->publish_road(topic_send, n);
+    Communication::sync_flags.wait_all(0x01);
+    n = Communication::sync;
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Road.h	Tue Dec 10 22:29:09 2019 +0000
@@ -0,0 +1,40 @@
+#ifndef _ROAD_H_
+#define _ROAD_H_
+
+#include "mbed.h"
+#include "Car.h"
+#include "AccCar.h"
+#include "Intersection.h"
+#include "Communication.h"
+
+class Car;
+class AccCar;
+class Intersection;
+
+class Road {
+public:
+    EventFlags go_flags;
+    EventFlags done_flags;
+    Intersection *intersection;
+
+    Road(char* t1, char* t2);
+    void add_car(Car* car);
+    void add_acc_car(AccCar* car, int id);
+    void let_cars_update();
+    void wait_for_car_update();
+    
+private:
+    AccCar* car1;
+    AccCar* car2;
+    AccCar* car3;
+    AccCar* car4;
+    AccCar* car5;
+
+    int active_cars;
+    char* topic_send;
+    char* topic_receive;
+    Communication* comm;
+    
+    int n; // update number
+};
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/TextLCD.lib	Tue Dec 10 22:29:09 2019 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/simon/code/TextLCD/#308d188a2d3a

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Dec 10 22:29:09 2019 +0000
@@ -0,0 +1,183 @@
+/* mbed Microcontroller Library
+ * Copyright (c) 2018 ARM Limited
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#include "mbed.h"
+#include <cctype>
+#include "Car.h"
+#include "AccCar.h"
+#include "TextLCD.h"
+#include "Road.h"
+#include "Intersection.h"
+#include "Communication.h"
+
+#include <algorithm>
+#include <vector>
+#include <string>
+#include <stdlib.h>
+
+Serial pc(USBTX, USBRX);
+TextLCD lcd(p15, p16, p17, p18, p19, p20);
+
+#define ROADLENGTH 100
+#define STOP 54
+#define MIN_SPEED 5
+#define MAX_SPEED 15
+
+// Read the max number of services to perform from pc input
+int read_int(char* prompt) {
+    int maxService = 0;
+
+    pc.printf(prompt);
+
+    char input;
+    while(1) {
+        input = pc.getc();
+        pc.putc(input);
+
+        if( std::isdigit(input) ) {
+            maxService = (maxService * 10) + (input-'0');
+        } else {
+            pc.putc(input);
+            break;
+        }
+    }
+
+    return maxService;
+}
+
+// main() runs in its own thread in the OS
+int main()
+{
+
+    pc.printf("\nStarting simulation\n");
+    // ------------------------------------------------------------------------------
+    // The following three variables are used for timing statistics, do not modify them
+    Timer stopwatch;    // A timer to keep track of how long the updates take, for statistics purposes
+    int numberCycles = 0;
+    int totalUpdateTime = 0;
+    // ------------------------------------------------------------------------------
+
+    Intersection intersection;
+
+    // Initialize Communication
+    Communication* communication = Communication::getInstance();
+    // Initialize 5 AccCars and the Road
+    Road road1("Rahman/Sync/Receive/1", "Rahman/Sync/Send/1");
+    intersection.road1 = &road1;
+    road1.intersection = &intersection;
+    AccCar car11(1, &road1, 0x01, "Rahman/Position/1/1", "Rahman/Control/1/1");
+    AccCar car12(2, &road1, 0x02, "Rahman/Position/1/2", "Rahman/Control/1/2");
+    AccCar car13(3, &road1, 0x04, "Rahman/Position/1/3", "Rahman/Control/1/3");
+    AccCar car14(4, &road1, 0x08, "Rahman/Position/1/4", "Rahman/Control/1/4");
+    AccCar car15(5, &road1, 0x10, "Rahman/Position/1/5", "Rahman/Control/1/5");
+
+    std::vector<AccCar*> q1;
+    q1.push_back(&car15);
+    q1.push_back(&car14);
+    q1.push_back(&car13);
+    q1.push_back(&car12);
+    q1.push_back(&car11);
+
+    for (int i = 0; i < 4; i++) {
+        q1[i]->set_forward_car(q1[i+1]);
+    }
+
+    AccCar *lastCar1 = q1.front();
+
+    stopwatch.start();
+    
+    communication->reset(); // start thread
+
+    int interval = MAX_SPEED - MIN_SPEED + 1;
+    car11.reset(rand() % interval + MIN_SPEED); // set random speed [5, 15]
+    car12.reset(rand() % interval + MIN_SPEED);
+    car13.reset(rand() % interval + MIN_SPEED);
+    car14.reset(rand() % interval + MIN_SPEED);
+    car15.reset(rand() % interval + MIN_SPEED);
+
+    stopwatch.reset();
+
+    int waitTime1 = 0;
+
+    do {
+        int enterRoad1 = -1;
+        int enterCross1 = -1;
+
+        if (numberCycles == 0) {
+            // first car enters unconditionally
+            AccCar *car1 = q1.back();
+            enterRoad1 = car1->id;
+            road1.add_acc_car(car1, car1->id);
+            q1.pop_back();
+            waitTime1 = rand() % 4;
+        }
+        else {
+          if (q1.size() > 0) {
+            if (waitTime1 == 0) {
+                AccCar *car1 = q1.back();
+                if (car1->forward_car->position - 2 >= car1->speed) {
+                    road1.add_acc_car(car1, car1->id);
+                    enterRoad1 = car1->id;
+                    q1.pop_back();
+                    waitTime1 = rand() % 4;
+                }
+            } else {
+                waitTime1 = waitTime1 - 1;
+            }
+          }
+        }
+
+        road1.let_cars_update();
+        road1.wait_for_car_update();
+        // ------------------------------------------------------------------
+        // Timing statistics logic, do not modify
+        totalUpdateTime += stopwatch.read_ms();
+        numberCycles++;
+        stopwatch.reset();
+        // ------------------------------------------------------------------
+
+        if (car11.position == STOP) {
+            enterCross1 = 1;
+        } else if (car12.position == STOP) {
+            enterCross1 = 2;
+        } else if (car13.position == STOP) {
+            enterCross1 = 3;
+        } else if (car14.position == STOP) {
+            enterCross1 = 4;
+        } else if (car15.position == STOP) {
+            enterCross1 = 5;
+        }
+        lcd.cls();
+        if (enterRoad1 == -1 && enterCross1 == -1) {
+            lcd.printf("x, x\n");
+        } else if (enterRoad1 == -1) {
+            lcd.printf("x, %d\n", enterCross1);
+        } else if (enterCross1 == -1) {
+            lcd.printf("%d, x\n", enterRoad1);
+        } else {
+            lcd.printf("%d, %d\n", enterRoad1, enterCross1);
+        }
+
+        pc.printf("Car 1 on road 1: position %d, speed %d\r\n", car11.position, car11.speed);
+        pc.printf("Car 2 on road 1: position %d, speed %d\r\n", car12.position, car12.speed);
+        pc.printf("Car 3 on road 1: position %d, speed %d\r\n", car13.position, car13.speed);
+        pc.printf("Car 4 on road 1: position %d, speed %d\r\n", car14.position, car14.speed);
+        pc.printf("Car 5 on road 1: position %d, speed %d\r\n", car15.position, car15.speed);
+
+    } while (lastCar1->position <= ROADLENGTH);
+    car11.stop();
+    car12.stop();
+    car13.stop();
+    car14.stop();
+    car15.stop();
+    communication->stop();
+
+            // ----------------------------------------------------------------------
+    // Timing statistics printout, do not modify
+    pc.printf("Average update cycle took: %fms \r\n", (totalUpdateTime*1.0)/(numberCycles*1.0));
+    totalUpdateTime = 0;
+    numberCycles = 0;
+    // ----------------------------------------------------------------------
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed-os.lib	Tue Dec 10 22:29:09 2019 +0000
@@ -0,0 +1,1 @@
+https://github.com/ARMmbed/mbed-os/#3a57ec7401a77b8b98f6356a1498cb154229483f

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed_app.json	Tue Dec 10 22:29:09 2019 +0000
@@ -0,0 +1,25 @@
+{
+    "config": {
+        "wifi-ssid": {
+            "help": "WiFi SSID",
+            "value": "\"AslamahFi\""
+        },
+        "wifi-password": {
+            "help": "WiFi Password",
+            "value": "\"finewhatever\""
+        },
+        "wifi-tx" : {
+            "help" : "Wifi TX pin",
+            "value" : "p28"
+        },
+        "wifi-rx" : {
+            "help" : "Wifi RX pin",
+            "value" : "p27"
+        }
+    },
+    "target_overrides": {
+        "*": {
+            "rtos.thread-stack-size": 512
+        }
+    }
+}
\ No newline at end of file