diff --git a/sysid/src/main/native/cpp/analysis/FilteringUtils.cpp b/sysid/src/main/native/cpp/analysis/FilteringUtils.cpp index db0381103a..8432e7c8ff 100644 --- a/sysid/src/main/native/cpp/analysis/FilteringUtils.cpp +++ b/sysid/src/main/native/cpp/analysis/FilteringUtils.cpp @@ -146,9 +146,18 @@ sysid::TrimStepVoltageData(std::vector* data, wpi::sgn(b.velocity) * b.acceleration; }); + // Current limiting can delay onset of the peak acceleration, so we need to + // find the first acceleration *near* the max. Magic number tolerance here + // because this whole file is tech debt already + auto accelBegins = std::find_if( + data->begin(), data->end(), [&maxAccel](const auto& measurement) { + return wpi::sgn(measurement.velocity) * measurement.acceleration > + 0.8 * wpi::sgn(maxAccel->velocity) * maxAccel->acceleration; + }); + units::second_t velocityDelay; - if (maxAccel != data->end()) { - velocityDelay = maxAccel->timestamp - firstTimestamp; + if (accelBegins != data->end()) { + velocityDelay = accelBegins->timestamp - firstTimestamp; // Trim data before max acceleration data->erase(data->begin(), maxAccel);