Han Yan / Mbed OS ms2b

Dependencies:   TextLCD MQTT

main.cpp@2:16b3bd337db2, 2019-12-11 (annotated)

Committer:
hyan99
Date:
Wed Dec 11 20:20:12 2019 +0000
Revision:
2:16b3bd337db2
Parent:
1:19c3299ea83a 
testing

Who changed what in which revision?

UserRevisionLine numberNew contents of line
hyan99 0:3b4906b8a747 1 /* mbed Microcontroller Library
hyan99 0:3b4906b8a747 2 * Copyright (c) 2018 ARM Limited
hyan99 0:3b4906b8a747 3 * SPDX-License-Identifier: Apache-2.0
hyan99 0:3b4906b8a747 4 */
hyan99 0:3b4906b8a747 5
hyan99 0:3b4906b8a747 6 #include "mbed.h"
hyan99 0:3b4906b8a747 7 #include <cctype>
hyan99 0:3b4906b8a747 8 #include "Car.h"
hyan99 0:3b4906b8a747 9 #include "AccCar.h"
hyan99 0:3b4906b8a747 10 #include "TextLCD.h"
hyan99 0:3b4906b8a747 11 #include "Road.h"
hyan99 0:3b4906b8a747 12 #include "Intersection.h"
hyan99 0:3b4906b8a747 13 #include "Communication.h"
hyan99 0:3b4906b8a747 14
hyan99 0:3b4906b8a747 15 #include <algorithm>
hyan99 0:3b4906b8a747 16 #include <vector>
hyan99 2:16b3bd337db2 17 #include <queue>
hyan99 0:3b4906b8a747 18 #include <string>
hyan99 0:3b4906b8a747 19 #include <stdlib.h>
hyan99 0:3b4906b8a747 20
hyan99 0:3b4906b8a747 21 Serial pc(USBTX, USBRX);
hyan99 0:3b4906b8a747 22 TextLCD lcd(p15, p16, p17, p18, p19, p20);
hyan99 0:3b4906b8a747 23
hyan99 0:3b4906b8a747 24 #define ROADLENGTH 100
hyan99 0:3b4906b8a747 25 #define STOP 54
hyan99 0:3b4906b8a747 26 #define MIN_SPEED 5
hyan99 0:3b4906b8a747 27 #define MAX_SPEED 15
hyan99 0:3b4906b8a747 28
hyan99 0:3b4906b8a747 29 // Read the max number of services to perform from pc input
hyan99 0:3b4906b8a747 30 int read_int(char* prompt) {
hyan99 0:3b4906b8a747 31 int maxService = 0;
hyan99 0:3b4906b8a747 32
hyan99 0:3b4906b8a747 33 pc.printf(prompt);
hyan99 0:3b4906b8a747 34
hyan99 0:3b4906b8a747 35 char input;
hyan99 0:3b4906b8a747 36 while(1) {
hyan99 0:3b4906b8a747 37 input = pc.getc();
hyan99 0:3b4906b8a747 38 pc.putc(input);
hyan99 0:3b4906b8a747 39
hyan99 0:3b4906b8a747 40 if( std::isdigit(input) ) {
hyan99 0:3b4906b8a747 41 maxService = (maxService * 10) + (input-'0');
hyan99 0:3b4906b8a747 42 } else {
hyan99 0:3b4906b8a747 43 pc.putc(input);
hyan99 0:3b4906b8a747 44 break;
hyan99 0:3b4906b8a747 45 }
hyan99 0:3b4906b8a747 46 }
hyan99 0:3b4906b8a747 47
hyan99 0:3b4906b8a747 48 return maxService;
hyan99 0:3b4906b8a747 49 }
hyan99 0:3b4906b8a747 50
hyan99 0:3b4906b8a747 51 // main() runs in its own thread in the OS
hyan99 0:3b4906b8a747 52 int main()
hyan99 0:3b4906b8a747 53 {
hyan99 0:3b4906b8a747 54
hyan99 0:3b4906b8a747 55 pc.printf("\nStarting simulation\n");
hyan99 0:3b4906b8a747 56 // ------------------------------------------------------------------------------
hyan99 0:3b4906b8a747 57 // The following three variables are used for timing statistics, do not modify them
hyan99 0:3b4906b8a747 58 Timer stopwatch; // A timer to keep track of how long the updates take, for statistics purposes
hyan99 0:3b4906b8a747 59 int numberCycles = 0;
hyan99 0:3b4906b8a747 60 int totalUpdateTime = 0;
hyan99 0:3b4906b8a747 61 // ------------------------------------------------------------------------------
hyan99 0:3b4906b8a747 62
hyan99 0:3b4906b8a747 63 Intersection intersection;
hyan99 0:3b4906b8a747 64
hyan99 0:3b4906b8a747 65 // Initialize Communication
hyan99 2:16b3bd337db2 66
hyan99 2:16b3bd337db2 67 char buf1[30] = "Rahman/Position/2";
hyan99 2:16b3bd337db2 68 char buf2[30] = "Rahman/Control/2";
hyan99 2:16b3bd337db2 69 char buf3[50] = "Rahman/Sync/Receive/2";
hyan99 2:16b3bd337db2 70 char buf4[50] = "Rahman/Sync/Send/2";
hyan99 2:16b3bd337db2 71 Communication* c = Communication::getInstance(buf1, buf2, buf3, buf4);
hyan99 2:16b3bd337db2 72
hyan99 0:3b4906b8a747 73 // Initialize 5 AccCars and the Road
hyan99 2:16b3bd337db2 74
hyan99 2:16b3bd337db2 75 Road road1(c);
hyan99 0:3b4906b8a747 76 intersection.road1 = &road1;
hyan99 0:3b4906b8a747 77 road1.intersection = &intersection;
hyan99 1:19c3299ea83a 78
hyan99 1:19c3299ea83a 79
hyan99 2:16b3bd337db2 80 AccCar car11(1, &road1, 0x01, c);
hyan99 2:16b3bd337db2 81 AccCar car12(2, &road1, 0x02, c);
hyan99 2:16b3bd337db2 82 AccCar car13(3, &road1, 0x04, c);
hyan99 2:16b3bd337db2 83 AccCar car14(4, &road1, 0x08, c);
hyan99 2:16b3bd337db2 84 AccCar car15(5, &road1, 0x10, c);
hyan99 0:3b4906b8a747 85
hyan99 0:3b4906b8a747 86 std::vector<AccCar*> q1;
hyan99 0:3b4906b8a747 87 q1.push_back(&car15);
hyan99 0:3b4906b8a747 88 q1.push_back(&car14);
hyan99 0:3b4906b8a747 89 q1.push_back(&car13);
hyan99 0:3b4906b8a747 90 q1.push_back(&car12);
hyan99 0:3b4906b8a747 91 q1.push_back(&car11);
hyan99 0:3b4906b8a747 92
hyan99 0:3b4906b8a747 93 for (int i = 0; i < 4; i++) {
hyan99 0:3b4906b8a747 94 q1[i]->set_forward_car(q1[i+1]);
hyan99 0:3b4906b8a747 95 }
hyan99 0:3b4906b8a747 96
hyan99 0:3b4906b8a747 97 AccCar *lastCar1 = q1.front();
hyan99 0:3b4906b8a747 98
hyan99 0:3b4906b8a747 99 stopwatch.start();
hyan99 0:3b4906b8a747 100
hyan99 2:16b3bd337db2 101 pc.printf("Dispatching communication thread.\r\n");
hyan99 2:16b3bd337db2 102
hyan99 2:16b3bd337db2 103 c->reset();
hyan99 2:16b3bd337db2 104
hyan99 0:3b4906b8a747 105 int interval = MAX_SPEED - MIN_SPEED + 1;
hyan99 0:3b4906b8a747 106 car11.reset(rand() % interval + MIN_SPEED); // set random speed [5, 15]
hyan99 0:3b4906b8a747 107 car12.reset(rand() % interval + MIN_SPEED);
hyan99 0:3b4906b8a747 108 car13.reset(rand() % interval + MIN_SPEED);
hyan99 0:3b4906b8a747 109 car14.reset(rand() % interval + MIN_SPEED);
hyan99 0:3b4906b8a747 110 car15.reset(rand() % interval + MIN_SPEED);
hyan99 0:3b4906b8a747 111
hyan99 2:16b3bd337db2 112 // c->reset();
hyan99 0:3b4906b8a747 113 stopwatch.reset();
hyan99 0:3b4906b8a747 114
hyan99 0:3b4906b8a747 115 int waitTime1 = 0;
hyan99 0:3b4906b8a747 116
hyan99 0:3b4906b8a747 117 do {
hyan99 0:3b4906b8a747 118 int enterRoad1 = -1;
hyan99 0:3b4906b8a747 119 int enterCross1 = -1;
hyan99 0:3b4906b8a747 120
hyan99 0:3b4906b8a747 121 if (numberCycles == 0) {
hyan99 0:3b4906b8a747 122 // first car enters unconditionally
hyan99 0:3b4906b8a747 123 AccCar *car1 = q1.back();
hyan99 0:3b4906b8a747 124 enterRoad1 = car1->id;
hyan99 0:3b4906b8a747 125 road1.add_acc_car(car1, car1->id);
hyan99 0:3b4906b8a747 126 q1.pop_back();
hyan99 0:3b4906b8a747 127 waitTime1 = rand() % 4;
hyan99 0:3b4906b8a747 128 }
hyan99 0:3b4906b8a747 129 else {
hyan99 0:3b4906b8a747 130 if (q1.size() > 0) {
hyan99 0:3b4906b8a747 131 if (waitTime1 == 0) {
hyan99 0:3b4906b8a747 132 AccCar *car1 = q1.back();
hyan99 0:3b4906b8a747 133 if (car1->forward_car->position - 2 >= car1->speed) {
hyan99 0:3b4906b8a747 134 road1.add_acc_car(car1, car1->id);
hyan99 0:3b4906b8a747 135 enterRoad1 = car1->id;
hyan99 0:3b4906b8a747 136 q1.pop_back();
hyan99 0:3b4906b8a747 137 waitTime1 = rand() % 4;
hyan99 0:3b4906b8a747 138 }
hyan99 0:3b4906b8a747 139 } else {
hyan99 0:3b4906b8a747 140 waitTime1 = waitTime1 - 1;
hyan99 0:3b4906b8a747 141 }
hyan99 0:3b4906b8a747 142 }
hyan99 0:3b4906b8a747 143 }
hyan99 0:3b4906b8a747 144
hyan99 0:3b4906b8a747 145 road1.let_cars_update();
hyan99 0:3b4906b8a747 146 road1.wait_for_car_update();
hyan99 0:3b4906b8a747 147 // ------------------------------------------------------------------
hyan99 0:3b4906b8a747 148 // Timing statistics logic, do not modify
hyan99 0:3b4906b8a747 149 totalUpdateTime += stopwatch.read_ms();
hyan99 0:3b4906b8a747 150 numberCycles++;
hyan99 0:3b4906b8a747 151 stopwatch.reset();
hyan99 0:3b4906b8a747 152 // ------------------------------------------------------------------
hyan99 0:3b4906b8a747 153
hyan99 0:3b4906b8a747 154 if (car11.position == STOP) {
hyan99 0:3b4906b8a747 155 enterCross1 = 1;
hyan99 0:3b4906b8a747 156 } else if (car12.position == STOP) {
hyan99 0:3b4906b8a747 157 enterCross1 = 2;
hyan99 0:3b4906b8a747 158 } else if (car13.position == STOP) {
hyan99 0:3b4906b8a747 159 enterCross1 = 3;
hyan99 0:3b4906b8a747 160 } else if (car14.position == STOP) {
hyan99 0:3b4906b8a747 161 enterCross1 = 4;
hyan99 0:3b4906b8a747 162 } else if (car15.position == STOP) {
hyan99 0:3b4906b8a747 163 enterCross1 = 5;
hyan99 0:3b4906b8a747 164 }
hyan99 0:3b4906b8a747 165 lcd.cls();
hyan99 0:3b4906b8a747 166 if (enterRoad1 == -1 && enterCross1 == -1) {
hyan99 0:3b4906b8a747 167 lcd.printf("x, x\n");
hyan99 0:3b4906b8a747 168 } else if (enterRoad1 == -1) {
hyan99 0:3b4906b8a747 169 lcd.printf("x, %d\n", enterCross1);
hyan99 0:3b4906b8a747 170 } else if (enterCross1 == -1) {
hyan99 0:3b4906b8a747 171 lcd.printf("%d, x\n", enterRoad1);
hyan99 0:3b4906b8a747 172 } else {
hyan99 0:3b4906b8a747 173 lcd.printf("%d, %d\n", enterRoad1, enterCross1);
hyan99 0:3b4906b8a747 174 }
hyan99 0:3b4906b8a747 175
hyan99 0:3b4906b8a747 176 pc.printf("Car 1 on road 1: position %d, speed %d\r\n", car11.position, car11.speed);
hyan99 0:3b4906b8a747 177 pc.printf("Car 2 on road 1: position %d, speed %d\r\n", car12.position, car12.speed);
hyan99 0:3b4906b8a747 178 pc.printf("Car 3 on road 1: position %d, speed %d\r\n", car13.position, car13.speed);
hyan99 0:3b4906b8a747 179 pc.printf("Car 4 on road 1: position %d, speed %d\r\n", car14.position, car14.speed);
hyan99 0:3b4906b8a747 180 pc.printf("Car 5 on road 1: position %d, speed %d\r\n", car15.position, car15.speed);
hyan99 0:3b4906b8a747 181
hyan99 0:3b4906b8a747 182 } while (lastCar1->position <= ROADLENGTH);
hyan99 0:3b4906b8a747 183 car11.stop();
hyan99 0:3b4906b8a747 184 car12.stop();
hyan99 0:3b4906b8a747 185 car13.stop();
hyan99 0:3b4906b8a747 186 car14.stop();
hyan99 0:3b4906b8a747 187 car15.stop();
hyan99 2:16b3bd337db2 188 c->stop();
hyan99 0:3b4906b8a747 189
hyan99 0:3b4906b8a747 190 // ----------------------------------------------------------------------
hyan99 0:3b4906b8a747 191 // Timing statistics printout, do not modify
hyan99 0:3b4906b8a747 192 pc.printf("Average update cycle took: %fms \r\n", (totalUpdateTime*1.0)/(numberCycles*1.0));
hyan99 0:3b4906b8a747 193 totalUpdateTime = 0;
hyan99 0:3b4906b8a747 194 numberCycles = 0;
hyan99 0:3b4906b8a747 195 // ----------------------------------------------------------------------
hyan99 0:3b4906b8a747 196 }