/**
* Calculates the heading error of the robot at the specified time.
*
* @param time The time to consider.
* @return The heading error of the robot.
*/
private HeadingDivergenceState getHeadingError(long time) {
double currHeading = robotData.getHeading(time);
double idealHeading = robotData.getIdealHeading(time);
double headingError = pharoslabut.navigate.Navigate.headingError(currHeading, idealHeading);
return new HeadingDivergenceState(time, currHeading, idealHeading, headingError);
}
