 
         state_pub.publish(&bool_msg);
         pressure_pub.publish(&float_msg);
+        servo_state_pub.publish(&servo_state_msg);
+        servo_angle_pub.publish(&servo_angle_msg);
 
         publisher_timer = millis() + 100;
     }

void init_servo()
{
    myservo.attach(5);
}

void initBME()

    dig_P7 = (data[19] << 8) | data[18];
    dig_P8 = (data[21] << 8) | data[20];
    dig_P9 = (data[23] << 8) | data[22];
}

void readData()
