2 replaced http://robotics.stackexchange.com/ with https://robotics.stackexchange.com/ edited Apr 13 '17 at 12:49 Your biggest problem is, as NBCKLY asked aboutNBCKLY asked about, the encoder counts that are returned to you from OP code 43 and 44 are not signed. So, when you your math, you are doing the following: Your biggest problem is, as NBCKLY asked about, the encoder counts that are returned to you from OP code 43 and 44 are not signed. So, when you your math, you are doing the following: Your biggest problem is, as NBCKLY asked about, the encoder counts that are returned to you from OP code 43 and 44 are not signed. So, when you your math, you are doing the following: 1 answered Jun 30 '16 at 13:59 Chuck♦ 11.4k22 gold badges1111 silver badges3434 bronze badges First, I'll point out that OP code 20 will just give you the angle directly. Remember if you use that code then, The value returned must be divided by 0.324056 to get degrees. Regarding your code specifically, it looks like you're trying to do: $$\frac{\mbox{right distance} - \mbox{left distance}}{\mbox{wheel base}} \\$$ This is a rearrangement of the arc length formula: $$s = r\theta \\$$ where $$s$$ is the arc traversed (difference in wheel distances), $$r$$ is the radius of the circle (the wheel base), and $$\theta$$ is the angle traversed, in radians. So, one of your problems is that you are using an int to define your angle value - int is short for integer, meaning that it can only be whole number. Radians are very small in magnitude compared to degrees. One complete circle is $$2\pi$$ radians, or about 6.28. Compare this to degrees, which is 360. This means that one radian is about 60 degrees; you won't get any update because of your unit definition. Your biggest problem is, as NBCKLY asked about, the encoder counts that are returned to you from OP code 43 and 44 are not signed. So, when you your math, you are doing the following: $$\frac{ \frac{\mbox{right encoder}}{\mbox{counts per rev}} (\pi d) - \frac{\mbox{left encoder}}{\mbox{counts per rev}} (\pi d)}{\mbox{wheel base}} \\$$ BUT, your drive commands are equal in magnitude. This means that $$\mbox{right encoder}\approx \mbox{left encoder}$$ because the encoder counts are not signed. That is, even though your drive command is negative, the counts come back as positive. So, here's what you can do to fix this**: Multiply the encoder counts by the sign (not sine) of the respective drive command. Use a float for the angle and/or use degrees for the angle by multiplying your existing code by (180/3.1415). ** - A final note on your code: You're going to have trouble doing it the way you are at the moment because you're using total encoder counts for your math. Say you get to 16,000 counts and change direction on that motor. What happens? Well, the way I've written above (which fixes your current issue), you go from +16,000 to -16,000 (or vice-versa). What you should consider doing instead is to accumulate an angle and to evaluate only the elapsed wheel encoder counts. That is, I would do the following (pseudo-code): float angle; angle = 0; leftEncoder = ; rightEncoder = ; /* Rollover protection on the encoder */ if leftDriveCommand > 0 if leftEncoder < prevLeftEncoder prevLeftEncoder = prevLeftEncoder - 65535; end elseif leftDriveCommand < 0 if leftEncoder > prevLeftEncoder prevLeftEncoder = prevLeftEncoder + 65535; end end elapsedLeft = leftEncoder - prevLeftEncoder; prevLeftEncoder = leftEncoder; elapsedRight = rightEncoder - prevRightEncoder; prevRightEncoder = rightEncoder; angleIncrement = (elapsedRight - elapsedLeft)*(pi*d/countsPerRev)/(wheelBase); angle = angle + angleIncrement;  So in this way you accumulate your angle by looking at how much of an angle has elapsed since the last time you updated your sensor. In this way, you don't have to worry about what happens with encoder counts when you reverse polarity on a drive command. You should also consider what is going to happen when you roll over from 65535 back to 0. Or again, just use OP code 20 to get the angle directly.