diff --git a/src/segway_RMP400.cpp b/src/segway_RMP400.cpp
index de5f2456bf86613f38cf93e3b157c7727ba4ad9b..841831375ceceb4753e3f10167d2c6bcd0be128b 100644
--- a/src/segway_RMP400.cpp
+++ b/src/segway_RMP400.cpp
@@ -198,7 +198,7 @@ CSegwayRMP400::get_yaw_displacement()
     if (use_correction_wheel_factor_)
         fix_with_wheel_factor(yaw_displacement_tmp);
 
-    yaw_displacement_ = yaw_displacement_tmp * 2 * PI; // convert to radians
+    yaw_displacement_ = yaw_displacement_tmp;
     return yaw_displacement_;
 }
 
@@ -207,7 +207,7 @@ CSegwayRMP400::get_yaw_rate()
 {
     need_to_be_connected();
 
-    yaw_rate_ = get_yaw_rate() * (PI / 180);
+    yaw_rate_ = get_yaw_rate();
     return yaw_rate_;
 }
 
@@ -252,7 +252,7 @@ CSegwayRMP400::move(float vT, float vR)
 {
     need_to_be_connected();
 
-    float vRradsec = vR * 2.0 * PI;  // revs per sec to rads per sec
+    float vRradsec = vR;
 
     if (fabs(vT) < fabs(RMP400_b * vRradsec / 2)) {
         // minimum translational velocity needed to enforce rotation about a ICR outside of the robot base
diff --git a/src/segway_RMP400.h b/src/segway_RMP400.h
index a7ddf7ec7231dcc6247e75ec3b25e014b35a4ec3..60dfe6003def4e344ddd5434691d9f7a4073587c 100644
--- a/src/segway_RMP400.h
+++ b/src/segway_RMP400.h
@@ -3,13 +3,11 @@
 
 #include <segway_rmp200.h>
 #include <ftdiserver.h>
-#include <math.h>
 #include <vector>
 #include <string>
 #include <iostream>
 
 #define RMP400_b 0.54 // distance between weels for one rmp unit in meters
-#define PI 3.141592
 #define RMP200_DEFAULT_WHEEL_RADIUS 0.24
 
 const int NUM_SEGWAY_200 = 2;