diff --git a/ros_diffdrive_robot.ino b/ros_diffdrive_robot.ino
index 313a749..a455d9c 100644
--- a/ros_diffdrive_robot.ino
+++ b/ros_diffdrive_robot.ino
@@ -176,8 +176,8 @@ void onTwist(const geometry_msgs::Twist &msg)
 
   // Calculate the intensity of left and right wheels. Simple version.
   // Taken from https://hackernoon.com/unicycle-to-differential-drive-courseras-control-of-mobile-robots-with-ros-and-rosbots-part-2-6d27d15f2010#1e59
-  float l = (msg.linear.x - msg.angular.z) / 2;
-  float r = (msg.linear.x + msg.angular.z) / 2;
+  float l = (x - z) / 2;
+  float r = (x + z) / 2;
 
   // Then map those values to PWM intensities. PWMRANGE = full speed, while PWM_MIN = the minimal amount of power at which the motors begin moving.
   uint16_t lPwm = mapPwm(fabs(l), PWM_MIN, PWMRANGE);