Diff: AccCar.cpp

Revision:

0:3b4906b8a747

Child:

2:16b3bd337db2

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/AccCar.cpp	Tue Dec 10 22:29:09 2019 +0000
@@ -0,0 +1,102 @@
+#include "AccCar.h"
+#include <stdlib.h>
+#include <algorithm>
+
+#define TICK 1000
+#define MONITOR_DIST 17
+#define SAFETY_GAP 2
+#define STOP 54
+#define MIN_SPEED 5
+#define MAX_SPEED 15
+
+Serial pc_acc(USBTX, USBRX);
+
+AccCar::AccCar(int id, Road* road, int flag, char* t1, char* t2) {
+    this->id = id;
+    this->road = road;
+    this->flag = flag;
+
+    this->thread = NULL;
+    this->forward_car = NULL;
+
+    this->cycle = 0;
+    this->comm = Communication::getInstance();
+    this->topic_position = t1;
+    this->topic_control = t2;
+//    communication->subscribe_to_position(topic_position);
+    comm->subscribe_to_topic_control(topic_control);
+}
+
+void AccCar::set_forward_car(AccCar* car) {
+    this->forward_car = car;
+}
+
+void AccCar::update() {
+    bool crossed = false;
+    int wait = 0;
+    while (true) {
+        ThisThread::sleep_for(TICK);
+        road->go_flags.wait_all(flag);
+
+        position = position + speed;
+
+        if (forward_car != NULL && forward_car->position - position < MONITOR_DIST) {
+            road->done_flags.wait_all(forward_car->flag, osWaitForever, false);
+
+            int diff = forward_car->position - position;
+            int maxSafeSpeed = diff - SAFETY_GAP;
+
+            speed = std::min(maxSafeSpeed, target_speed);
+        } else {
+            speed = target_speed;
+        }
+        // TODO: publish Position and Acc-speed
+        comm->publish_car(topic_position, speed, position);
+        Communication::control_flags.wait_all(flag); // wait for speed instruction from controller
+        speed = Communication::speeds[id - 1];
+//        // Crossing
+//        if (position < STOP) {
+//            speed = std::min(speed, STOP - position);
+//        } else if (!crossed && position == STOP) {
+//            speed = 0;
+//            if(wait == 0){
+//              road->intersection->add_to_q(this);
+//            }
+//            wait++;
+//            if(road->intersection->can_cross(this)) {
+//                crossed = true;
+//            }
+//        } else if (position < STOP + 2) {
+//            speed = 1;
+//        } else if(position == STOP + 2) {
+//            road->intersection->remove_from_q();
+//        }
+
+        cycle = (cycle + 1) % 5;
+        if (cycle == 0) {
+            target_speed = rand() % (MAX_SPEED - MIN_SPEED + 1) + MIN_SPEED;
+        }
+        road->done_flags.set(flag);
+    }
+}
+
+void AccCar::reset(int speed) {
+    road->done_flags.clear(flag);
+
+    if (thread != NULL) {
+        thread->terminate();
+    }
+
+    thread = new Thread();
+    thread->start( callback(this, &AccCar::update) );
+
+    this->position = 0;
+    this->speed = speed;
+    this->target_speed = speed;
+}
+
+void AccCar::stop() {
+    if (thread != NULL) {
+        thread->terminate();
+    }
+}