AccCar.cpp

00001 #include "AccCar.h"
00002 #include <stdlib.h>
00003 #include <algorithm>
00004 
00005 #define TICK 1000
00006 #define MONITOR_DIST 17
00007 #define SAFETY_GAP 2
00008 #define STOP 54
00009 #define MIN_SPEED 5
00010 #define MAX_SPEED 15
00011 
00012 Serial pc_acc(USBTX, USBRX);
00013 
00014 AccCar::AccCar(int id, Road* road, int flag, Communication* c) {
00015     this->id = id;
00016     this->road = road;
00017     this->flag = flag;
00018 
00019     this->thread = NULL;
00020     this->forward_car = NULL;
00021 
00022     this->cycle = 0;
00023     this->comm = c;
00024 }
00025 
00026 void AccCar::set_forward_car(AccCar* car) {
00027     this->forward_car = car;
00028 }
00029 
00030 void AccCar::update() {
00031     bool crossed = false;
00032     int wait = 0;
00033     while (true) {
00034         ThisThread::sleep_for(TICK);
00035         road->go_flags.wait_all(flag);
00036 
00037         position = position + speed;
00038 
00039         if (forward_car != NULL && forward_car->position - position < MONITOR_DIST) {
00040             road->done_flags.wait_all(forward_car->flag, osWaitForever, false);
00041 
00042             int diff = forward_car->position - position;
00043             int maxSafeSpeed = diff - SAFETY_GAP;
00044 
00045             speed = std::min(maxSafeSpeed, target_speed);
00046         } else {
00047             speed = target_speed;
00048         }
00049         // TODO: publish Position and Acc-speed
00050         if (position < 255) {
00051             comm->publish_car(id, speed, position);
00052             Communication::control_flags.wait_all(flag); // wait for speed instruction from controller
00053             speed = Communication::speeds[id - 1];
00054             pc_acc.printf("Suggested speed: %d\r\n", speed);
00055         }
00056 //        // Crossing
00057 //        if (position < STOP) {
00058 //            speed = std::min(speed, STOP - position);
00059 //        } else if (!crossed && position == STOP) {
00060 //            speed = 0;
00061 //            if(wait == 0){
00062 //              road->intersection->add_to_q(this);
00063 //            }
00064 //            wait++;
00065 //            if(road->intersection->can_cross(this)) {
00066 //                crossed = true;
00067 //            }
00068 //        } else if (position < STOP + 2) {
00069 //            speed = 1;
00070 //        } else if(position == STOP + 2) {
00071 //            road->intersection->remove_from_q();
00072 //        }
00073 
00074         cycle = (cycle + 1) % 5;
00075         if (cycle == 0) {
00076             target_speed = rand() % (MAX_SPEED - MIN_SPEED + 1) + MIN_SPEED;
00077         }
00078         road->done_flags.set(flag);
00079     }
00080 }
00081 
00082 void AccCar::reset(int speed) {
00083     road->done_flags.clear(flag);
00084 
00085     if (thread != NULL) {
00086         thread->terminate();
00087     }
00088 
00089     thread = new Thread();
00090     thread->start( callback(this, &AccCar::update) );
00091 
00092     this->position = 0;
00093     this->speed = speed;
00094     this->target_speed = speed;
00095 }
00096 
00097 void AccCar::stop() {
00098     if (thread != NULL) {
00099         thread->terminate();
00100     }
00101 }