/// <summary> /// Prioritizes up vector. /// </summary> public void FaceVectors2(Vector3D forward, Vector3D up) { // In (pitch, yaw, roll) Vector3D error = -GetAngles2(rc.WorldMatrix, forward, up); Vector3D angles = new Vector3D(anglePID.Control(error)); ApplyGyroOverride(rc.WorldMatrix, angles); }
public Vector3D ControlPosition(Vector3D targetPosition, Vector3D targetVelocity, double maxSpeed = double.PositiveInfinity) { return(ControlVelocity(targetVelocity + VectorClamp(-pid.Control(rc.GetPosition() - targetPosition), maxSpeed))); }