ソースを参照

change decition_brain. for change end point angle calculate.

yuchuli 3 年 前
コミット
b097adb1d0

+ 2 - 0
src/decition/decition_brain/decition/adc_controller/pid_controller.cpp

@@ -92,6 +92,7 @@ float iv::decition::PIDController::getPidAngle(float realSpeed, std::vector<Poin
     std::vector<Point2D> farTrace;
 
     int nsize = trace.size();
+    if(nsize<2)return 0;
     for (int i = 1; i < nsize-1; i++)
     {
         sumdis += GetDistance(trace[i - 1], trace[i]);
@@ -101,6 +102,7 @@ float iv::decition::PIDController::getPidAngle(float realSpeed, std::vector<Poin
             break;
         }
     }
+    if(gpsIndex == 0)gpsIndex = nsize -1;
 
     EPos = trace[gpsIndex].x;
 

+ 1 - 1
src/decition/decition_brain/decition/decide_gps_00.cpp

@@ -1130,7 +1130,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
 
 
     controlAng= getAngle(gpsTraceNow,now_gps_ins,gps_decition);
-    if(!circleMode && PathPoint>(gpsMapLine.size()-100)){
+    if(!circleMode && PathPoint>(gpsMapLine.size()-3)){
         controlAng=0;
     }