Emanuel Kuflik / Mbed OS smat_controller

Dependencies:   MQTT

simulator.cpp@5:e0d8e5e922f1, 2019-12-10 (annotated)

Committer:
micallef25
Date:
Tue Dec 10 23:39:25 2019 +0000
Revision:
5:e0d8e5e922f1
Child:
7:fd8e0604faaa
stable

Who changed what in which revision?

UserRevisionLine numberNew contents of line
micallef25 5:e0d8e5e922f1 1 /* mbed Microcontroller Library
micallef25 5:e0d8e5e922f1 2 * Copyright (c) 2018 ARM Limited
micallef25 5:e0d8e5e922f1 3 * SPDX-License-Identifier: Apache-2.0
micallef25 5:e0d8e5e922f1 4 */
micallef25 5:e0d8e5e922f1 5
micallef25 5:e0d8e5e922f1 6 #include "mbed.h"
micallef25 5:e0d8e5e922f1 7 #include <cctype>
micallef25 5:e0d8e5e922f1 8 #include "AccCar.h"
micallef25 5:e0d8e5e922f1 9 #include "Road.h"
micallef25 5:e0d8e5e922f1 10 #include "mqtt.h"
micallef25 5:e0d8e5e922f1 11
micallef25 5:e0d8e5e922f1 12 #define MAGIC_NUMBER 2
micallef25 5:e0d8e5e922f1 13
micallef25 5:e0d8e5e922f1 14 #define SIM_DEBUG
micallef25 5:e0d8e5e922f1 15 #define SIM_TAG "[SIM] "
micallef25 5:e0d8e5e922f1 16
micallef25 5:e0d8e5e922f1 17 Serial pc(USBTX, USBRX);
micallef25 5:e0d8e5e922f1 18
micallef25 5:e0d8e5e922f1 19 // prints statistics
micallef25 5:e0d8e5e922f1 20 static void print_wait_time(Road* road)
micallef25 5:e0d8e5e922f1 21 {
micallef25 5:e0d8e5e922f1 22 int active_cars = road->get_active_cars();
micallef25 5:e0d8e5e922f1 23 for( int i = 0; i < active_cars; i++)
micallef25 5:e0d8e5e922f1 24 {
micallef25 5:e0d8e5e922f1 25 AccCar* car = road->get_car(i);
micallef25 5:e0d8e5e922f1 26 assert(car != NULL);
micallef25 5:e0d8e5e922f1 27 pc.printf(SIM_TAG "Car %d Road %d wait_time %d"DELIM, i,road->get_road_id(), car->wait_time);
micallef25 5:e0d8e5e922f1 28 }
micallef25 5:e0d8e5e922f1 29 }
micallef25 5:e0d8e5e922f1 30
micallef25 5:e0d8e5e922f1 31 // handles adding a new car safely to the intersection
micallef25 5:e0d8e5e922f1 32 static void add_new_car(Road* road,int* cycles,mqtt* mqtt_singleton)
micallef25 5:e0d8e5e922f1 33 {
micallef25 5:e0d8e5e922f1 34 // get rand speed
micallef25 5:e0d8e5e922f1 35 int temp_speed = rand() %11;
micallef25 5:e0d8e5e922f1 36 temp_speed += 5;
micallef25 5:e0d8e5e922f1 37
micallef25 5:e0d8e5e922f1 38 //
micallef25 5:e0d8e5e922f1 39 assert( temp_speed >=5 && temp_speed <= 15);
micallef25 5:e0d8e5e922f1 40
micallef25 5:e0d8e5e922f1 41 if(road->can_car_enter( temp_speed ))
micallef25 5:e0d8e5e922f1 42 {
micallef25 5:e0d8e5e922f1 43 *cycles = 0; // we added a car reset our timer
micallef25 5:e0d8e5e922f1 44
micallef25 5:e0d8e5e922f1 45 int id = road->get_new_car_id();
micallef25 5:e0d8e5e922f1 46
micallef25 5:e0d8e5e922f1 47 AccCar* car = new AccCar(id, road, (1 << id),mqtt_singleton );
micallef25 5:e0d8e5e922f1 48 assert(car != NULL);
micallef25 5:e0d8e5e922f1 49
micallef25 5:e0d8e5e922f1 50 #ifdef SIM_DEBUG
micallef25 5:e0d8e5e922f1 51 pc.printf(SIM_TAG "adding new car to road %d car %d"DELIM,road->get_road_id(),id);
micallef25 5:e0d8e5e922f1 52 #endif
micallef25 5:e0d8e5e922f1 53
micallef25 5:e0d8e5e922f1 54 road->add_acc_car(car);
micallef25 5:e0d8e5e922f1 55
micallef25 5:e0d8e5e922f1 56 AccCar* forward_car = road->get_forward_car(id);
micallef25 5:e0d8e5e922f1 57
micallef25 5:e0d8e5e922f1 58 // lead car will get a null pointer
micallef25 5:e0d8e5e922f1 59 car->set_forward_car( forward_car );
micallef25 5:e0d8e5e922f1 60
micallef25 5:e0d8e5e922f1 61 // this needs to be a rand number [0,15]
micallef25 5:e0d8e5e922f1 62 car->reset(temp_speed);
micallef25 5:e0d8e5e922f1 63
micallef25 5:e0d8e5e922f1 64 }
micallef25 5:e0d8e5e922f1 65 }
micallef25 5:e0d8e5e922f1 66
micallef25 5:e0d8e5e922f1 67 void print_positions(Road* road)
micallef25 5:e0d8e5e922f1 68 {
micallef25 5:e0d8e5e922f1 69 //#ifdef SIM_DEBUG
micallef25 5:e0d8e5e922f1 70 pc.printf(SIM_TAG " -------------- "DELIM);
micallef25 5:e0d8e5e922f1 71 //#endif
micallef25 5:e0d8e5e922f1 72 int active_cars = road->get_active_cars();
micallef25 5:e0d8e5e922f1 73 for( int i = 0; i < active_cars; i++)
micallef25 5:e0d8e5e922f1 74 {
micallef25 5:e0d8e5e922f1 75 AccCar* car = road->get_car(i);
micallef25 5:e0d8e5e922f1 76 assert(car != NULL);
micallef25 5:e0d8e5e922f1 77 pc.printf(SIM_TAG "Car %d Road %d position %d : speed %d cycles: %d car_cycles %d "DELIM, i,road->get_road_id(), car->position,car->speed, road->get_road_clock(), car->get_cycles());
micallef25 5:e0d8e5e922f1 78 }
micallef25 5:e0d8e5e922f1 79 //#ifdef SIM_DEBUG
micallef25 5:e0d8e5e922f1 80 pc.printf(SIM_TAG " -------------- "DELIM);
micallef25 5:e0d8e5e922f1 81 //#endif
micallef25 5:e0d8e5e922f1 82 }
micallef25 5:e0d8e5e922f1 83
micallef25 5:e0d8e5e922f1 84
micallef25 5:e0d8e5e922f1 85 int start_simulation(mqtt* mqtt_singleton)
micallef25 5:e0d8e5e922f1 86 {
micallef25 5:e0d8e5e922f1 87 // ------------------------------------------------------------------------------
micallef25 5:e0d8e5e922f1 88 // The following three variables are used for timing statistics, do not modify them
micallef25 5:e0d8e5e922f1 89 Timer stopwatch; // A timer to keep track of how long the updates take, for statistics purposes
micallef25 5:e0d8e5e922f1 90 int numberCycles = 0;
micallef25 5:e0d8e5e922f1 91 int totalUpdateTime = 0;
micallef25 5:e0d8e5e922f1 92
micallef25 5:e0d8e5e922f1 93 // every 3 cycles we need to add a new car
micallef25 5:e0d8e5e922f1 94 int new_car_cycles = 0;
micallef25 5:e0d8e5e922f1 95
micallef25 5:e0d8e5e922f1 96 // ------------------------------------------------------------------------------
micallef25 5:e0d8e5e922f1 97 // seed rand number generator
micallef25 5:e0d8e5e922f1 98 srand(mqtt_singleton->mqtt_id);
micallef25 5:e0d8e5e922f1 99
micallef25 5:e0d8e5e922f1 100 // make new road based off of ID
micallef25 5:e0d8e5e922f1 101 Road* road = new Road(mqtt_singleton->mqtt_id, mqtt_singleton);
micallef25 5:e0d8e5e922f1 102
micallef25 5:e0d8e5e922f1 103 // add first cars to the road to kick everything off
micallef25 5:e0d8e5e922f1 104 add_new_car(road,&new_car_cycles,mqtt_singleton);
micallef25 5:e0d8e5e922f1 105
micallef25 5:e0d8e5e922f1 106 stopwatch.start();
micallef25 5:e0d8e5e922f1 107
micallef25 5:e0d8e5e922f1 108 stopwatch.reset();
micallef25 5:e0d8e5e922f1 109
micallef25 5:e0d8e5e922f1 110 bool other_simulating=false;
micallef25 5:e0d8e5e922f1 111 bool simulating = true;
micallef25 5:e0d8e5e922f1 112
micallef25 5:e0d8e5e922f1 113 //
micallef25 5:e0d8e5e922f1 114 //road->synchronize(simulating);
micallef25 5:e0d8e5e922f1 115
micallef25 5:e0d8e5e922f1 116 do {
micallef25 5:e0d8e5e922f1 117
micallef25 5:e0d8e5e922f1 118 // if we get a random number thats right OR if we hit our max of 3
micallef25 5:e0d8e5e922f1 119 // then add a new car
micallef25 5:e0d8e5e922f1 120 if( (rand() %3) == MAGIC_NUMBER || new_car_cycles >= 3)
micallef25 5:e0d8e5e922f1 121 {
micallef25 5:e0d8e5e922f1 122 // add new car to road and then set the forward car to it
micallef25 5:e0d8e5e922f1 123 add_new_car(road,&new_car_cycles,mqtt_singleton);
micallef25 5:e0d8e5e922f1 124 }
micallef25 5:e0d8e5e922f1 125
micallef25 5:e0d8e5e922f1 126 //
micallef25 5:e0d8e5e922f1 127 road->let_cars_update();
micallef25 5:e0d8e5e922f1 128
micallef25 5:e0d8e5e922f1 129 //
micallef25 5:e0d8e5e922f1 130 road->wait_for_car_update();
micallef25 5:e0d8e5e922f1 131
micallef25 5:e0d8e5e922f1 132 // are we still simulating? true if yes
micallef25 5:e0d8e5e922f1 133 simulating = road->simulating();
micallef25 5:e0d8e5e922f1 134
micallef25 5:e0d8e5e922f1 135 #ifndef PUBLISH_ONLY
micallef25 5:e0d8e5e922f1 136 // is the other road still simulating? true if yes
micallef25 5:e0d8e5e922f1 137 other_simulating = road->synchronize(simulating);
micallef25 5:e0d8e5e922f1 138 #endif
micallef25 5:e0d8e5e922f1 139
micallef25 5:e0d8e5e922f1 140 // ------------------------------------------------------------------
micallef25 5:e0d8e5e922f1 141 // Timing statistics logic, do not modify
micallef25 5:e0d8e5e922f1 142 totalUpdateTime += stopwatch.read_ms();
micallef25 5:e0d8e5e922f1 143 numberCycles++;
micallef25 5:e0d8e5e922f1 144 new_car_cycles++;
micallef25 5:e0d8e5e922f1 145 stopwatch.reset();
micallef25 5:e0d8e5e922f1 146 // ------------------------------------------------------------------
micallef25 5:e0d8e5e922f1 147
micallef25 5:e0d8e5e922f1 148 print_positions(road);
micallef25 5:e0d8e5e922f1 149
micallef25 5:e0d8e5e922f1 150 #ifndef PUBLISH_ONLY
micallef25 5:e0d8e5e922f1 151 // TODO figure out how to end simulation like they want
micallef25 5:e0d8e5e922f1 152 } while (other_simulating || simulating);
micallef25 5:e0d8e5e922f1 153 #else
micallef25 5:e0d8e5e922f1 154 } while (simulating);
micallef25 5:e0d8e5e922f1 155 #endif
micallef25 5:e0d8e5e922f1 156 // ----------------------------------------------------------------------
micallef25 5:e0d8e5e922f1 157 // Timing statistics printout, do not modify
micallef25 5:e0d8e5e922f1 158 pc.printf(SIM_TAG "Average update cycle took: %fms "DELIM, (totalUpdateTime*1.0)/(numberCycles*1.0));
micallef25 5:e0d8e5e922f1 159 totalUpdateTime = 0;
micallef25 5:e0d8e5e922f1 160 numberCycles = 0;
micallef25 5:e0d8e5e922f1 161
micallef25 5:e0d8e5e922f1 162 // print wait times
micallef25 5:e0d8e5e922f1 163 print_wait_time(road);
micallef25 5:e0d8e5e922f1 164
micallef25 5:e0d8e5e922f1 165 // ----------------------------------------------------------------------
micallef25 5:e0d8e5e922f1 166 road->DESTROY_ALL_CARS();
micallef25 5:e0d8e5e922f1 167 delete road->msg;
micallef25 5:e0d8e5e922f1 168 delete road;
micallef25 5:e0d8e5e922f1 169 return 0;
micallef25 5:e0d8e5e922f1 170 }
micallef25 5:e0d8e5e922f1 171