diff --git a/src/Braccio++.cpp b/src/Braccio++.cpp
index b1c9384..8116a8f 100644
--- a/src/Braccio++.cpp
+++ b/src/Braccio++.cpp
@@ -1,3 +1,7 @@
+/**************************************************************************************
+ * INCLUDE
+ **************************************************************************************/
+
 #include "Braccio++.h"
 
 #if LVGL_VERSION_MAJOR < 8 || (LVGL_VERSION_MAJOR == 8 && LVGL_VERSION_MINOR < 1)
@@ -6,25 +10,37 @@
 
 #include "mbed.h"
 
+/**************************************************************************************
+ * FUNCTION DECLARATION
+ **************************************************************************************/
+
 #if LV_USE_LOG
-void lvgl_my_print(const char * dsc)
-{
-  Serial.println(dsc);
-}
+void lvgl_my_print(const char * dsc);
 #endif /* #if LV_USE_LOG */
 
-extern "C" {
+extern "C"
+{
   void braccio_disp_flush(lv_disp_drv_t *disp, const lv_area_t *area, lv_color_t *color_p);
   void braccio_read_keypad(lv_indev_drv_t * indev, lv_indev_data_t * data);
+  void braccio_unlock_pd_semaphore_irq();
+  void braccio_unlock_pd_semaphore();
 };
 
+/**************************************************************************************
+ * NAMESPACE
+ **************************************************************************************/
+
 using namespace std::chrono_literals;
 
+/**************************************************************************************
+ * CTOR/DTOR
+ **************************************************************************************/
+
 BraccioClass::BraccioClass()
 : _i2c_mtx{}
-, serial485{Serial1, 0, 7, 8} /* TX, DE, RE */
-, servos{serial485}
-, PD_UFP{PD_LOG_LEVEL_VERBOSE}
+, _serial485{Serial1, 0, 7, 8} /* TX, DE, RE */
+, _servos{_serial485}
+, _PD_UFP{PD_LOG_LEVEL_VERBOSE}
 , _expander{TCA6424A_ADDRESS_ADDR_HIGH, _i2c_mtx}
 , _is_ping_allowed{true}
 , _is_motor_connected{false}
@@ -39,88 +55,51 @@ BraccioClass::BraccioClass()
 , _lvgl_p_obj_group{nullptr}
 , _lvgl_kb_indev{nullptr}
 , _display_thd{}
+, _pd_events{}
+, _pd_timer{}
+, _start_pd_burst{0xFFFFFFFF}
+, _pd_thd{osPriorityHigh}
 {
 
 }
 
+/**************************************************************************************
+ * PUBLIC MEMBER FUNCTIONS
+ **************************************************************************************/
+
 bool BraccioClass::begin(voidFuncPtr custom_menu)
 {
+  static int constexpr RS485_RX_PIN = 1;
+
   SPI.begin();
   Wire.begin();
   Serial.begin(115200);
 
   pinMode(PIN_FUSB302_INT, INPUT_PULLUP);
+  pinMode(RS485_RX_PIN, INPUT_PULLUP);
 
-  static rtos::Thread th(osPriorityHigh);
-  th.start(mbed::callback(this, &BraccioClass::pd_thread));
-  attachInterrupt(PIN_FUSB302_INT, mbed::callback(this, &BraccioClass::unlock_pd_semaphore_irq), FALLING);
-  pd_timer.attach(mbed::callback(this, &BraccioClass::unlock_pd_semaphore), 10ms);
-
-  PD_UFP.init_PPS(PPS_V(7.2), PPS_A(2.0));
-
-/*
-  while (millis() < 200) {
-    PD_UFP.run();
-  }
-*/
-
-  pinMode(1, INPUT_PULLUP);
-
-  _bl.begin();
-  if (_bl.getChipID() != 0xCE)
-    return false;
-  _bl.turnOn();
-
-  if (!expander_init())
-    return false;
-
-  pinMode(BTN_LEFT, INPUT_PULLUP);
-  pinMode(BTN_RIGHT, INPUT_PULLUP);
-  pinMode(BTN_UP, INPUT_PULLUP);
-  pinMode(BTN_DOWN, INPUT_PULLUP);
-  pinMode(BTN_SEL, INPUT_PULLUP);
-  pinMode(BTN_ENTER, INPUT_PULLUP);
-
-#if LV_USE_LOG
-  lv_log_register_print_cb(lvgl_my_print);
-#endif
+  _pd_thd.start(mbed::callback(this, &BraccioClass::pd_thread_func));
+  attachInterrupt(PIN_FUSB302_INT, braccio_unlock_pd_semaphore_irq, FALLING);
+  _pd_timer.attach(braccio_unlock_pd_semaphore, 10ms);
+  _PD_UFP.init_PPS(PPS_V(7.2), PPS_A(2.0));
 
-  lv_init();
+  button_init();
 
-  lv_disp_draw_buf_init(&_lvgl_disp_buf, _lvgl_draw_buf, NULL, LVGL_DRAW_BUFFER_SIZE);
-
-  /*Initialize the display*/
-  lv_disp_drv_init(&_lvgl_disp_drv);
-  _lvgl_disp_drv.hor_res = 240;
-  _lvgl_disp_drv.ver_res = 240;
-  _lvgl_disp_drv.flush_cb = braccio_disp_flush;
-  _lvgl_disp_drv.draw_buf = &_lvgl_disp_buf;
-  lv_disp_drv_register(&_lvgl_disp_drv);
-
-  lv_indev_drv_init(&_lvgl_indev_drv);
-  _lvgl_indev_drv.type = LV_INDEV_TYPE_KEYPAD;
-  _lvgl_indev_drv.read_cb = braccio_read_keypad;
-  _lvgl_kb_indev = lv_indev_drv_register(&_lvgl_indev_drv);
-
-  lv_style_init(&_lv_style);
-
-  _gfx.init();
-  _gfx.setRotation(4);
-  _gfx.fillScreen(TFT_WHITE);
-  _gfx.setAddrWindow(0, 0, 240, 240);
-
-  _lvgl_p_obj_group = lv_group_create();
-  lv_group_set_default(_lvgl_p_obj_group);
+  if (!expander_init()) return false;
 
+  display_init();
+  if (!backlight_init()) return false;
+  lvgl_init();
   _display_thd.start(mbed::callback(this, &BraccioClass::display_thread_func));
 
+
   auto check_power_func = [this]()
   {
-    if (!PD_UFP.is_PPS_ready())
+    if (!_PD_UFP.is_PPS_ready())
     {
       _i2c_mtx.lock();
-      PD_UFP.print_status(Serial);
-      PD_UFP.set_PPS(PPS_V(7.2), PPS_A(2.0));
+      _PD_UFP.print_status(Serial);
+      _PD_UFP.set_PPS(PPS_V(7.2), PPS_A(2.0));
       delay(10);
       _i2c_mtx.unlock();
     }
@@ -129,11 +108,11 @@ bool BraccioClass::begin(voidFuncPtr custom_menu)
   lvgl_splashScreen(2000, check_power_func);
   lv_obj_clean(lv_scr_act());
 
-  if (!PD_UFP.is_PPS_ready())
+  if (!_PD_UFP.is_PPS_ready())
     lvgl_pleaseConnectPower();
 
   /* Loop forever, if no power is attached. */
-  while(!PD_UFP.is_PPS_ready())
+  while(!_PD_UFP.is_PPS_ready())
     check_power_func();
   lv_obj_clean(lv_scr_act());
 
@@ -142,9 +121,9 @@ bool BraccioClass::begin(voidFuncPtr custom_menu)
   else
     lvgl_defaultMenu();
 
-  servos.begin();
-  servos.setTime(SmartServoClass::BROADCAST, SLOW);
-  servos.setPositionMode(PositionMode::IMMEDIATE);
+  _servos.begin();
+  _servos.setTime(SmartServoClass::BROADCAST, SLOW);
+  _servos.setPositionMode(PositionMode::IMMEDIATE);
 
   _motors_connected_thd.start(mbed::callback(this, &BraccioClass::motorConnectedThreadFunc));
 
@@ -171,7 +150,7 @@ bool BraccioClass::connected(int const id)
 
 Servo BraccioClass::move(int const id)
 {
-  Servo wrapper(servos, id);
+  Servo wrapper(_servos, id);
   return wrapper;
 }
 
@@ -182,32 +161,54 @@ Servo BraccioClass::get(int const id)
 
 void BraccioClass::moveTo(float const a1, float const a2, float const a3, float const a4, float const a5, float const a6)
 {
-  servos.setPositionMode(PositionMode::SYNC);
-  servos.setPosition(1, a1);
-  servos.setPosition(2, a2);
-  servos.setPosition(3, a3);
-  servos.setPosition(4, a4);
-  servos.setPosition(5, a5);
-  servos.setPosition(6, a6);
-  servos.synchronize();
-  servos.setPositionMode(PositionMode::IMMEDIATE);
+  _servos.setPositionMode(PositionMode::SYNC);
+  _servos.setPosition(1, a1);
+  _servos.setPosition(2, a2);
+  _servos.setPosition(3, a3);
+  _servos.setPosition(4, a4);
+  _servos.setPosition(5, a5);
+  _servos.setPosition(6, a6);
+  _servos.synchronize();
+  _servos.setPositionMode(PositionMode::IMMEDIATE);
 }
 
 void BraccioClass::positions(float * buffer)
 {
   for (int id = SmartServoClass::MIN_MOTOR_ID; id <= SmartServoClass::MAX_MOTOR_ID; id++)
-    *buffer++ = servos.getPosition(id);
+    *buffer++ = _servos.getPosition(id);
 }
 
 void BraccioClass::positions(float & a1, float & a2, float & a3, float & a4, float & a5, float & a6)
 {
   // TODO: add check if motors are actually connected
-  a1 = servos.getPosition(1);
-  a2 = servos.getPosition(2);
-  a3 = servos.getPosition(3);
-  a4 = servos.getPosition(4);
-  a5 = servos.getPosition(5);
-  a6 = servos.getPosition(6);
+  a1 = _servos.getPosition(1);
+  a2 = _servos.getPosition(2);
+  a3 = _servos.getPosition(3);
+  a4 = _servos.getPosition(4);
+  a5 = _servos.getPosition(5);
+  a6 = _servos.getPosition(6);
+}
+
+int BraccioClass::getKey() {
+  if (isJoystickPressed_LEFT()) {
+    return 1;
+  }
+  if (isJoystickPressed_RIGHT()) {
+    return 2;
+  }
+  if (isJoystickPressed_SELECT()) {
+    return 3;
+  }
+  if (isJoystickPressed_UP()) {
+    return 4;
+  }
+  if (isJoystickPressed_DOWN()) {
+    return 5;
+  }
+  if (isButtonPressed_ENTER()) {
+    return 6;
+  }
+  return 0;
 }
 
 void BraccioClass::connectJoystickTo(lv_obj_t* obj)
@@ -229,84 +230,29 @@ void BraccioClass::lvgl_disp_flush(lv_disp_drv_t *disp, const lv_area_t *area, l
   lv_disp_flush_ready(disp);
 }
 
-void BraccioClass::pd_thread() {
-  start_pd_burst = millis();
-  size_t last_time_ask_pps = 0;
-  while (1) {
-    auto ret = pd_events.wait_any(0xFF);
-    if ((ret & 1) && (millis() - start_pd_burst > 1000)) {
-      pd_timer.detach();
-      pd_timer.attach(mbed::callback(this, &BraccioClass::unlock_pd_semaphore), 5s);
-    }
-    if (ret & 2) {
-      pd_timer.detach();
-      pd_timer.attach(mbed::callback(this, &BraccioClass::unlock_pd_semaphore), 50ms);
-    }
-    _i2c_mtx.lock();
-    if (millis() - last_time_ask_pps > 5000) {
-      PD_UFP.set_PPS(PPS_V(7.2), PPS_A(2.0));
-      last_time_ask_pps = millis();
-    }
-    PD_UFP.run();
-    _i2c_mtx.unlock();
-    if (PD_UFP.is_power_ready() && PD_UFP.is_PPS_ready()) {
-
-    }
-  }
-}
-
-void BraccioClass::display_thread_func()
+void BraccioClass::unlock_pd_semaphore_irq()
 {
-  for(;;)
-  {
-    lv_task_handler();
-    lv_tick_inc(LV_DISP_DEF_REFR_PERIOD);
-    delay(LV_DISP_DEF_REFR_PERIOD);
-  }
+ _start_pd_burst = millis();
+ _pd_events.set(2);
 }
 
-void BraccioClass::lvgl_splashScreen(unsigned long const duration_ms, std::function<void()> check_power_func)
+void BraccioClass::unlock_pd_semaphore()
 {
-  extern const lv_img_dsc_t img_bulb_gif;
-
-  LV_IMG_DECLARE(img_bulb_gif);
-  lv_obj_t* img = lv_gif_create(lv_scr_act());
-  lv_gif_set_src(img, &img_bulb_gif);
-  lv_obj_align(img, LV_ALIGN_CENTER, 0, 0);
-
-  /* Wait until the splash screen duration is over.
-   * Meanwhile use the wait time for checking the
-   * power.
-   */
-  for (unsigned long const start = millis(); millis() - start < duration_ms;)
-  {
-    check_power_func();
-  }
-
-  lv_obj_del(img);
+  _pd_events.set(1);
 }
 
-void BraccioClass::lvgl_pleaseConnectPower()
-{
-  lv_style_set_text_font(&_lv_style, &lv_font_montserrat_32);
-  lv_obj_t * label1 = lv_label_create(lv_scr_act());
-  lv_obj_add_style(label1, &_lv_style, 0);
-  lv_label_set_text(label1, "Please\nconnect\npower.");
-  lv_label_set_long_mode(label1, LV_LABEL_LONG_SCROLL);
-  lv_obj_set_align(label1, LV_ALIGN_CENTER);
-  lv_obj_set_pos(label1, 0, 0);
-}
+/**************************************************************************************
+ * PRIVATE MEMBER FUNCTIONS
+ **************************************************************************************/
 
-void BraccioClass::lvgl_defaultMenu()
+void BraccioClass::button_init()
 {
-  // TODO: create a meaningful default menu
-  lv_style_set_text_font(&_lv_style, &lv_font_montserrat_32);
-  lv_obj_t * label1 = lv_label_create(lv_scr_act());
-  lv_obj_add_style(label1, &_lv_style, 0);
-  lv_label_set_text(label1, "Braccio++");
-  lv_label_set_long_mode(label1, LV_LABEL_LONG_SCROLL);
-  lv_obj_set_align(label1, LV_ALIGN_CENTER);
-  lv_obj_set_pos(label1, 0, 0);
+  pinMode(BTN_LEFT,  INPUT_PULLUP);
+  pinMode(BTN_RIGHT, INPUT_PULLUP);
+  pinMode(BTN_UP,    INPUT_PULLUP);
+  pinMode(BTN_DOWN,  INPUT_PULLUP);
+  pinMode(BTN_SEL,   INPUT_PULLUP);
+  pinMode(BTN_ENTER, INPUT_PULLUP);
 }
 
 bool BraccioClass::expander_init()
@@ -338,12 +284,24 @@ bool BraccioClass::expander_init()
 
   /* Set all motor status LEDs to red. */
   for (int id = SmartServoClass::MIN_MOTOR_ID; id <= SmartServoClass::MAX_MOTOR_ID; id++) {
-    setRed(id);
+    expander_setRed(id);
   }
 
   return true;
 }
 
+void BraccioClass::expander_setGreen(int const i)
+{
+  _expander.writePin(i * 2 - 1, 0);
+  _expander.writePin(i * 2 - 2, 1);
+}
+
+void BraccioClass::expander_setRed(int const i)
+{
+  _expander.writePin(i * 2 - 1, 1);
+  _expander.writePin(i * 2 - 2, 0);
+}
+
 bool BraccioClass::isPingAllowed()
 {
   mbed::ScopedLock<rtos::Mutex> lock(_motors_connected_mtx);
@@ -364,43 +322,161 @@ void BraccioClass::motorConnectedThreadFunc()
     {
       for (int id = SmartServoClass::MIN_MOTOR_ID; id <= SmartServoClass::MAX_MOTOR_ID; id++)
       {
-        bool const is_connected = (servos.ping(id) == 0);
+        bool const is_connected = (_servos.ping(id) == 0);
         setMotorConnectionStatus(id, is_connected);
       }
 
       for (int id = SmartServoClass::MIN_MOTOR_ID; id <= SmartServoClass::MAX_MOTOR_ID; id++) {
         if (connected(id))
-          setGreen(id);
+          expander_setGreen(id);
         else
-          setRed(id);
+          expander_setRed(id);
       }
     }
     delay(1000);
   }
 }
 
-int BraccioClass::getKey() {
-  if (isJoystickPressed_LEFT()) {
-    return 1;
-  }
-  if (isJoystickPressed_RIGHT()) {
-    return 2;
-  }
-  if (isJoystickPressed_SELECT()) {
-    return 3;
-  }
-  if (isJoystickPressed_UP()) {
-    return 4;
+bool BraccioClass::backlight_init()
+{
+  _bl.begin();
+  if (_bl.getChipID() != 0xCE)
+    return false;
+
+  _bl.turnOn();
+  return true;
+}
+
+void BraccioClass::display_init()
+{
+  _gfx.init();
+  _gfx.setRotation(4);
+  _gfx.fillScreen(TFT_WHITE);
+  _gfx.setAddrWindow(0, 0, 240, 240);
+}
+
+void BraccioClass::lvgl_init()
+{
+  lv_init();
+
+#if LV_USE_LOG
+  lv_log_register_print_cb(lvgl_my_print);
+#endif
+
+  lv_disp_draw_buf_init(&_lvgl_disp_buf, _lvgl_draw_buf, NULL, LVGL_DRAW_BUFFER_SIZE);
+
+  lv_disp_drv_init(&_lvgl_disp_drv);
+  _lvgl_disp_drv.hor_res = 240;
+  _lvgl_disp_drv.ver_res = 240;
+  _lvgl_disp_drv.flush_cb = braccio_disp_flush;
+  _lvgl_disp_drv.draw_buf = &_lvgl_disp_buf;
+  lv_disp_drv_register(&_lvgl_disp_drv);
+
+  lv_indev_drv_init(&_lvgl_indev_drv);
+  _lvgl_indev_drv.type = LV_INDEV_TYPE_KEYPAD;
+  _lvgl_indev_drv.read_cb = braccio_read_keypad;
+  _lvgl_kb_indev = lv_indev_drv_register(&_lvgl_indev_drv);
+
+  lv_style_init(&_lv_style);
+
+  _lvgl_p_obj_group = lv_group_create();
+  lv_group_set_default(_lvgl_p_obj_group);
+}
+
+void BraccioClass::display_thread_func()
+{
+  for(;;)
+  {
+    lv_task_handler();
+    lv_tick_inc(LV_DISP_DEF_REFR_PERIOD);
+    delay(LV_DISP_DEF_REFR_PERIOD);
   }
-  if (isJoystickPressed_DOWN()) {
-    return 5;
+}
+
+void BraccioClass::lvgl_splashScreen(unsigned long const duration_ms, std::function<void()> check_power_func)
+{
+  extern const lv_img_dsc_t img_bulb_gif;
+
+  LV_IMG_DECLARE(img_bulb_gif);
+  lv_obj_t* img = lv_gif_create(lv_scr_act());
+  lv_gif_set_src(img, &img_bulb_gif);
+  lv_obj_align(img, LV_ALIGN_CENTER, 0, 0);
+
+  /* Wait until the splash screen duration is over.
+   * Meanwhile use the wait time for checking the
+   * power.
+   */
+  for (unsigned long const start = millis(); millis() - start < duration_ms;)
+  {
+    check_power_func();
   }
-  if (isButtonPressed_ENTER()) {
-    return 6;
+
+  lv_obj_del(img);
+}
+
+void BraccioClass::lvgl_pleaseConnectPower()
+{
+  lv_style_set_text_font(&_lv_style, &lv_font_montserrat_32);
+  lv_obj_t * label1 = lv_label_create(lv_scr_act());
+  lv_obj_add_style(label1, &_lv_style, 0);
+  lv_label_set_text(label1, "Please\nconnect\npower.");
+  lv_label_set_long_mode(label1, LV_LABEL_LONG_SCROLL);
+  lv_obj_set_align(label1, LV_ALIGN_CENTER);
+  lv_obj_set_pos(label1, 0, 0);
+}
+
+void BraccioClass::lvgl_defaultMenu()
+{
+  // TODO: create a meaningful default menu
+  lv_style_set_text_font(&_lv_style, &lv_font_montserrat_32);
+  lv_obj_t * label1 = lv_label_create(lv_scr_act());
+  lv_obj_add_style(label1, &_lv_style, 0);
+  lv_label_set_text(label1, "Braccio++");
+  lv_label_set_long_mode(label1, LV_LABEL_LONG_SCROLL);
+  lv_obj_set_align(label1, LV_ALIGN_CENTER);
+  lv_obj_set_pos(label1, 0, 0);
+}
+
+void BraccioClass::pd_thread_func()
+{
+  _start_pd_burst = millis();
+  size_t last_time_ask_pps = 0;
+
+  for(;;)
+  {
+    auto ret = _pd_events.wait_any(0xFF);
+    if ((ret & 1) && (millis() - _start_pd_burst > 1000)) {
+      _pd_timer.detach();
+      _pd_timer.attach(braccio_unlock_pd_semaphore, 5s);
+    }
+    if (ret & 2) {
+      _pd_timer.detach();
+      _pd_timer.attach(braccio_unlock_pd_semaphore, 50ms);
+    }
+    _i2c_mtx.lock();
+    if (millis() - last_time_ask_pps > 5000) {
+      _PD_UFP.set_PPS(PPS_V(7.2), PPS_A(2.0));
+      last_time_ask_pps = millis();
+    }
+    _PD_UFP.run();
+    _i2c_mtx.unlock();
+    if (_PD_UFP.is_power_ready() && _PD_UFP.is_PPS_ready()) {
+
+    }
   }
-  return 0;
 }
 
+/**************************************************************************************
+ * FUNCTION DEFINITION
+ **************************************************************************************/
+
+#if LV_USE_LOG
+void lvgl_my_print(const char * dsc)
+{
+  Serial.println(dsc);
+}
+#endif /* #if LV_USE_LOG */
+
 /* Display flushing */
 extern "C" void braccio_disp_flush(lv_disp_drv_t *disp, const lv_area_t *area, lv_color_t *color_p)
 {
@@ -446,3 +522,13 @@ extern "C" void braccio_read_keypad(lv_indev_drv_t * drv, lv_indev_data_t* data)
 
     data->key = last_key;
 }
+
+void braccio_unlock_pd_semaphore_irq()
+{
+  Braccio.unlock_pd_semaphore_irq();
+}
+
+void braccio_unlock_pd_semaphore()
+{
+  Braccio.unlock_pd_semaphore();
+}
diff --git a/src/Braccio++.h b/src/Braccio++.h
index 5c66713..068bf67 100644
--- a/src/Braccio++.h
+++ b/src/Braccio++.h
@@ -1,6 +1,10 @@
 #ifndef __BRACCIO_PLUSPLUS_H__
 #define __BRACCIO_PLUSPLUS_H__
 
+/**************************************************************************************
+ * INCLUDE
+ **************************************************************************************/
+
 #include "Arduino.h"
 #include "lib/powerdelivery/PD_UFP.h"
 #include "lib/ioexpander/TCA6424A.h"
@@ -11,17 +15,29 @@
 #include "lib/TFT_eSPI/TFT_eSPI.h" // Hardware-specific library
 #include <lvgl.h>
 
+#include <chrono>
+using namespace std::chrono;
+
+/**************************************************************************************
+ * TYPEDEF
+ **************************************************************************************/
+
 enum speed_grade_t {
   FAST = 10,
   MEDIUM = 100,
   SLOW = 1000,
 };
 
-#include <chrono>
-using namespace std::chrono;
+/**************************************************************************************
+ * FORWARD DECLARATION
+ **************************************************************************************/
 
 class Servo;
 
+/**************************************************************************************
+ * CLASS DECLARATION
+ **************************************************************************************/
+
 class BraccioClass
 {
 public:
@@ -44,11 +60,11 @@ class BraccioClass
   void positions(float * buffer);
   void positions(float & a1, float & a2, float & a3, float & a4, float & a5, float & a6);
 
-  inline void speed(speed_grade_t const speed_grade) { servos.setTime(SmartServoClass::BROADCAST, speed_grade); }
-  inline void speed(int const id, speed_grade_t const speed_grade) { servos.setTime(id, speed_grade); }
+  inline void speed(speed_grade_t const speed_grade) { _servos.setTime(SmartServoClass::BROADCAST, speed_grade); }
+  inline void speed(int const id, speed_grade_t const speed_grade) { _servos.setTime(id, speed_grade); }
 
-  inline void disengage(int const id = SmartServoClass::BROADCAST) { servos.disengage(id); }
-  inline void engage   (int const id = SmartServoClass::BROADCAST) { servos.engage(id); }
+  inline void disengage(int const id = SmartServoClass::BROADCAST) { _servos.disengage(id); }
+  inline void engage   (int const id = SmartServoClass::BROADCAST) { _servos.engage(id); }
 
   int getKey();
   void connectJoystickTo(lv_obj_t* obj);
@@ -65,20 +81,27 @@ class BraccioClass
     return dev;
   }
 
+  /* Those functions MUST NOT be used by the user. */
   void lvgl_disp_flush(lv_disp_drv_t *disp, const lv_area_t *area, lv_color_t *color_p);
+  void unlock_pd_semaphore_irq();
+  void unlock_pd_semaphore();
 
 protected:
 
-  inline void setID(int const id) { servos.setID(id); }
+  inline void setID(int const id) { _servos.setID(id); }
 
 private:
 
+  void button_init();
+
   rtos::Mutex _i2c_mtx;
-  RS485Class serial485;
-  SmartServoClass servos;
-  PD_UFP_log_c PD_UFP;
+  RS485Class _serial485;
+  SmartServoClass _servos;
+  PD_UFP_log_c _PD_UFP;
   TCA6424A _expander;
   bool expander_init();
+  void expander_setGreen(int const i);
+  void expander_setRed(int const i);
 
   bool _is_ping_allowed;
   bool _is_motor_connected[SmartServoClass::NUM_MOTORS];
@@ -109,37 +132,20 @@ class BraccioClass
   lv_indev_t * _lvgl_kb_indev;
   lv_style_t _lv_style;
   rtos::Thread _display_thd;
+  bool backlight_init();
+  void display_init();
+  void lvgl_init();
   void display_thread_func();
   void lvgl_splashScreen(unsigned long const duration_ms, std::function<void()> check_power_func);
   void lvgl_pleaseConnectPower();
   void lvgl_defaultMenu();
 
 
-  rtos::EventFlags pd_events;
-  mbed::Ticker pd_timer;
-
-  unsigned int start_pd_burst = 0xFFFFFFFF;
-
-  void unlock_pd_semaphore_irq() {
-    start_pd_burst = millis();
-    pd_events.set(2);
-  }
-
-  void unlock_pd_semaphore() {
-    pd_events.set(1);
-  }
-
-  void setGreen(int i) {
-    _expander.writePin(i * 2 - 1, 0);
-    _expander.writePin(i * 2 - 2, 1);
-  }
-
-  void setRed(int i) {
-    _expander.writePin(i * 2 - 1, 1);
-    _expander.writePin(i * 2 - 2, 0);
-  }
-
-  void pd_thread();
+  rtos::EventFlags _pd_events;
+  mbed::Ticker _pd_timer;
+  unsigned int _start_pd_burst;
+  rtos::Thread _pd_thd;
+  void pd_thread_func();
 };
 
 #define Braccio BraccioClass::get_default_instance()
@@ -171,18 +177,4 @@ class Servo
   int const _id;
 };
 
-struct __callback__container__ {
-  mbed::Callback<void()> fn;
-};
-
-inline void attachInterrupt(pin_size_t interruptNum, mbed::Callback<void()> func, PinStatus mode) {
-  struct __callback__container__* a = new __callback__container__();
-  a->fn = func;
-  auto callback = [](void* a) -> void {
-    ((__callback__container__*)a)->fn();
-  };
-
-  attachInterruptParam(interruptNum, callback, mode, (void*)a);
-}
-
 #endif //__BRACCIO_PLUSPLUS_H__