Dependencies:   TextLCD MQTT

AccCar.cpp

Committer:
kchen7
Date:
2019-11-11
Revision:
1:54512aca944d
Parent:
0:ca7cb51e9fd1
Child:
4:ef8866873df5

File content as of revision 1:54512aca944d:

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

#define TICK 1000
#define MONITOR_DIST 17
#define SAFETY_GAP 2

AccCar::AccCar(int id, Road* road, int flag) {
    this->id = id;
    this->road = road;
    this->flag = flag;
    
    waited = false;
        
    this->thread = NULL;
}

void AccCar::set_forward_car(AccCar* car) {
    this->forward_car = car;   
}

void AccCar::set_target_speed(int speed) {
    this->target_speed = speed;
    this->speed = speed;   
}

void AccCar::update() {
    do {
        ThisThread::sleep_for(TICK);
        road->go_flags.wait_all(flag);
        cycle++;
                
        position = position + speed;
        
        if (cycle % 5 == 0) {
            target_speed = rand() % 11 + 5;
        }
        
        if( position < 54 ) {       
            if (forward_car != NULL && forward_car->position > -1) {
                if (forward_car->position - position < MONITOR_DIST && forward_car->position < 55) {
                    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;   
                }
            } else {
                speed = target_speed;
            }
            
            int distance_to_intersection = 54 - position;
            speed = std::min(distance_to_intersection, speed);
                  
        } else if( position == 54 ) {
            if( waited) {
                speed = road->intersection->attemptEnterIntersection(this->id); 
            } else {
                speed = 0;   
                waited = true;
                road->intendToEnter(this->id); 
            }
        } else if( position < 56 ) {
            speed = 1;
        } else {
            if( position == 56 ) {
                road->intersection->leaveIntersection(); 
            }
            
            if (forward_car != NULL && forward_car->position > -1) {
                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);
                } else {
                    speed = target_speed;   
                }

            } else {
                speed = target_speed;
            }
        } 
        
        road->done_flags.set(flag);   
    } while( position < 100 );
}

void AccCar::reset() {
    road->done_flags.clear(flag);
    
    if (thread != NULL) {
        thread->terminate();   
    }
    
    thread = new Thread();
    thread->start( callback(this, &AccCar::update) );
    
    this->position = 0;
    this->cycle = 0;
}