protected override MotionSnapshot TakeSnapshot()
{
return(new MotionSnapshot {
Position = positionTracker.Position,
Yaw = yawGyroscope.GetAngle()
});
}
public void Update()
{
var theta = gyroscope.GetAngle();
// capture device data
var leftShaftAngle = leftShaftEncoder.Angle;
var rightShaftAngle = rightShaftEncoder.Angle;
var leftShaftAngularVelocity = leftShaftEncoder.AngularVelocity;
var rightShaftAngularVelocity = rightShaftEncoder.AngularVelocity;
Console.WriteLine(leftShaftEncoder.Name + " " + leftShaftAngle + " " + leftShaftAngularVelocity + " " + rightShaftEncoder.Name + " " + rightShaftAngle + " " + rightShaftAngularVelocity);
// compute position from wheel displacement and gyro
var deltaLeftShaftAngle = leftShaftAngle - lastLeftShaftAngle;
var deltaRightShaftAngle = rightShaftAngle - lastRightShaftAngle;
var forwardAlignedDisplacementMagnitude = wheelMetersPerShaftRadian * (deltaLeftShaftAngle + deltaRightShaftAngle) / 2.0f;
var displacement = new Vector2D(0, forwardAlignedDisplacementMagnitude).Rotate(Angle.FromRadians(theta));
position += displacement;
// compute velocity from wheel rate and gyro.
var forwardAlignedVelocityMagnitude = wheelMetersPerShaftRadian * (leftShaftAngularVelocity + rightShaftAngularVelocity) / 2.0f;
velocity = new Vector2D(0, forwardAlignedVelocityMagnitude).Rotate(Angle.FromRadians(theta));
// Console.WriteLine(displacement + " " + position);
// update last angle
lastLeftShaftAngle = leftShaftAngle;
lastRightShaftAngle = rightShaftAngle;
}