main.cpp

00001 /* mbed Microcontroller Library
00002  * Copyright (c) 2018 ARM Limited
00003  * SPDX-License-Identifier: Apache-2.0
00004  */
00005 
00006 #include "mbed.h"
00007 #include <cctype>
00008 #include "Car.h"
00009 #include "AccCar.h"
00010 #include "TextLCD.h"
00011 #include "Road.h"
00012 #include "Intersection.h"
00013 #include "Communication.h"
00014 
00015 #include <algorithm>
00016 #include <vector>
00017 #include <queue>
00018 #include <string>
00019 #include <stdlib.h>
00020 
00021 Serial pc(USBTX, USBRX);
00022 TextLCD lcd(p15, p16, p17, p18, p19, p20);
00023 
00024 #define ROADLENGTH 100
00025 #define STOP 54
00026 #define MIN_SPEED 5
00027 #define MAX_SPEED 15
00028 
00029 // Read the max number of services to perform from pc input
00030 int read_int(char* prompt) {
00031     int maxService = 0;
00032 
00033     pc.printf(prompt);
00034 
00035     char input;
00036     while(1) {
00037         input = pc.getc();
00038         pc.putc(input);
00039 
00040         if( std::isdigit(input) ) {
00041             maxService = (maxService * 10) + (input-'0');
00042         } else {
00043             pc.putc(input);
00044             break;
00045         }
00046     }
00047 
00048     return maxService;
00049 }
00050 
00051 // main() runs in its own thread in the OS
00052 int main()
00053 {
00054 
00055     pc.printf("\nStarting simulation\n");
00056     // ------------------------------------------------------------------------------
00057     // The following three variables are used for timing statistics, do not modify them
00058     Timer stopwatch;    // A timer to keep track of how long the updates take, for statistics purposes
00059     int numberCycles = 0;
00060     int totalUpdateTime = 0;
00061     // ------------------------------------------------------------------------------
00062 
00063     Intersection intersection;
00064 
00065     // Initialize Communication
00066     
00067     char buf1[30] = "Rahman/Position/2";
00068     char buf2[30] = "Rahman/Control/2";
00069     char buf3[50] = "Rahman/Sync/Receive/2";
00070     char buf4[50] = "Rahman/Sync/Send/2";
00071     Communication* c = Communication::getInstance(buf1, buf2, buf3, buf4);
00072     
00073     // Initialize 5 AccCars and the Road
00074     
00075     Road road1(c);
00076     intersection.road1 = &road1;
00077     road1.intersection = &intersection;
00078     
00079     
00080     AccCar car11(1, &road1, 0x01, c);
00081     AccCar car12(2, &road1, 0x02, c);
00082     AccCar car13(3, &road1, 0x04, c);
00083     AccCar car14(4, &road1, 0x08, c);
00084     AccCar car15(5, &road1, 0x10, c);
00085 
00086     std::vector<AccCar*> q1;
00087     q1.push_back(&car15);
00088     q1.push_back(&car14);
00089     q1.push_back(&car13);
00090     q1.push_back(&car12);
00091     q1.push_back(&car11);
00092 
00093     for (int i = 0; i < 4; i++) {
00094         q1[i]->set_forward_car(q1[i+1]);
00095     }
00096 
00097     AccCar *lastCar1 = q1.front();
00098 
00099     stopwatch.start();
00100     
00101     pc.printf("Dispatching communication thread.\r\n");
00102     
00103     c->reset();
00104     
00105     int interval = MAX_SPEED - MIN_SPEED + 1;
00106     car11.reset(rand() % interval + MIN_SPEED); // set random speed [5, 15]
00107     car12.reset(rand() % interval + MIN_SPEED);
00108     car13.reset(rand() % interval + MIN_SPEED);
00109     car14.reset(rand() % interval + MIN_SPEED);
00110     car15.reset(rand() % interval + MIN_SPEED);
00111 
00112 //    c->reset();
00113     stopwatch.reset();
00114 
00115     int waitTime1 = 0;
00116 
00117     do {
00118         int enterRoad1 = -1;
00119         int enterCross1 = -1;
00120 
00121         if (numberCycles == 0) {
00122             // first car enters unconditionally
00123             AccCar *car1 = q1.back();
00124             enterRoad1 = car1->id;
00125             road1.add_acc_car(car1, car1->id);
00126             q1.pop_back();
00127             waitTime1 = rand() % 4;
00128         }
00129         else {
00130           if (q1.size() > 0) {
00131             if (waitTime1 == 0) {
00132                 AccCar *car1 = q1.back();
00133                 if (car1->forward_car->position - 2 >= car1->speed) {
00134                     road1.add_acc_car(car1, car1->id);
00135                     enterRoad1 = car1->id;
00136                     q1.pop_back();
00137                     waitTime1 = rand() % 4;
00138                 }
00139             } else {
00140                 waitTime1 = waitTime1 - 1;
00141             }
00142           }
00143         }
00144 
00145         road1.let_cars_update();
00146         road1.wait_for_car_update();
00147         // ------------------------------------------------------------------
00148         // Timing statistics logic, do not modify
00149         totalUpdateTime += stopwatch.read_ms();
00150         numberCycles++;
00151         stopwatch.reset();
00152         // ------------------------------------------------------------------
00153 
00154         if (car11.position == STOP) {
00155             enterCross1 = 1;
00156         } else if (car12.position == STOP) {
00157             enterCross1 = 2;
00158         } else if (car13.position == STOP) {
00159             enterCross1 = 3;
00160         } else if (car14.position == STOP) {
00161             enterCross1 = 4;
00162         } else if (car15.position == STOP) {
00163             enterCross1 = 5;
00164         }
00165         lcd.cls();
00166         if (enterRoad1 == -1 && enterCross1 == -1) {
00167             lcd.printf("x, x\n");
00168         } else if (enterRoad1 == -1) {
00169             lcd.printf("x, %d\n", enterCross1);
00170         } else if (enterCross1 == -1) {
00171             lcd.printf("%d, x\n", enterRoad1);
00172         } else {
00173             lcd.printf("%d, %d\n", enterRoad1, enterCross1);
00174         }
00175 
00176         pc.printf("Car 1 on road 1: position %d, speed %d\r\n", car11.position, car11.speed);
00177         pc.printf("Car 2 on road 1: position %d, speed %d\r\n", car12.position, car12.speed);
00178         pc.printf("Car 3 on road 1: position %d, speed %d\r\n", car13.position, car13.speed);
00179         pc.printf("Car 4 on road 1: position %d, speed %d\r\n", car14.position, car14.speed);
00180         pc.printf("Car 5 on road 1: position %d, speed %d\r\n", car15.position, car15.speed);
00181 
00182     } while (lastCar1->position <= ROADLENGTH);
00183     car11.stop();
00184     car12.stop();
00185     car13.stop();
00186     car14.stop();
00187     car15.stop();
00188     c->stop();
00189 
00190             // ----------------------------------------------------------------------
00191     // Timing statistics printout, do not modify
00192     pc.printf("Average update cycle took: %fms \r\n", (totalUpdateTime*1.0)/(numberCycles*1.0));
00193     totalUpdateTime = 0;
00194     numberCycles = 0;
00195     // ----------------------------------------------------------------------
00196 }