   timer1_enable(TIM_DIV1, TIM_EDGE, TIM_LOOP);
   timer1_write(8000000);
 }
+
 // odometry configuration
 ros::Time current_time = nh.now();
 ros::Time last_time = current_time;
-double DistancePerCount = (2 * 3.14159265 * 0.035) / 20;   // 2*PI*R/CPR
+double DistancePerCount = (TWO_PI * 0.035) / 20;   // 2*PI*R/CPR  WHEEL ENCODER 20 CPR
 double lengthBetweenTwoWheels = 0.13;
 int last_lmc = lmc;
 int last_rmc = rmc;

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

char ultrafrid[] = "/ultrasound";

void setup() {
  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;   // ultrasound frame id
  range_msg.field_of_view = 0.1;
  range_msg.min_range = 0.0;
  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
