Advertisement
Guest User

Why is this stuck in 'Slowing'?

a guest
Jan 17th, 2019
85
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C++ 3.90 KB | None | 0 0
  1. enum driverStates
  2. {
  3.     Accelerating,
  4.     Slowing,
  5.     Cornering,
  6. };
  7.  
  8. driverStates currentState;
  9.  
  10. int timeInCurrentGear = 0;
  11.  
  12. int distToCorner = 0;
  13.  
  14. int maxAllowedCornerSpeed = 20;
  15.  
  16. static void  
  17. drive(int index, tCarElt* car, tSituation *s)
  18. {
  19.     memset((void *)&car->ctrl, 0, sizeof(tCarCtrl));
  20.    
  21.     currentState = Accelerating;
  22.  
  23.     int currentGear = car->_gear;
  24.     int currentSpeed = car->_speed_x;
  25.  
  26.     if (car->_trkPos.seg->type == TR_LFT || car->_trkPos.seg->type == TR_RGT)
  27.     {
  28.         currentState = Cornering;
  29.     }
  30.     else if (car->_trkPos.seg->type == TR_STR)
  31.     {
  32.         currentState = Accelerating;
  33.     }
  34.  
  35.     //Track types are stored as ints, and defined as
  36.     //      TR_RGT = 1
  37.     //      TR_LFT = 2
  38.     //      TR_RGT = 3
  39.     //within track.h, making this for loop and list iteration nice and simple.
  40.  
  41.     std::list<trackSeg *> nextThirtySegs;
  42.     trackSeg * currSeg = car->_trkPos.seg;
  43.  
  44.     for (int i = 0; i < 30; i++)       
  45.     {
  46.         nextThirtySegs.push_front(currSeg);
  47.         currSeg = currSeg->next;
  48.     }
  49.  
  50.     //The below is a range-based for loop, apparently introduced with C++11, more readable for me
  51.     //than using something like 'for (nextTenSegs::iterator i = nextTenSegs.begin(); i != nextTenSegs.end(); i++){}'
  52.  
  53.     //We check the next 30 segments for a corner, and when we find one we break the loop
  54.     //and calculate the distance to that segment.
  55.     for (auto const& i : nextThirtySegs)
  56.     {
  57.         if (i->type == TR_RGT || i->type == TR_LFT)
  58.         {
  59.             currentState = Slowing;
  60.             std::cout << "corner detected" << std::endl;
  61.             break;
  62.         }
  63.         else
  64.         {
  65.             distToCorner += i->length;
  66.            
  67.         }
  68.     }
  69.  
  70.     nextThirtySegs.clear();
  71.     //std::cout << distToCorner << std::endl;
  72.  
  73.     //Tells us the angle of the car.
  74.     float carAngle = car->_yaw;
  75.    
  76.     //Tells us (in metres) how far we are from the middle 'line' of the current track segment,
  77.     //we then divide it by the width of the track segment to avoid oversteering.
  78.     float disToMiddle = car->_trkPos.toMiddle / car->_trkPos.seg->width;
  79.  
  80.     //Tells us the width of the current track segment (in metres).
  81.     float segWidth = car->_trkPos.seg->width;
  82.  
  83.     //This calculates the turning angle of the current track segment in radians.
  84.     float angleOfTrack = RtTrackSideTgAngleL(&(car->_trkPos));
  85.    
  86.     float steering = angleOfTrack - carAngle - disToMiddle;
  87.    
  88.     //Bringing the steering value to within -PI / PI.
  89.     NORM_PI_PI(steering);
  90.  
  91.     //the 'car->_steerLock' ensures the value of the steering stays within -1 to +1.
  92.     car->_steerCmd = steering / car->_steerLock;
  93.  
  94.  
  95.     if (currentState = Accelerating)
  96.     {
  97.  
  98.         //std::cout << "Accelerating" << std::endl;
  99.         car->ctrl.brakeCmd = 0.0;
  100.         car->_accelCmd = 0.4;
  101.  
  102.         if (car->_enginerpm >= 850.0)
  103.         {
  104.             currentGear++;
  105.             timeInCurrentGear = 0;
  106.         }
  107.         else if (car->_enginerpm <= 450.0 && currentGear >= 2 && timeInCurrentGear >= 50)
  108.         {
  109.             currentGear--;
  110.             timeInCurrentGear = 0;
  111.         }
  112.     }
  113.     else if (currentState = Slowing)
  114.     {
  115.         //std::cout << "Slowing" << std::endl;
  116.         if (currentSpeed > maxAllowedCornerSpeed)
  117.         {
  118.             car->_accelCmd = 0.0;
  119.             car->ctrl.brakeCmd = 0.5;
  120.         }
  121.         else
  122.         {
  123.             car->ctrl.brakeCmd = 0.0;
  124.             car->_accelCmd = 0.4;
  125.  
  126.             if (car->_enginerpm >= 850.0)
  127.             {
  128.                 currentGear++;
  129.                 timeInCurrentGear = 0;
  130.             }
  131.             else if (car->_enginerpm <= 450.0 && currentGear >= 2 && timeInCurrentGear >= 50)
  132.             {
  133.                 currentGear--;
  134.                 timeInCurrentGear = 0;
  135.             }
  136.         }
  137.     }
  138.     else if (currentState = Cornering)
  139.     {
  140.         //std::cout << "Cornering" << std::endl;
  141.  
  142.         car->_brakeCmd = car->_trkPos.seg->next->arc * 2;
  143.         car->_accelCmd = 0.4;
  144.  
  145.         if (car->_enginerpm >= 600 && currentGear >= 2)
  146.         {
  147.             currentGear--;
  148.             timeInCurrentGear = 0;
  149.         }
  150.     }
  151.  
  152.     timeInCurrentGear++;
  153.  
  154.     distToCorner = 0;
  155.  
  156.     car->_gearCmd = currentGear;
  157.  
  158.     //std::cout << car->_enginerpm << std::endl;
  159.     //std::cout << timeInCurrentGear << std::endl;
  160.     //std::cout << car->_trkPos.seg->type << std::endl;
  161.     //std::cout << currentState << std::endl;
  162.     //std::cout << currentSpeed << std::endl;
  163. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement