File content as of revision 2:16b3bd337db2:

#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, Communication* c) {
    this->id = id;
    this->road = road;
    this->flag = flag;

    this->thread = NULL;
    this->forward_car = NULL;

    this->cycle = 0;
    this->comm = c;
}

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
        if (position < 255) {
            comm->publish_car(id, speed, position);
            Communication::control_flags.wait_all(flag); // wait for speed instruction from controller
            speed = Communication::speeds[id - 1];
            pc_acc.printf("Suggested speed: %d\r\n", speed);
        }
//        // 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();
    }
}