 #include <sensor_msgs/Range.h>
 #include <Servo.h>
 
+// Init constants and global variables
+
 #define DEBUG 1
 #define TRIGGER D8  // ultrasonic trigger pin
 #define ECHO    D0  // ultrasonic echo pin  
                     //(!!Note: use 5v Vcc on ultrasonic board & a 2k,1k divider for ECHO GPIO protection)
-
 int spd=800;
 int lpwm=spd;
 int rpwm=spd;
-int len=300; // period in ms
-int lmc=0;   // left motor counter
-int rmc=0;   // right motor counter
-int lmc0=0; // last encoder value 
+int len=300;    // period in ms
+int lmc=0;      // left motor counter
+int rmc=0;   
+int lmc0=0;     // last encoder value 
 int rmc0=0;
-int ldir=1;  // left motor direction
+int ldir=1;     // left motor direction
 int rdir=1;
-int lv=0; // left motor actual speed
-int rv=0; // measured in n tics/timer period
-int ltp=0; // left motor target position 
+int lv=0;       // left motor actual speed
+int rv=0;       // measured in n tics/timer period
+int ltp=0;      // left motor target position 
 int rtp=0;
 Servo s;
-int sa=80; // Servo center position
-int sd=1;  // Servo step
-int smax=120; // Servo max angle
-int smin=40;  // Servo min Angle
-int sr=0;     // counter for 
-int sp=10;    // number of loops among range measurements
-
-// WiFi configuration. Replace *** by your data//
+int sa=80;      // Servo center position
+int sd=6;       // Servo position steps during swaping
+int smax=140;   // Servo max angle
+int smin=20;    // Servo min Angle
+int sr=0;       // counter for... 
+int sp=10;      // number of loops among range measurements
+
+// WiFi configuration. Replace '***' with your data
 const char* ssid = "***";
 const char* password = "***";
-// Set the rosserial socket server IP address
-IPAddress server(192,168,1,***);
-// Set the rosserial socket server port
-const uint16_t serverPort = 11411;
+IPAddress server(192.168.1.***);      // Set the rosserial socket server IP address
+const uint16_t serverPort = 11411;    // Set the rosserial socket server port
+
 
 // ROS nodes //
 ros::NodeHandle nh;
-geometry_msgs::TransformStamped t;
-geometry_msgs::TransformStamped t2;
-tf::TransformBroadcaster broadcaster;
-nav_msgs::Odometry odom_pub;
-sensor_msgs::Range range_msg;
+geometry_msgs::TransformStamped t;    // transformation frame for base toodom
+geometry_msgs::TransformStamped t2;   // transformation frame for ultrasonic to base
+tf::TransformBroadcaster broadcaster; 
+nav_msgs::Odometry odom_pub;          // Odometry message
+sensor_msgs::Range range_msg;         // Ultrasonic Range message
 
 // Functions definitions //
 
-void setupWiFi() { /// connect to ROS server WiFi as a client
-  // Use ESP8266 serial to monitor the process
-  Serial.begin(115200);
+void setupWiFi() {                    // connect to ROS server as as a client
+  Serial.begin(115200);               // Use ESP8266 serial only for to monitor the process
   Serial.println();
   Serial.print("Connecting to ");
   Serial.println(ssid);
-
-  // Connect the ESP8266 the the wifi AP
   WiFi.begin(ssid, password);
   while (WiFi.status() != WL_CONNECTED) {
     delay(500);

    t2.transform.translation.x = 0.05; 
    t2.transform.translation.y = 0.0; 
    t2.transform.translation.z = 0.1;
    double saangle = (sa-80)*0.0314159265;
    t2.transform.rotation = tf::createQuaternionFromYaw(saangle);
    t2.header.stamp = current_time;
    broadcaster.sendTransform(t2);
    int_msg.data = sa;

  motion(lpwm,rpwm,LOW,LOW,len);
}

int sstep(){                  // servo swaping commands
  sa+=sd;
  if(sa>smax || sa<smin) sd=-sd;
  s.write(sa);
}

double radian(int serout){               // servo 80 = 0 deg, servo -80 = - 90deg, servo 160 = 90 deg
  return (serout-80)*0.01963495408;  // angrad =(serout-80)/80 * 90 * PI / 180;
}
int srange(){                // calculate distance from ultrasonic sensor
  long duration, distance;
  digitalWrite(TRIGGER, LOW);  
  delayMicroseconds(2);   
  digitalWrite(TRIGGER, HIGH);
  delayMicroseconds(10); 
  digitalWrite(TRIGGER, LOW);
  duration = pulseIn(ECHO, HIGH);
  distance = (duration/2) / 29.1;
