 char ultrafrid[] = "/ultrasound";
 
 void setup() {
-  if(DEBUG)Serial.begin(115200);
+  if(DEBUG) Serial.begin(115200);
   setupWiFi();
   delay(2000);
+  
   nh.getHardware()->setConnection(server, serverPort);
   nh.initNode();
   broadcaster.init(nh);
+
   nh.advertise(leftenc);
   nh.advertise(rightenc);
   nh.advertise(pub_range);
   range_msg.radiation_type = sensor_msgs::Range::ULTRASOUND;
-  range_msg.header.frame_id =  ultrafrid;
+  range_msg.header.frame_id =  ultrafrid;   // ultrasound frame id
   range_msg.field_of_view = 0.1;
   range_msg.min_range = 0.0;
-  range_msg.max_range = 6.47;
+  range_msg.max_range = 20;
   nh.advertise(angle);
   nh.subscribe(sub_r);
   nh.subscribe(sub_l);
   nh.subscribe(sub_f);
   nh.subscribe(sub_b);
-
+// configure GPIO's
   pinMode(D0, OUTPUT); // Ultrasonic Trigger
   pinMode(D1, OUTPUT); // 1,2EN aka D1 pwm left
   pinMode(D2, OUTPUT); // 3,4EN aka D2 pwm right

#include <sensor_msgs/Range.h>
#include <Servo.h>

#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 rmc0=0;
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 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//
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;

// 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;

// Functions definitions //

void setupWiFi() { /// connect to ROS server WiFi as a client
  // Use ESP8266 serial to monitor the process
  Serial.begin(115200);
  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);

  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;
