modify table look-up

src/decition/decition_brain_sf/decition/adc_controller/pid_controller.cpp

@@ -100,10 +100,11 @@ float iv::decition::PIDController::getPidAngle(float realSpeed, std::vector<Poin
         }
     }
 
+    if(gpsIndex == 0)gpsIndex = nsize -1;
     EPos = trace[gpsIndex].x;
 
     for (unsigned int i = max(0, gpsIndex - 3); i < min((size_t)(gpsIndex + 3), trace.size()); i++) {
-        farTrace.push_back(trace[gpsIndex]);
+        farTrace.push_back(trace[i]);
     }
     if (farTrace.size() == 0) {
         EAng = 0;

src/decition/decition_brain_sf/decition/decide_gps_00.cpp

@@ -958,6 +958,28 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
 
     double brake_distance=200;
     brake_distance=max(250,(int)(dSpeed*dSpeed+150));
+
+
+    int nmapsize = gpsMapLine.size();
+    double distoend = sqrt(pow(now_gps_ins.gps_x - gpsMapLine[nmapsize-1]->gps_x,2)
+                           +pow(now_gps_ins.gps_y - gpsMapLine[nmapsize-1]->gps_y,2));
+
+    double acc_end = 0.1;
+
+    if(1)//(IsINterpolationOK())
+    {
+                if(!circleMode && distoend<50){
+                double nowspeed = realspeed/3.6;
+                if((distoend<10)||(distoend<(nowspeed*nowspeed)))
+                {
+                    if(distoend == 0.0)distoend = 0.1;
+                    acc_end = (-1.0)*nowspeed*nowspeed/(2.0*distoend);
+                    if(acc_end<(-3.0))acc_end = -3.0;
+                }
+
+                if(distoend < 0.1)acc_end = -0.5;
+                }
+    }else{
     if(!circleMode && PathPoint>gpsMapLine.size()-brake_distance){
             minDecelerate=-0.5;
             std::cout<<"map finish brake"<<std::endl;
@@ -996,31 +1018,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
                                 }
                     }
             }
-//            if(PathPoint+500<gpsMapLine.size())
-//            {
-//                if(gpsMapLine[PathPoint+500]->mode2==23 && realspeed>20)
-//                                        minDecelerate=-0.5;
-//                else if(gpsMapLine[PathPoint+300]->mode2==23 && realspeed>15)
-//                                        minDecelerate=-0.5;
-//                else if(gpsMapLine[PathPoint+100]->mode2==23)
-//                                        minDecelerate=-0.6;
-//            }
-//            else if(PathPoint+300<gpsMapLine.size()){
-//                if(gpsMapLine[PathPoint+300]->mode2==23 && realspeed>15)
-//                                        minDecelerate=-0.5;
-//                else if(gpsMapLine[PathPoint+100]->mode2==23)
-//                                        minDecelerate=-0.6;
-//            }
-//            else if(PathPoint+150<gpsMapLine.size()){
-//                if(gpsMapLine[PathPoint+150]->mode2==23 && realspeed>10)
-//                                        minDecelerate=-0.5;
-//                else if(gpsMapLine[PathPoint+100]->mode2==23)
-//                                        minDecelerate=-0.6;
-//            }
-//            else if(PathPoint+100<gpsMapLine.size()){
-//                    if(gpsMapLine[PathPoint+100]->mode2==23)
-//                                        minDecelerate=-0.6;
-//            }
+    }
     }
 
     if(!ServiceCarStatus.inRoadAvoid){
@@ -1181,11 +1179,14 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
     }
 
     controlAng= getAngle(gpsTraceNow,now_gps_ins,gps_decition);
-    if(!circleMode && PathPoint>(gpsMapLine.size()-250)){
-        if(realspeed<0.5)
-            controlAng=0;
-    }
+//    if(!circleMode && PathPoint>(gpsMapLine.size()-250)){
+//        if(realspeed<0.5)
+//            controlAng=0;
+//    }
 
+    if(!circleMode && PathPoint>(gpsMapLine.size()-20)){
+        controlAng=0;
+    }
 
 
     //1220
@@ -2010,6 +2011,14 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
     }
 
     gps_decition->wheel_angle = 0.0 - controlAng;
+    if(1)//(IsINterpolationOK())
+    {
+    if(acc_end<0)
+    {
+        if(minDecelerate > acc_end)minDecelerate = acc_end;
+    }
+    }
+
     phaseSpeedDecition(gps_decition, secSpeed, obsDistance, obsSpeed,now_gps_ins);
 
 
@@ -2071,6 +2080,14 @@ void iv::decition::DecideGps00::phaseSpeedDecition(iv::decition::Decition  decit
 
     pid_Controller->getControlDecition( gpsIns, gpsTraceNow,  dSpeed,  obsDis,  obsSpeed, false, true, &decition);
 
+    if(1)//(IsINterpolationOK())
+    {
+        if((decition->accelerator > minDecelerate) && (minDecelerate < 0))
+        {
+            decition->accelerator = minDecelerate;
+        }
+    }
+
     if(ServiceCarStatus.msysparam.mvehtype=="ge3"){
         ge3_Adapter->getAdapterDeciton(gpsIns, gpsTraceNow, dSpeed, obsDis,obsSpeed,  decition->accelerator,ServiceCarStatus.mfCalAcc  ,changingDangWei, &decition);
     }else if( ServiceCarStatus.msysparam.mvehtype=="qingyuan"){