Eric Micallef / Mbed OS smat_controller

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   MQTT

car_controller.cpp@3:663c6fc11321, 2019-12-06 (annotated)

Committer:
micallef25
Date:
Fri Dec 06 18:29:12 2019 +0000
Revision:
3:663c6fc11321
Parent:
2:f10d6fecb345 
for merging

Who changed what in which revision?

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