Emanuel Kuflik / Mbed OS smat_controller

541 smart traffic controller

Dependencies:   MQTT

Road.cpp@7:fd8e0604faaa, 2019-12-12 (annotated)

Committer:
micallef25
Date:
Thu Dec 12 17:25:27 2019 +0000
Revision:
7:fd8e0604faaa
Parent:
6:6cb13ac483e0 
stable, passed 1000 simulation regression

Who changed what in which revision?

UserRevisionLine numberNew contents of line
micallef25 2:f10d6fecb345 1 #include "Road.h"
micallef25 5:e0d8e5e922f1 2 #include "mbed.h"
micallef25 2:f10d6fecb345 3
micallef25 2:f10d6fecb345 4 #define ROADLENGTH 100
micallef25 5:e0d8e5e922f1 5 #define ROAD_TAG "[ROAD] "
micallef25 5:e0d8e5e922f1 6 //#define ROAD_DEBUG
micallef25 5:e0d8e5e922f1 7
micallef25 5:e0d8e5e922f1 8 Serial road_pc (USBTX, USBRX);
micallef25 2:f10d6fecb345 9
MannyK 4:64c6fc70ddb7 10 Road::Road(int id, mqtt* mqtt_singleton)
micallef25 2:f10d6fecb345 11 {
micallef25 5:e0d8e5e922f1 12 this->road_id = id;
MannyK 4:64c6fc70ddb7 13 this->singleton = mqtt_singleton;
micallef25 5:e0d8e5e922f1 14 this->active_cars = 0;
micallef25 5:e0d8e5e922f1 15 this->active_car_bits = 0x00;
micallef25 5:e0d8e5e922f1 16 this->road_clock = 0;
micallef25 2:f10d6fecb345 17 // clear out our pointer table
micallef25 2:f10d6fecb345 18 for (int i = 0; i < MAX_CARS_ON_ROAD; ++i)
micallef25 2:f10d6fecb345 19 {
micallef25 5:e0d8e5e922f1 20 this->car_table[i] = NULL;
micallef25 2:f10d6fecb345 21 }
micallef25 2:f10d6fecb345 22 }
micallef25 2:f10d6fecb345 23
micallef25 2:f10d6fecb345 24 void Road::add_acc_car(AccCar* car) {
micallef25 2:f10d6fecb345 25
micallef25 2:f10d6fecb345 26 // make sure all is well on the road
micallef25 2:f10d6fecb345 27 assert( active_cars < MAX_CARS_ON_ROAD );
micallef25 2:f10d6fecb345 28
micallef25 2:f10d6fecb345 29 car_table[active_cars] = car;
micallef25 2:f10d6fecb345 30
micallef25 2:f10d6fecb345 31 // no need to lock as main thread is only callee
micallef25 2:f10d6fecb345 32 active_car_bits = active_car_bits | car->flag;
micallef25 2:f10d6fecb345 33 active_cars++;
micallef25 2:f10d6fecb345 34 }
micallef25 2:f10d6fecb345 35
micallef25 2:f10d6fecb345 36 void Road::let_cars_update() {
micallef25 2:f10d6fecb345 37 go_flags.set(active_car_bits);
micallef25 2:f10d6fecb345 38 road_clock++;
micallef25 2:f10d6fecb345 39 }
micallef25 2:f10d6fecb345 40
micallef25 2:f10d6fecb345 41 void Road::wait_for_car_update() {
micallef25 2:f10d6fecb345 42 done_flags.wait_all(active_car_bits);
micallef25 2:f10d6fecb345 43 }
micallef25 2:f10d6fecb345 44
micallef25 2:f10d6fecb345 45 bool Road::can_car_enter(int speed)
micallef25 2:f10d6fecb345 46 {
micallef25 2:f10d6fecb345 47 // if no cars on the road enter
micallef25 2:f10d6fecb345 48 if(active_cars == 0)
micallef25 2:f10d6fecb345 49 return true;
micallef25 2:f10d6fecb345 50
micallef25 2:f10d6fecb345 51 // else make sure you have not exceed the max amount of cars and make sure you can safely enter
micallef25 2:f10d6fecb345 52 // safely being that you maintain your safety gap with your speed
micallef25 2:f10d6fecb345 53 AccCar* caboose = car_table[active_cars-1];
micallef25 2:f10d6fecb345 54 assert( caboose != NULL);
micallef25 2:f10d6fecb345 55 if(active_cars < MAX_CARS_ON_ROAD && speed <= caboose->position - 2 ) // 2 is safety gap
micallef25 2:f10d6fecb345 56 {
micallef25 2:f10d6fecb345 57 return true;
micallef25 2:f10d6fecb345 58 }
micallef25 2:f10d6fecb345 59 return false;
micallef25 2:f10d6fecb345 60 }
micallef25 2:f10d6fecb345 61
micallef25 2:f10d6fecb345 62 // whats the id of next car
micallef25 2:f10d6fecb345 63 // used to set bit flags really
micallef25 2:f10d6fecb345 64 int Road::get_new_car_id()
micallef25 2:f10d6fecb345 65 {
micallef25 2:f10d6fecb345 66 return active_cars;
micallef25 2:f10d6fecb345 67 }
micallef25 2:f10d6fecb345 68
micallef25 2:f10d6fecb345 69 // how many active cars are there
micallef25 2:f10d6fecb345 70 int Road::get_active_cars()
micallef25 2:f10d6fecb345 71 {
micallef25 2:f10d6fecb345 72 return active_cars;
micallef25 2:f10d6fecb345 73 }
micallef25 2:f10d6fecb345 74
micallef25 2:f10d6fecb345 75 // get a car pointer based off some id
micallef25 2:f10d6fecb345 76 AccCar* Road::get_car(int id)
micallef25 2:f10d6fecb345 77 {
micallef25 2:f10d6fecb345 78 assert(id < MAX_CARS_ON_ROAD);
micallef25 2:f10d6fecb345 79 return car_table[id];
micallef25 2:f10d6fecb345 80 }
micallef25 2:f10d6fecb345 81
micallef25 2:f10d6fecb345 82 // 0 is the lead car
micallef25 2:f10d6fecb345 83 // 1 is the second car and so on
micallef25 2:f10d6fecb345 84 AccCar* Road::get_forward_car(int id)
micallef25 2:f10d6fecb345 85 {
micallef25 2:f10d6fecb345 86 // make sure we don't go out of bounds
micallef25 2:f10d6fecb345 87 assert( id >= 0 && id <= MAX_CARS_ON_ROAD );
micallef25 2:f10d6fecb345 88 if( id > 0 )
micallef25 2:f10d6fecb345 89 return car_table[id-1];
micallef25 2:f10d6fecb345 90 else
micallef25 2:f10d6fecb345 91 return NULL; // return null means there is no forward car
micallef25 2:f10d6fecb345 92 }
micallef25 2:f10d6fecb345 93
micallef25 2:f10d6fecb345 94 // manages any clean up
micallef25 2:f10d6fecb345 95 void Road::DESTROY_ALL_CARS()
micallef25 2:f10d6fecb345 96 {
micallef25 2:f10d6fecb345 97 for( int i = 0; i < active_cars; i++)
micallef25 2:f10d6fecb345 98 {
micallef25 2:f10d6fecb345 99 AccCar* car = car_table[i];
micallef25 2:f10d6fecb345 100 car->stop();
micallef25 2:f10d6fecb345 101 delete car;
micallef25 2:f10d6fecb345 102 }
micallef25 2:f10d6fecb345 103 }
micallef25 2:f10d6fecb345 104
micallef25 2:f10d6fecb345 105 // dumb function to end simulation only returns true when we have
micallef25 2:f10d6fecb345 106 // 5 cars and the last car is passed 100 or road length
micallef25 2:f10d6fecb345 107 bool Road::simulating()
micallef25 2:f10d6fecb345 108 {
micallef25 2:f10d6fecb345 109 if(active_cars != MAX_CARS_ON_ROAD )
micallef25 7:fd8e0604faaa 110 return 1;
micallef25 2:f10d6fecb345 111
micallef25 2:f10d6fecb345 112 else
micallef25 2:f10d6fecb345 113 {
micallef25 2:f10d6fecb345 114 AccCar* car = car_table[active_cars-1];
micallef25 2:f10d6fecb345 115 assert( car != NULL );
micallef25 2:f10d6fecb345 116 if(car->position > ROADLENGTH) // TODO use macro
micallef25 7:fd8e0604faaa 117 return 0;
micallef25 2:f10d6fecb345 118 }
micallef25 7:fd8e0604faaa 119 return 1;
micallef25 2:f10d6fecb345 120 }
micallef25 2:f10d6fecb345 121
micallef25 2:f10d6fecb345 122 // typical getter functions
micallef25 2:f10d6fecb345 123 int Road::get_road_id()
micallef25 2:f10d6fecb345 124 {
micallef25 2:f10d6fecb345 125 return road_id;
micallef25 2:f10d6fecb345 126 }
micallef25 2:f10d6fecb345 127
micallef25 2:f10d6fecb345 128 int Road::get_road_clock()
micallef25 2:f10d6fecb345 129 {
micallef25 2:f10d6fecb345 130 return road_clock;
micallef25 2:f10d6fecb345 131 }
micallef25 2:f10d6fecb345 132
micallef25 2:f10d6fecb345 133 AccCar* Road::get_last_car()
micallef25 2:f10d6fecb345 134 {
micallef25 2:f10d6fecb345 135 return car_table[active_cars-1];
micallef25 2:f10d6fecb345 136 }
micallef25 2:f10d6fecb345 137
micallef25 7:fd8e0604faaa 138 int Road::synchronize(int simulating)
micallef25 5:e0d8e5e922f1 139 {
micallef25 7:fd8e0604faaa 140 //#ifdef ROAD_DEBUG
micallef25 7:fd8e0604faaa 141 // road_pc.printf(ROAD_TAG "synchronizing cycle %d complete=%d ..."DELIM, road_clock,simulating);
micallef25 7:fd8e0604faaa 142 //#endif
micallef25 7:fd8e0604faaa 143 road_msg_t* msg = new road_msg_t;
micallef25 7:fd8e0604faaa 144 msg->road_id = road_id;
micallef25 7:fd8e0604faaa 145 msg->simulating = simulating;
micallef25 7:fd8e0604faaa 146 msg->road_clock = road_clock;
micallef25 7:fd8e0604faaa 147 this->singleton->add_to_road_to_network_queue(msg);
micallef25 6:6cb13ac483e0 148
micallef25 7:fd8e0604faaa 149
micallef25 5:e0d8e5e922f1 150 //
micallef25 5:e0d8e5e922f1 151 road_msg_t* rmsg = this->singleton->get_network_to_road_msg();
micallef25 7:fd8e0604faaa 152 int rc = rmsg->simulating;
micallef25 6:6cb13ac483e0 153
micallef25 5:e0d8e5e922f1 154 #ifdef ROAD_DEBUG
micallef25 7:fd8e0604faaa 155 road_pc.printf(ROAD_TAG "my id %d rcvd id %d"DELIM, road_clock, rmsg->road_clock);
micallef25 5:e0d8e5e922f1 156 #endif
micallef25 7:fd8e0604faaa 157 assert(rmsg->road_clock == road_clock);
micallef25 7:fd8e0604faaa 158
micallef25 5:e0d8e5e922f1 159 delete rmsg;
micallef25 5:e0d8e5e922f1 160 return rc;
micallef25 5:e0d8e5e922f1 161 }
micallef25 7:fd8e0604faaa 162 void Road::free_msg()
micallef25 7:fd8e0604faaa 163 {
micallef25 7:fd8e0604faaa 164 // wait for any last emssages then clean up for next iter
micallef25 7:fd8e0604faaa 165 ThisThread::sleep_for(1000);
micallef25 7:fd8e0604faaa 166 this->singleton->clear_queues();
micallef25 7:fd8e0604faaa 167 return;
micallef25 7:fd8e0604faaa 168 }