Han Yan / Mbed OS ms2b

not running

Dependencies:   TextLCD MQTT

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

Committer:
hyan99
Date:
Wed Dec 11 20:20:12 2019 +0000
Revision:
2:16b3bd337db2
Parent:
0:3b4906b8a747 
testing

Who changed what in which revision?

UserRevisionLine numberNew contents of line
hyan99 0:3b4906b8a747 1 #include "AccCar.h"
hyan99 0:3b4906b8a747 2 #include <stdlib.h>
hyan99 0:3b4906b8a747 3 #include <algorithm>
hyan99 0:3b4906b8a747 4
hyan99 0:3b4906b8a747 5 #define TICK 1000
hyan99 0:3b4906b8a747 6 #define MONITOR_DIST 17
hyan99 0:3b4906b8a747 7 #define SAFETY_GAP 2
hyan99 0:3b4906b8a747 8 #define STOP 54
hyan99 0:3b4906b8a747 9 #define MIN_SPEED 5
hyan99 0:3b4906b8a747 10 #define MAX_SPEED 15
hyan99 0:3b4906b8a747 11
hyan99 0:3b4906b8a747 12 Serial pc_acc(USBTX, USBRX);
hyan99 0:3b4906b8a747 13
hyan99 2:16b3bd337db2 14 AccCar::AccCar(int id, Road* road, int flag, Communication* c) {
hyan99 0:3b4906b8a747 15 this->id = id;
hyan99 0:3b4906b8a747 16 this->road = road;
hyan99 0:3b4906b8a747 17 this->flag = flag;
hyan99 0:3b4906b8a747 18
hyan99 0:3b4906b8a747 19 this->thread = NULL;
hyan99 0:3b4906b8a747 20 this->forward_car = NULL;
hyan99 0:3b4906b8a747 21
hyan99 0:3b4906b8a747 22 this->cycle = 0;
hyan99 2:16b3bd337db2 23 this->comm = c;
hyan99 0:3b4906b8a747 24 }
hyan99 0:3b4906b8a747 25
hyan99 0:3b4906b8a747 26 void AccCar::set_forward_car(AccCar* car) {
hyan99 0:3b4906b8a747 27 this->forward_car = car;
hyan99 0:3b4906b8a747 28 }
hyan99 0:3b4906b8a747 29
hyan99 0:3b4906b8a747 30 void AccCar::update() {
hyan99 0:3b4906b8a747 31 bool crossed = false;
hyan99 0:3b4906b8a747 32 int wait = 0;
hyan99 0:3b4906b8a747 33 while (true) {
hyan99 0:3b4906b8a747 34 ThisThread::sleep_for(TICK);
hyan99 0:3b4906b8a747 35 road->go_flags.wait_all(flag);
hyan99 0:3b4906b8a747 36
hyan99 0:3b4906b8a747 37 position = position + speed;
hyan99 0:3b4906b8a747 38
hyan99 0:3b4906b8a747 39 if (forward_car != NULL && forward_car->position - position < MONITOR_DIST) {
hyan99 0:3b4906b8a747 40 road->done_flags.wait_all(forward_car->flag, osWaitForever, false);
hyan99 0:3b4906b8a747 41
hyan99 0:3b4906b8a747 42 int diff = forward_car->position - position;
hyan99 0:3b4906b8a747 43 int maxSafeSpeed = diff - SAFETY_GAP;
hyan99 0:3b4906b8a747 44
hyan99 0:3b4906b8a747 45 speed = std::min(maxSafeSpeed, target_speed);
hyan99 0:3b4906b8a747 46 } else {
hyan99 0:3b4906b8a747 47 speed = target_speed;
hyan99 0:3b4906b8a747 48 }
hyan99 0:3b4906b8a747 49 // TODO: publish Position and Acc-speed
hyan99 2:16b3bd337db2 50 if (position < 255) {
hyan99 2:16b3bd337db2 51 comm->publish_car(id, speed, position);
hyan99 2:16b3bd337db2 52 Communication::control_flags.wait_all(flag); // wait for speed instruction from controller
hyan99 2:16b3bd337db2 53 speed = Communication::speeds[id - 1];
hyan99 2:16b3bd337db2 54 pc_acc.printf("Suggested speed: %d\r\n", speed);
hyan99 2:16b3bd337db2 55 }
hyan99 0:3b4906b8a747 56 // // Crossing
hyan99 0:3b4906b8a747 57 // if (position < STOP) {
hyan99 0:3b4906b8a747 58 // speed = std::min(speed, STOP - position);
hyan99 0:3b4906b8a747 59 // } else if (!crossed && position == STOP) {
hyan99 0:3b4906b8a747 60 // speed = 0;
hyan99 0:3b4906b8a747 61 // if(wait == 0){
hyan99 0:3b4906b8a747 62 // road->intersection->add_to_q(this);
hyan99 0:3b4906b8a747 63 // }
hyan99 0:3b4906b8a747 64 // wait++;
hyan99 0:3b4906b8a747 65 // if(road->intersection->can_cross(this)) {
hyan99 0:3b4906b8a747 66 // crossed = true;
hyan99 0:3b4906b8a747 67 // }
hyan99 0:3b4906b8a747 68 // } else if (position < STOP + 2) {
hyan99 0:3b4906b8a747 69 // speed = 1;
hyan99 0:3b4906b8a747 70 // } else if(position == STOP + 2) {
hyan99 0:3b4906b8a747 71 // road->intersection->remove_from_q();
hyan99 0:3b4906b8a747 72 // }
hyan99 0:3b4906b8a747 73
hyan99 0:3b4906b8a747 74 cycle = (cycle + 1) % 5;
hyan99 0:3b4906b8a747 75 if (cycle == 0) {
hyan99 0:3b4906b8a747 76 target_speed = rand() % (MAX_SPEED - MIN_SPEED + 1) + MIN_SPEED;
hyan99 0:3b4906b8a747 77 }
hyan99 0:3b4906b8a747 78 road->done_flags.set(flag);
hyan99 0:3b4906b8a747 79 }
hyan99 0:3b4906b8a747 80 }
hyan99 0:3b4906b8a747 81
hyan99 0:3b4906b8a747 82 void AccCar::reset(int speed) {
hyan99 0:3b4906b8a747 83 road->done_flags.clear(flag);
hyan99 0:3b4906b8a747 84
hyan99 0:3b4906b8a747 85 if (thread != NULL) {
hyan99 0:3b4906b8a747 86 thread->terminate();
hyan99 0:3b4906b8a747 87 }
hyan99 0:3b4906b8a747 88
hyan99 0:3b4906b8a747 89 thread = new Thread();
hyan99 0:3b4906b8a747 90 thread->start( callback(this, &AccCar::update) );
hyan99 0:3b4906b8a747 91
hyan99 0:3b4906b8a747 92 this->position = 0;
hyan99 0:3b4906b8a747 93 this->speed = speed;
hyan99 0:3b4906b8a747 94 this->target_speed = speed;
hyan99 0:3b4906b8a747 95 }
hyan99 0:3b4906b8a747 96
hyan99 0:3b4906b8a747 97 void AccCar::stop() {
hyan99 0:3b4906b8a747 98 if (thread != NULL) {
hyan99 0:3b4906b8a747 99 thread->terminate();
hyan99 0:3b4906b8a747 100 }
hyan99 0:3b4906b8a747 101 }