     analogWrite(D2, 0);
 }
  
-void motion(int lpw, int rpw, int llevel, int rlevel, int steps) {  // generic motion with "steps" duration in milisecods
+void motion(int lpw, int rpw, int llevel, int rlevel, int period) {  // generic motion for a period in milisecods
     if (llevel==HIGH) {
       ldir=1; 
     } else {

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