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