File content as of revision 7:fd8e0604faaa:

#include "mbed.h"
#include "AccCar.h"
#include "mqtt.h"
#include <stdlib.h>
#include <algorithm>

// ACC macros
#define TICK 1000
#define MONITOR_DIST 17
#define SAFETY_GAP 2

//#define DEBUG_ACC
#define ACC_TAG "[ACC] "
#define ERROR_TAG "[ERROR] "
// speed for a legal stop
#define STOPPED_SPEED 0

AccCar::AccCar(int id, Road* road, int flag, mqtt* singleton)
{
    this->car_id = id;
    this->road = road;
    this->flag = flag;
    this->lead = false;
    this->thread = NULL;
    this->state = NORMAL_STATE;
    this->waited = false;
    this->cycles = 0;
    this->position = 0;
    this->target_speed = 0;
    this->speed = 0;
    this->wait_time = 0;
    this->msg = new position_msg_t;
    this->singleton = singleton;
}

void AccCar::make_position_msg(position_msg_t* msg)
{
    assert(msg != NULL);
    msg->position = this->position;
    msg->speed = this->speed;
    msg->car_id = this->car_id;
    msg->road_id = this->road->get_road_id(); 
    msg->counter = this->road->get_road_clock();
    //msg->state = this->state; 
}

// if a null pointer is sent this means no car is in front and it is the lead car.
// this makes the update section slightly different
void AccCar::set_forward_car(AccCar* car)
{
    if(car == NULL) {
        lead = true;
    } else {
        this->forward_car = car;
        lead = false;
    }
}

// update your speed based on forward car
// unless you are lead car then just mozy on
void AccCar::update_speed()
{
    if( !lead )
    {
        if (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);
            //if(speed < 0)
           // {
             //   printf(ERROR_TAG"f_pos %d pos %d id %d"DELIM,forward_car->position,position,car_id);
            assert(speed >= 0);
            //}
        }   else {
            speed = target_speed;
        }
    }
    else{
        speed = target_speed;    
    }
}

// gets new speed from 5-15
void AccCar::get_new_target_speed()
{
    int temp_speed = 0;
    // get a valid speed 5-15
    temp_speed = rand() %11;
    temp_speed +=5;
    
    this->target_speed = temp_speed;
}

/*
* assuming the car has some way to see a stop sign
* will kick in until a position message with the right ID comes
* which is not implemented
*/
int AccCar::enter_safety_mode()
{
    int old_position = position;
    // using previous speed
    int next_position = position + speed;
    if( old_position < 54 && next_position >= 54 ) 
    {
        // return new speed 0 or 
        return 54 - position;
    }
    return speed;  
}

// update FSM 
void AccCar::update()
{
    while (true) 
    {
        ThisThread::sleep_for(TICK);
        road->go_flags.wait_all(flag);
        cycles++;
#ifndef PUBLISH_ONLY 
        if(cycles > 1)
        {
            // get control msg  
            control_msg_t* c_msg = this->singleton->get_control_msg(this->car_id);
            if(c_msg == NULL)
            {
                // test if we actually have a large delay
               assert(c_msg != NULL);
               speed = enter_safety_mode();
            }
            else{
            //
            // do something with the control message
                assert(c_msg->car_id == car_id );
                if(c_msg->speed > speed){
                    printf(ERROR_TAG" msg speed %d , speed %d car %d"DELIM,c_msg->speed,speed, car_id);
                }
                
                else{
                    // add wait time if we got 0 or something less than our sent speed
                    if(speed == 0 || c_msg->speed < speed)
                    {
                        wait_time++;
                        printf(ACC_TAG"car %d has waited %d"DELIM,car_id,wait_time);
                    }
                    speed = c_msg->speed;
                }
                
                
                //  
                delete c_msg;
            }
        }
#endif

        position = position + speed;
        
        // ensure correctness
        if(!lead){
            //if(position >= this->forward_car->position - 2){
              //  printf(ERROR_TAG"position %d forward pos %d id %d speed %d"DELIM,position,this->forward_car->position, car_id,speed);
                assert(position <= this->forward_car->position - 2);    
           // }
        }
        
        if( (cycles % 5) == 0)
        {
            get_new_target_speed();    
        }
        
        // update position and check intersections
        if( state == NORMAL_STATE ) 
        {
            drive_normal();
        }
        else
        {   
            // case should not happen
            assert(0);
        }
        
//#ifdef DEBUG_ACC
//        printf(ACC_TAG "sending position to server DELIM");
//#endif
        
//#ifdef DEBUG_ACC
//        printf(ACC_TAG "car %d done DELIM",car_id);
//#endif

        // make a message
        make_position_msg(msg);
        
        // stuff a message in the queue
        this->singleton->add_to_position_queue(msg);
        
        // set flag for completion of cycle
        road->done_flags.set(this->flag);
    }
}

void AccCar::reset(int speed)
{
    road->done_flags.clear(flag);

    if (thread != NULL) {
        thread->terminate();
    }


    thread = new Thread();
    assert(thread != NULL);
    thread->start( callback(this, &AccCar::update) );

    this->position = 0;
    this->speed = speed;
    this->target_speed = speed;
}

void AccCar::stop()
{
    if (thread != NULL) {
        thread->terminate();
    }
    delete thread;
    delete msg;
}


// check if we are going to go through an intersection
// and stop or update speed accordingly
void AccCar::drive_normal()
{
    //assert( state == NORMAL_STATE );
    // first update our speed
    if( !lead ) 
    {
        update_speed();
    } else 
    {
        speed = target_speed;
    }
}

int AccCar::get_car_id()
{
    return car_id;
}

int AccCar::get_cycles()
{
    return cycles;
}

int AccCar::get_target_speed()
{
    return target_speed;
}