/// <summary>
/// takes current state/pose, new sensors data, odometry and everything else - and evaluates new state and pose
/// </summary>
/// <param name="behaviorData"></param>
public void EvaluatePoseAndState(IBehaviorData behaviorData, IDrive driveController)
{
// consider compass or AHRS reading for determining direction:
if(behaviorData.sensorsData.CompassHeadingDegrees != null)
{
behaviorData.robotPose.direction.heading = behaviorData.sensorsData.CompassHeadingDegrees;
}
// use GPS data to update position:
if (behaviorData.sensorsData.GpsFixType != GpsFixTypes.None)
{
behaviorData.robotPose.geoPosition.Lat = behaviorData.sensorsData.GpsLatitude;
behaviorData.robotPose.geoPosition.Lng = behaviorData.sensorsData.GpsLongitude;
}
// utilize odometry data:
if (driveController.hardwareBrickOdometry != null)
{
// already calculated odometry comes from the hardware brick (i.e. Arduino)
IOdometry odom = driveController.hardwareBrickOdometry;
behaviorData.robotPose.XMeters = odom.XMeters;
behaviorData.robotPose.YMeters = odom.YMeters;
behaviorData.robotPose.ThetaRadians = odom.ThetaRadians;
}
else
{
// we have wheels encoders data and must calculate odometry here:
long[] encoderTicks = new long[] { behaviorData.sensorsData.WheelEncoderLeftTicks, behaviorData.sensorsData.WheelEncoderRightTicks };
driveController.OdometryCompute(behaviorData.robotPose, encoderTicks);
}
}
/// <summary>
/// takes current state/pose, new sensors data, odometry and everything else - and evaluates new state and pose
/// </summary>
/// <param name="behaviorData"></param>
public void EvaluatePoseAndState(IBehaviorData behaviorData, IDrive driveController)
{
bool wasHeadingSetByCompass = false;
IDisplacement displacement = null;
// consider compass or AHRS reading for determining direction:
//if (!useOdometryHeading && behaviorData.sensorsData.CompassHeadingDegrees != null)
//{
// behaviorData.robotPose.direction.heading = behaviorData.sensorsData.CompassHeadingDegrees;
// wasHeadingSetByCompass = true;
//}
// use GPS data to update position:
if (behaviorData.sensorsData.GpsFixType != GpsFixTypes.None)
{
// we need to know displacement in meters:
displacement = new GeoPosition(behaviorData.sensorsData.GpsLongitude, behaviorData.sensorsData.GpsLatitude) - behaviorData.robotPose.geoPosition;
behaviorData.robotPose.geoPosition.Lat = behaviorData.sensorsData.GpsLatitude;
behaviorData.robotPose.geoPosition.Lng = behaviorData.sensorsData.GpsLongitude;
}
else
{
// utilize odometry data
// we might have wheels encoders data - it will be ignored if using hardwareBrickOdometry:
long[] encoderTicks = new long[] { behaviorData.sensorsData.WheelEncoderLeftTicks, behaviorData.sensorsData.WheelEncoderRightTicks };
// compute dXMeters dYMeters dThetaRadians:
displacement = driveController.OdometryCompute(behaviorData.robotPose, encoderTicks);
}
if (displacement != null)
{
// update robotPose: XMeters YMeters, Lat, Lng, ThetaRadians, heading:
behaviorData.robotPose.translate(displacement.dXMeters, displacement.dYMeters);
behaviorData.robotPose.rotate(displacement.dThetaRadians);
}
if (!wasHeadingSetByCompass)
{
// rotate according to odometry:
SetCurrentHeadingByTheta(behaviorData);
}
}