|
@@ -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;
|
|
|
|