 //Get desired input position while generating the actual position of the pillar in mm
  void pillar_cb( const std_msgs::Float32& cmd_msg){
   
-  des_pos = cmd_msg.data;
+  //convert from meters to mm (which is expected by the controller)
+  des_pos = cmd_msg.data * 1000;
   if (des_pos == -1.0){ //Homing condition
    md.setM1Speed(400);
    delay(5000);
   }
   else{
-  while(abs(des_pos-act_pos)>0.75)
-  {
-   // Read Current Position   
-   n = digitalRead(encoder0PinA);
-   if ((encoder0PinALast == LOW) && (n == HIGH)) {
-     if (digitalRead(encoder0PinB) == LOW) {
-       encoder0Pos--;
-     } else {
-       encoder0Pos++;
-     }
-     act_pos = (encoder0Pos*9.0)/14.0;
-     //Serial.print (act_pos);
-     //Serial.println (" mm");
+    while(abs(des_pos-act_pos)>0.75)
+    {
+     // Read Current Position   
+     n = digitalRead(encoder0PinA);
+     if ((encoder0PinALast == LOW) && (n == HIGH)) {
+       if (digitalRead(encoder0PinB) == LOW) {
+         encoder0Pos--;
+       } else {
+         encoder0Pos++;
+       }
+       act_pos = (encoder0Pos*9.0)/14.0;
+     } 
+     encoder0PinALast = n;
+  
+  
+     //Feedback proportional gain control
+     gain = -70.0;
+     err = gain*(des_pos - act_pos);
+  
+  
+    //Saturation
+    if (err>400.0) err =400.0;
+    if (err<-400.0) err = -400.0;
+    //Stop chatter due to inaccuracies.
+    if (abs(err)<10.0)  md.setM1Speed(0);
+    else md.setM1Speed((int)err);
      
-   } 
-   encoder0PinALast = n;
-
-
-   //Feedback proportional gain control
-   gain = -70.0;
-   err = gain*(des_pos - act_pos);
-
-
-  //Saturation
-  if (err>400.0) err =400.0;
-  if (err<-400.0) err = -400.0;
-  //Serial.print("Motor Speed = ");
-  //Serial.println(err);
-  //Stop chatter due to inaccuracies.
-  if (abs(err)<10.0)  md.setM1Speed(0);
-  else md.setM1Speed((int)err);
-   
-  stopIfFault();
-  
-  }
+    stopIfFault();
+    
+    }
   } 
 }
-
- std_msgs::Float32 enc_data;
+ std_msgs::Float32 enc_data; 
  //Subscribe to the node that provides desired position
- ros::Subscriber<std_msgs::Float32> s("Desired", &pillar_cb);
+ ros::Subscriber<std_msgs::Float32> s("/pillar/request", &pillar_cb);
  //Publish the encoder data
- ros::Publisher p("Current", &enc_data);
+ ros::Publisher p("/pillar/current", &enc_data);
  
  void setup() { 
    pinMode (encoder0PinA,INPUT);
    pinMode (encoder0PinB,INPUT);
    pinMode (inputPinA,INPUT);
    pinMode (inputPinB,INPUT);
-   //Serial.begin (115200);
    //Serial.println("Initializing Pillar...");
    //Enforce Baud rate in ROS  
    nh.getHardware()->setBaud(115200);

/* Read Quadrature Encoder
  * Connect Encoder to Pins encoder0PinA, encoder0PinB, and +5V.
  *
  * Sketch by max wolf / www.meso.net
  * v. 0.1 - very basic functions - mw 20061220
  *
  */  

#include <ros.h>
#include <std_msgs/Float32.h>

#include "DualMC33926MotorShield.h"

DualMC33926MotorShield md;

   nh.subscribe(s);
   //Initialize the motor controller
   md.init();
   
 } 

 void loop() {
  // convert from mm to meters
  enc_data.data = act_pos / 1000;
  p.publish(&enc_data);
  nh.spinOnce();
  delay(10);
