AccCar.cpp

00001 #include "mbed.h"
00002 #include "AccCar.h"
00003 #include "mqtt.h"
00004 #include <stdlib.h>
00005 #include <algorithm>
00006 
00007 // ACC macros
00008 #define TICK 1000
00009 #define MONITOR_DIST 17
00010 #define SAFETY_GAP 2
00011 
00012 #define DEBUG_ACC
00013 
00014 // speed for a legal stop
00015 #define STOPPED_SPEED 0
00016 
00017 AccCar::AccCar(int id, Road* road, int flag, mqtt* singleton)
00018 {
00019     this->car_id = id;
00020     this->road = road;
00021     this->flag = flag;
00022     this->lead = false;
00023     this->thread = NULL;
00024     this->state = NORMAL_STATE;
00025     this->waited = false;
00026     this->cycles = 0;
00027     this->position = 0;
00028     this->target_speed = 0;
00029     this->speed = 0;
00030     this->wait_time = 0;
00031     this->msg = new position_msg_t;
00032     this->singleton = singleton;
00033 }
00034 
00035 void AccCar::make_position_msg(position_msg_t* msg)
00036 {
00037     assert(msg != NULL);
00038     msg->position = this->position;
00039     msg->speed = this->speed;
00040     msg->car_id = this->car_id;
00041     msg->road_id = this->road->get_road_id(); 
00042     msg->counter = this->road->get_road_clock();
00043     msg->state = this->state; 
00044 }
00045 
00046 // if a null pointer is sent this means no car is in front and it is the lead car.
00047 // this makes the update section slightly different
00048 void AccCar::set_forward_car(AccCar* car)
00049 {
00050     if(car == NULL) {
00051         lead = true;
00052     } else {
00053         this->forward_car = car;
00054         lead = false;
00055     }
00056 }
00057 
00058 // update your speed based on forward car
00059 // unless you are lead car then just mozy on
00060 void AccCar::update_speed()
00061 {
00062     if( !lead )
00063     {
00064         if (forward_car->position - position < MONITOR_DIST) {
00065             road->done_flags.wait_all(forward_car->flag, osWaitForever, false);
00066 
00067             int diff = forward_car->position - position;
00068             int maxSafeSpeed = diff - SAFETY_GAP;
00069             speed = std::min(maxSafeSpeed, target_speed);
00070             assert(speed >= 0);
00071         }   else {
00072             speed = target_speed;
00073         }
00074     }
00075     else{
00076         speed = target_speed;    
00077     }
00078 }
00079 
00080 // gets new speed from 5-15
00081 void AccCar::get_new_target_speed()
00082 {
00083     int temp_speed = 0;
00084     // get a valid speed 5-15
00085     temp_speed = rand() %11;
00086     temp_speed +=5;
00087     
00088     this->target_speed = temp_speed;
00089 }
00090 
00091 // update FSM 
00092 void AccCar::update()
00093 {
00094     while (true) 
00095     {
00096         ThisThread::sleep_for(TICK);
00097         road->go_flags.wait_all(flag);
00098         cycles++;
00099         position = position + speed;
00100         if( (cycles % 5) == 0)
00101         {
00102             get_new_target_speed();    
00103         }
00104         
00105         // update position and check intersections
00106         if( state == NORMAL_STATE ) 
00107         {
00108             drive_normal();
00109         }
00110         
00111         //TODO?
00112         else if( state == STOPPED_STATE ) 
00113         {
00114             speed = STOPPED_SPEED;  
00115         }
00116         else
00117         {  
00118             // state can only be normal or stopped so end program
00119             assert(state == NORMAL_STATE);
00120         }
00121         
00122 #ifdef DEBUG_ACC
00123         printf("sending position to server\r\n");
00124 #endif
00125         
00126         // make a message
00127         make_position_msg(msg);
00128         
00129         // stuff a message in the queue
00130         this->singleton->add_to_position_queue(msg);
00131         
00132         //
00133         // get_control message may need to malloc the message or something
00134         control_msg_t* c_msg = this->singleton->get_control_msg(this->car_id);
00135         
00136         // set flag for completion of cycle
00137         road->done_flags.set(this->flag);
00138     }
00139 }
00140 
00141 void AccCar::reset(int speed)
00142 {
00143     road->done_flags.clear(flag);
00144 
00145     if (thread != NULL) {
00146         thread->terminate();
00147     }
00148 
00149 
00150     thread = new Thread();
00151     assert(thread != NULL);
00152     thread->start( callback(this, &AccCar::update) );
00153 
00154     this->position = 0;
00155     this->speed = speed;
00156     this->target_speed = speed;
00157 }
00158 
00159 void AccCar::stop()
00160 {
00161     if (thread != NULL) {
00162         thread->terminate();
00163     }
00164     delete msg;
00165 }
00166 
00167 
00168 // check if we are going to go through an intersection
00169 // and stop or update speed accordingly
00170 void AccCar::drive_normal()
00171 {
00172     assert( state == NORMAL_STATE );
00173     // first update our speed
00174     if( !lead ) 
00175     {
00176         update_speed();
00177     } else 
00178     {
00179         speed = target_speed;
00180     }
00181 }
00182 
00183 int AccCar::get_car_id()
00184 {
00185     return car_id;
00186 }
00187 
00188 int AccCar::get_cycles()
00189 {
00190     return cycles;
00191 }