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 #define ACC_TAG "[ACC] "
00014 #define ERROR_TAG "[ERROR] "
00015 // speed for a legal stop
00016 #define STOPPED_SPEED 0
00017 
00018 AccCar::AccCar(int id, Road* road, int flag, mqtt* singleton)
00019 {
00020     this->car_id = id;
00021     this->road = road;
00022     this->flag = flag;
00023     this->lead = false;
00024     this->thread = NULL;
00025     this->state = NORMAL_STATE;
00026     this->waited = false;
00027     this->cycles = 0;
00028     this->position = 0;
00029     this->target_speed = 0;
00030     this->speed = 0;
00031     this->wait_time = 0;
00032     this->msg = new position_msg_t;
00033     this->singleton = singleton;
00034 }
00035 
00036 void AccCar::make_position_msg(position_msg_t* msg)
00037 {
00038     assert(msg != NULL);
00039     msg->position = this->position;
00040     msg->speed = this->speed;
00041     msg->car_id = this->car_id;
00042     msg->road_id = this->road->get_road_id(); 
00043     msg->counter = this->road->get_road_clock();
00044     //msg->state = this->state; 
00045 }
00046 
00047 // if a null pointer is sent this means no car is in front and it is the lead car.
00048 // this makes the update section slightly different
00049 void AccCar::set_forward_car(AccCar* car)
00050 {
00051     if(car == NULL) {
00052         lead = true;
00053     } else {
00054         this->forward_car = car;
00055         lead = false;
00056     }
00057 }
00058 
00059 // update your speed based on forward car
00060 // unless you are lead car then just mozy on
00061 void AccCar::update_speed()
00062 {
00063     if( !lead )
00064     {
00065         if (forward_car->position - position < MONITOR_DIST) {
00066             road->done_flags.wait_all(forward_car->flag, osWaitForever, false);
00067 
00068             int diff = forward_car->position - position;
00069             int maxSafeSpeed = diff - SAFETY_GAP;
00070             speed = std::min(maxSafeSpeed, target_speed);
00071             //if(speed < 0)
00072            // {
00073              //   printf(ERROR_TAG"f_pos %d pos %d id %d"DELIM,forward_car->position,position,car_id);
00074             assert(speed >= 0);
00075             //}
00076         }   else {
00077             speed = target_speed;
00078         }
00079     }
00080     else{
00081         speed = target_speed;    
00082     }
00083 }
00084 
00085 // gets new speed from 5-15
00086 void AccCar::get_new_target_speed()
00087 {
00088     int temp_speed = 0;
00089     // get a valid speed 5-15
00090     temp_speed = rand() %11;
00091     temp_speed +=5;
00092     
00093     this->target_speed = temp_speed;
00094 }
00095 
00096 /*
00097 * assuming the car has some way to see a stop sign
00098 * will kick in until a position message with the right ID comes
00099 * which is not implemented
00100 */
00101 int AccCar::enter_safety_mode()
00102 {
00103     int old_position = position;
00104     // using previous speed
00105     int next_position = position + speed;
00106     if( old_position < 54 && next_position >= 54 ) 
00107     {
00108         // return new speed 0 or 
00109         return 54 - position;
00110     }
00111     return speed;  
00112 }
00113 
00114 // update FSM 
00115 void AccCar::update()
00116 {
00117     while (true) 
00118     {
00119         ThisThread::sleep_for(TICK);
00120         road->go_flags.wait_all(flag);
00121         cycles++;
00122 #ifndef PUBLISH_ONLY 
00123         if(cycles > 1)
00124         {
00125             // get control msg  
00126             control_msg_t* c_msg = this->singleton->get_control_msg(this->car_id);
00127             if(c_msg == NULL)
00128             {
00129                 // test if we actually have a large delay
00130                assert(c_msg != NULL);
00131                speed = enter_safety_mode();
00132             }
00133             else{
00134             //
00135             // do something with the control message
00136                 assert(c_msg->car_id == car_id );
00137                 if(c_msg->speed > speed){
00138                     printf(ERROR_TAG" msg speed %d , speed %d car %d"DELIM,c_msg->speed,speed, car_id);
00139                 }
00140                 
00141                 else{
00142                     // add wait time if we got 0 or something less than our sent speed
00143                     if(speed == 0 || c_msg->speed < speed)
00144                     {
00145                         wait_time++;
00146                         printf(ACC_TAG"car %d has waited %d"DELIM,car_id,wait_time);
00147                     }
00148                     speed = c_msg->speed;
00149                 }
00150                 
00151                 
00152                 //  
00153                 delete c_msg;
00154             }
00155         }
00156 #endif
00157 
00158         position = position + speed;
00159         
00160         // ensure correctness
00161         if(!lead){
00162             //if(position >= this->forward_car->position - 2){
00163               //  printf(ERROR_TAG"position %d forward pos %d id %d speed %d"DELIM,position,this->forward_car->position, car_id,speed);
00164                 assert(position <= this->forward_car->position - 2);    
00165            // }
00166         }
00167         
00168         if( (cycles % 5) == 0)
00169         {
00170             get_new_target_speed();    
00171         }
00172         
00173         // update position and check intersections
00174         if( state == NORMAL_STATE ) 
00175         {
00176             drive_normal();
00177         }
00178         else
00179         {   
00180             // case should not happen
00181             assert(0);
00182         }
00183         
00184 //#ifdef DEBUG_ACC
00185 //        printf(ACC_TAG "sending position to server DELIM");
00186 //#endif
00187         
00188 //#ifdef DEBUG_ACC
00189 //        printf(ACC_TAG "car %d done DELIM",car_id);
00190 //#endif
00191 
00192         // make a message
00193         make_position_msg(msg);
00194         
00195         // stuff a message in the queue
00196         this->singleton->add_to_position_queue(msg);
00197         
00198         // set flag for completion of cycle
00199         road->done_flags.set(this->flag);
00200     }
00201 }
00202 
00203 void AccCar::reset(int speed)
00204 {
00205     road->done_flags.clear(flag);
00206 
00207     if (thread != NULL) {
00208         thread->terminate();
00209     }
00210 
00211 
00212     thread = new Thread();
00213     assert(thread != NULL);
00214     thread->start( callback(this, &AccCar::update) );
00215 
00216     this->position = 0;
00217     this->speed = speed;
00218     this->target_speed = speed;
00219 }
00220 
00221 void AccCar::stop()
00222 {
00223     if (thread != NULL) {
00224         thread->terminate();
00225     }
00226     delete thread;
00227     delete msg;
00228 }
00229 
00230 
00231 // check if we are going to go through an intersection
00232 // and stop or update speed accordingly
00233 void AccCar::drive_normal()
00234 {
00235     //assert( state == NORMAL_STATE );
00236     // first update our speed
00237     if( !lead ) 
00238     {
00239         update_speed();
00240     } else 
00241     {
00242         speed = target_speed;
00243     }
00244 }
00245 
00246 int AccCar::get_car_id()
00247 {
00248     return car_id;
00249 }
00250 
00251 int AccCar::get_cycles()
00252 {
00253     return cycles;
00254 }
00255 
00256 int AccCar::get_target_speed()
00257 {
00258     return target_speed;
00259 }