     compass = HMC5883L();
     compass.SetScale(1.3);
     compass.SetMeasurementMode(Measurement_Continuous);
+ //SERVOS
+    srv_carpet.attach(SERVO1_PIN);
+    srv_clapper.attach(SERVO2_PIN);
  //MISCELLANEOUS
     pinMode(25, OUTPUT); // BLUE LED
     pinMode(26, OUTPUT); // YELLOW LED
     pinMode(27, OUTPUT); // RED LED
+    pinMode(BTN_PIN, INPUT);
  // INTERRUPTS
     attachInterrupt(ENCODER0_INT, encoder0_cb, RISING);
     pinMode(ENCODER0_INT_WIRE, INPUT);

 // VAR INITIALIZATION
    encoder0_cnt = 0;
    encoder1_cnt = 0;
    data_raw.data_length = 13; // The number of output message entries
    data_raw.data = (float *)malloc(sizeof(float)*data_raw.data_length);
 //MOTORS           
    md.init();
 //ROS        
    nh.initNode();
    nh.subscribe(sub);
    nh.advertise(pub_array);
}

void loop()
{  
    if ( (millis()-publish_timer) > 200) {
        MagnetometerScaled scaled = compass.ReadScaledAxis();      
 //IMU
        data_raw.data[IMU_ANG_VX] = ToD((float)(imu.GyroX()));

#define BAT_VOLT_2 10 // Battery voltage on BAT2_PIN
#define ENC_DATA_0 11 // Speed on encoder 0
#define ENC_DATA_1 12 // Speed on encoder 1
#define BTN_STATE  13 // Power button state
#define DATA_LENGTH 14
           
//FUNCTIONS
void setup();
void loop();
void motor_cb(const geometry_msgs::Twist& cmd_msg);
void srv_carpet_cb(const std_msgs::Int32& msg);
void srv_clapper_cb(const std_msgs::Int32& msg);
void encoder0_cb();
void encoder1_cb();

