 #include <std_msgs/Empty.h>
 #include <std_msgs/Int32.h>
 
-
 Adafruit_MotorShield AFMS = Adafruit_MotorShield();
 Adafruit_DCMotor *myMotor1 = AFMS.getMotor(1);
 

  vel = msg.data;
}
void BWD1(){
  encoder1_msg.data++;
}

ros::Subscriber<std_msgs::Float32> sub_vel1("/motor1_vel", &motor1VelCallBack);


  //Mega2560
  // external interrupt int.0    int.1    int.2   int.3   int.4   int.5            
  // pin                  2         3      21      20      19      18
  pinMode(19, INPUT); 
  pinMode(18, INPUT);
  
  attachInterrupt(4, BWD1, CHANGE); // Also LOW, RISING, FALLING
  attachInterrupt(5, BWD2, CHANGE); // Also LOW, RISING, FALLING

  AFMS.begin(); // create with the default frequency 1.6KHz
  //AFMS.begin(1000); // OR with a different frequency, say 1KHz
