diff --git a/app/src/main/java/xdsopl/robot36/FrequencyModulation.java b/app/src/main/java/xdsopl/robot36/FrequencyModulation.java
index 7f2c287..cb85245 100644
--- a/app/src/main/java/xdsopl/robot36/FrequencyModulation.java
+++ b/app/src/main/java/xdsopl/robot36/FrequencyModulation.java
@@ -9,12 +9,22 @@ package xdsopl.robot36;
 public class FrequencyModulation {
 	private float prev;
 	private final float scale;
+	private final float Pi, TwoPi;
 	FrequencyModulation(float bandwidth, float sampleRate) {
-		scale = sampleRate / (bandwidth * (float) Math.PI);
+		this.Pi = (float) Math.PI;
+		this.TwoPi = 2 * this.Pi;
+		scale = sampleRate / (bandwidth * Pi);
+	}
+	private float wrap(float value) {
+		if (value < -Pi)
+			return value + TwoPi;
+		if (value > Pi)
+			return value - TwoPi;
+		return value;
 	}
 	float demod(Complex input) {
 		float phase = input.arg();
-		float delta = (phase - prev) % (float) Math.PI;
+		float delta = wrap(phase - prev);
 		prev = phase;
 		return scale * delta;
 	}