 #define ToG(x) (x*9.80665/16384)
 #define BAT1_PIN 4
 #define BAT2_PIN 5
+#define BTN_PIN 60
+#define SERVO1_PIN 7
+#define SERVO2_PIN 8
 
 #define ENCODER0_INT 0       // Interrupt number
 #define ENCODER0_INT_WIRE 2  // Interrupt input pin (according to datasheet)


//ROS-TOPICS
ros::Publisher pub_array("apm/data_raw",&data_raw);
ros::Subscriber<geometry_msgs::Twist> sub("cmd_vel", motor_cb);

void setup() {
 //I2C        

        data_raw.data[ENC_DATA_1] = encoder1_cnt;
        encoder0_cnt = 0;
        encoder1_cnt = 0;
 //BUTTON
        data_raw.data[BTN_STATE] = digitalRead(BTN_PIN);
 //ROS                     
        pub_array.publish(&data_raw);
        publish_timer = millis();
