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

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

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

  vel = msg.data;
}
void BWD1(){
  // look for a low-to-high on channel A
  if (digitalRead(19) == HIGH) { 
    // check channel B to see which way encoder is turning
    if (digitalRead(18) == HIGH) {  
      encoder1_msg.data--;         // CW
    } 
    else {
      //encoder1_msg.data--;         // CCW
    }
  }
 
}

void BWD2(){
  // look for a low-to-high on channel A
  if (digitalRead(18) == HIGH) { 
    // check channel B to see which way encoder is turning
    if (digitalRead(19) == HIGH) {  
      encoder1_msg.data++;         // CW
    } 
    else {
      //encoder1_msg.data--;         // CCW
    }
  }
 
}



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

