diff --git a/BTD.cpp b/BTD.cpp
index de83da95..0bc8591f 100755
--- a/BTD.cpp
+++ b/BTD.cpp
@@ -511,6 +511,7 @@ void BTD::HCI_event_task() {
                                                 if(remote_name[i] == '\0') // End of string
                                                         break;
                                         }
+                                        // TODO: Altid sæt '\0' i remote name!
                                         hci_set_flag(HCI_FLAG_REMOTE_NAME_COMPLETE);
                                 }
                                 break;
diff --git a/BTD.h b/BTD.h
index c62ad5eb..37191827 100755
--- a/BTD.h
+++ b/BTD.h
@@ -19,6 +19,7 @@
 #define _btd_h_
 
 #include "Usb.h"
+#include "Hid.h"
 
 //PID and VID of the Sony PS3 devices
 #define PS3_VID                 0x054C  // Sony Corporation
@@ -34,9 +35,6 @@
 
 // Used in control endpoint header for HCI Commands
 #define bmREQ_HCI_OUT USB_SETUP_HOST_TO_DEVICE|USB_SETUP_TYPE_CLASS|USB_SETUP_RECIPIENT_DEVICE
-// Used in control endpoint header for HID Commands
-#define bmREQ_HID_OUT USB_SETUP_HOST_TO_DEVICE|USB_SETUP_TYPE_CLASS|USB_SETUP_RECIPIENT_INTERFACE
-#define HID_REQUEST_SET_REPORT      0x09
 
 /* Bluetooth HCI states for hci_task() */
 #define HCI_INIT_STATE                  0
@@ -60,15 +58,15 @@
 #define HCI_DISCONNECT_STATE            16
 
 /* HCI event flags*/
-#define HCI_FLAG_CMD_COMPLETE           0x01
-#define HCI_FLAG_CONNECT_COMPLETE       0x02
-#define HCI_FLAG_DISCONNECT_COMPLETE    0x04
-#define HCI_FLAG_REMOTE_NAME_COMPLETE   0x08
-#define HCI_FLAG_INCOMING_REQUEST       0x10
-#define HCI_FLAG_READ_BDADDR            0x20
-#define HCI_FLAG_READ_VERSION           0x40
-#define HCI_FLAG_DEVICE_FOUND           0x80
-#define HCI_FLAG_CONNECT_EVENT          0x100
+#define HCI_FLAG_CMD_COMPLETE           (1UL << 0)
+#define HCI_FLAG_CONNECT_COMPLETE       (1UL << 1)
+#define HCI_FLAG_DISCONNECT_COMPLETE    (1UL << 2)
+#define HCI_FLAG_REMOTE_NAME_COMPLETE   (1UL << 3)
+#define HCI_FLAG_INCOMING_REQUEST       (1UL << 4)
+#define HCI_FLAG_READ_BDADDR            (1UL << 5)
+#define HCI_FLAG_READ_VERSION           (1UL << 6)
+#define HCI_FLAG_DEVICE_FOUND           (1UL << 7)
+#define HCI_FLAG_CONNECT_EVENT          (1UL << 8)
 
 /* Macros for HCI event flag tests */
 #define hci_check_flag(flag) (hci_event_flag & (flag))
@@ -133,28 +131,28 @@
 #define WII_INIT_MOTION_PLUS_STATE      21
 
 /* L2CAP event flags for HID Control channel */
-#define L2CAP_FLAG_CONNECTION_CONTROL_REQUEST           0x00000001
-#define L2CAP_FLAG_CONFIG_CONTROL_SUCCESS               0x00000002
-#define L2CAP_FLAG_CONTROL_CONNECTED                    0x00000004
-#define L2CAP_FLAG_DISCONNECT_CONTROL_RESPONSE          0x00000008
+#define L2CAP_FLAG_CONNECTION_CONTROL_REQUEST           (1UL << 0)
+#define L2CAP_FLAG_CONFIG_CONTROL_SUCCESS               (1UL << 1)
+#define L2CAP_FLAG_CONTROL_CONNECTED                    (1UL << 2)
+#define L2CAP_FLAG_DISCONNECT_CONTROL_RESPONSE          (1UL << 3)
 
 /* L2CAP event flags for HID Interrupt channel */
-#define L2CAP_FLAG_CONNECTION_INTERRUPT_REQUEST         0x00000010
-#define L2CAP_FLAG_CONFIG_INTERRUPT_SUCCESS             0x00000020
-#define L2CAP_FLAG_INTERRUPT_CONNECTED                  0x00000040
-#define L2CAP_FLAG_DISCONNECT_INTERRUPT_RESPONSE        0x00000080
+#define L2CAP_FLAG_CONNECTION_INTERRUPT_REQUEST         (1UL << 4)
+#define L2CAP_FLAG_CONFIG_INTERRUPT_SUCCESS             (1UL << 5)
+#define L2CAP_FLAG_INTERRUPT_CONNECTED                  (1UL << 6)
+#define L2CAP_FLAG_DISCONNECT_INTERRUPT_RESPONSE        (1UL << 7)
 
 /* L2CAP event flags for SDP channel */
-#define L2CAP_FLAG_CONNECTION_SDP_REQUEST               0x00000100
-#define L2CAP_FLAG_CONFIG_SDP_SUCCESS                   0x00000200
-#define L2CAP_FLAG_DISCONNECT_SDP_REQUEST               0x00000400
+#define L2CAP_FLAG_CONNECTION_SDP_REQUEST               (1UL << 8)
+#define L2CAP_FLAG_CONFIG_SDP_SUCCESS                   (1UL << 9)
+#define L2CAP_FLAG_DISCONNECT_SDP_REQUEST               (1UL << 10)
 
 /* L2CAP event flags for RFCOMM channel */
-#define L2CAP_FLAG_CONNECTION_RFCOMM_REQUEST            0x00000800
-#define L2CAP_FLAG_CONFIG_RFCOMM_SUCCESS                0x00001000
-#define L2CAP_FLAG_DISCONNECT_RFCOMM_REQUEST            0x00002000
+#define L2CAP_FLAG_CONNECTION_RFCOMM_REQUEST            (1UL << 11)
+#define L2CAP_FLAG_CONFIG_RFCOMM_SUCCESS                (1UL << 12)
+#define L2CAP_FLAG_DISCONNECT_RFCOMM_REQUEST            (1UL << 13)
 
-#define L2CAP_FLAG_DISCONNECT_RESPONSE                  0x00004000
+#define L2CAP_FLAG_DISCONNECT_RESPONSE                  (1UL << 14)
 
 /* Macros for L2CAP event flag tests */
 #define l2cap_check_flag(flag) (l2cap_event_flag & (flag))
@@ -207,9 +205,12 @@
 #define UHS_ACL_HANDLE_OK(x, y) ((x[0] == (y & 0xff)) && (x[1] == ((y >> 8) | 0x20)))
 #endif
 
+class BTD;
+
 /** All Bluetooth services should inherit this class. */
 class BluetoothService {
 public:
+        BluetoothService(BTD *p) : pBtd(p) {};
         /**
          * Used to pass acldata to the Bluetooth service.
          * @param ACLData Pointer to the incoming acldata.
@@ -221,6 +222,39 @@ public:
         virtual void Reset() = 0;
         /** Used to disconnect both the L2CAP Channel and the HCI Connection for the Bluetooth service. */
         virtual void disconnect() = 0;
+
+        /**
+         * Used to call your own function when the device is successfully initialized.
+         * @param funcOnInit Function to call.
+         */
+        void attachOnInit(void (*funcOnInit)(void)) {
+                pFuncOnInit = funcOnInit; // TODO: This really belong in a class of it's own as it is repeated several times
+        };
+
+protected:
+        /**
+         * Called when a device is successfully initialized.
+         * Use attachOnInit(void (*funcOnInit)(void)) to call your own function.
+         * This is useful for instance if you want to set the LEDs in a specific way.
+         */
+        virtual void onInit() = 0;
+
+        // TODO: Implement "UHS_ACL_HANDLE_OK" function
+
+        /** Pointer to function called in onInit(). */
+        void (*pFuncOnInit)(void);
+
+        /** Pointer to BTD instance. */
+        BTD *pBtd;
+
+        /** The HCI Handle for the connection. */
+        uint16_t hci_handle;
+
+        /** L2CAP flags of received Bluetooth events. */
+        uint32_t l2cap_event_flag;
+
+        /** Identifier for L2CAP commands. */
+        uint8_t identifier;
 };
 
 /**
diff --git a/BTHID.cpp b/BTHID.cpp
index 01f190a7..3a625b2d 100644
--- a/BTHID.cpp
+++ b/BTHID.cpp
@@ -21,7 +21,7 @@
 //#define PRINTREPORT // Uncomment to print the report send by the HID device
 
 BTHID::BTHID(BTD *p, bool pair, const char *pin) :
-pBtd(p), // pointer to USB class instance - mandatory
+BluetoothService(p), // Pointer to USB class instance - mandatory
 protocolMode(HID_BOOT_PROTOCOL) {
         for(uint8_t i = 0; i < NUM_PARSERS; i++)
                 pRptParser[i] = NULL;
diff --git a/BTHID.h b/BTHID.h
index 2419e58b..799c59a8 100644
--- a/BTHID.h
+++ b/BTHID.h
@@ -37,15 +37,6 @@ public:
         BTHID(BTD *p, bool pair = false, const char *pin = "0000");
 
         /** @name BluetoothService implementation */
-        /**
-         * Used to pass acldata to the services.
-         * @param ACLData Incoming acldata.
-         */
-        virtual void ACLData(uint8_t* ACLData);
-        /** Used to run part of the state machine. */
-        virtual void Run();
-        /** Use this to reset the service. */
-        virtual void Reset();
         /** Used this to disconnect the devices. */
         virtual void disconnect();
         /**@}*/
@@ -97,15 +88,29 @@ public:
                         pBtd->pairWithHID();
         };
 
-        /**
-         * Used to call your own function when the device is successfully initialized.
-         * @param funcOnInit Function to call.
-         */
-        void attachOnInit(void (*funcOnInit)(void)) {
-                pFuncOnInit = funcOnInit;
-        };
-
 protected:
+        /** @name BluetoothService implementation */
+        /**
+         * Used to pass acldata to the services.
+         * @param ACLData Incoming acldata.
+         */
+        virtual void ACLData(uint8_t* ACLData);
+        /** Used to run part of the state machine. */
+        virtual void Run();
+        /** Use this to reset the service. */
+        virtual void Reset();
+        /**
+         * Called when a device is successfully initialized.
+         * Use attachOnInit(void (*funcOnInit)(void)) to call your own function.
+         * This is useful for instance if you want to set the LEDs in a specific way.
+         */
+        virtual void onInit() {
+                if(pFuncOnInit)
+                        pFuncOnInit(); // Call the user function
+                OnInitBTHID();
+        };
+        /**@}*/
+
         /** @name Overridable functions */
         /**
          * Used to parse Bluetooth HID data to any class that inherits this class.
@@ -125,14 +130,7 @@ protected:
         }
         /**@}*/
 
-        /** Pointer to BTD instance */
-        BTD *pBtd;
-
-        /** HCI Handle for connection */
-        uint16_t hci_handle;
-
         /** L2CAP source CID for HID_Control */
-
         uint8_t control_scid[2];
 
         /** L2CAP source CID for HID_Interrupt */
@@ -145,18 +143,6 @@ private:
         void setProtocol();
         uint8_t protocolMode;
 
-        /**
-         * Called when a device is successfully initialized.
-         * Use attachOnInit(void (*funcOnInit)(void)) to call your own function.
-         * This is useful for instance if you want to set the LEDs in a specific way.
-         */
-        void onInit() {
-                if(pFuncOnInit)
-                        pFuncOnInit(); // Call the user function
-                OnInitBTHID();
-        };
-        void (*pFuncOnInit)(void); // Pointer to function called in onInit()
-
         void L2CAP_task(); // L2CAP state machine
 
         bool activeConnection; // Used to indicate if it already has established a connection
@@ -164,8 +150,6 @@ private:
         /* Variables used for L2CAP communication */
         uint8_t control_dcid[2]; // L2CAP device CID for HID_Control - Always 0x0070
         uint8_t interrupt_dcid[2]; // L2CAP device CID for HID_Interrupt - Always 0x0071
-        uint8_t identifier; // Identifier for connection
         uint8_t l2cap_state;
-        uint32_t l2cap_event_flag; // l2cap flags of received Bluetooth events
 };
 #endif
diff --git a/PS3BT.cpp b/PS3BT.cpp
index 38d6c3dc..74928ca7 100644
--- a/PS3BT.cpp
+++ b/PS3BT.cpp
@@ -21,7 +21,7 @@
 //#define PRINTREPORT // Uncomment to print the report send by the PS3 Controllers
 
 PS3BT::PS3BT(BTD *p, uint8_t btadr5, uint8_t btadr4, uint8_t btadr3, uint8_t btadr2, uint8_t btadr1, uint8_t btadr0) :
-pBtd(p) // pointer to USB class instance - mandatory
+BluetoothService(p) // Pointer to USB class instance - mandatory
 {
         if(pBtd)
                 pBtd->registerServiceClass(this); // Register it as a Bluetooth service
diff --git a/PS3BT.h b/PS3BT.h
index d07a4577..17b7b0c9 100644
--- a/PS3BT.h
+++ b/PS3BT.h
@@ -41,15 +41,6 @@ public:
         PS3BT(BTD *pBtd, uint8_t btadr5 = 0, uint8_t btadr4 = 0, uint8_t btadr3 = 0, uint8_t btadr2 = 0, uint8_t btadr1 = 0, uint8_t btadr0 = 0);
 
         /** @name BluetoothService implementation */
-        /**
-         * Used to pass acldata to the services.
-         * @param ACLData Incoming acldata.
-         */
-        virtual void ACLData(uint8_t* ACLData);
-        /** Used to run part of the state machine. */
-        virtual void Run();
-        /** Use this to reset the service. */
-        virtual void Reset();
         /** Used this to disconnect any of the controllers. */
         virtual void disconnect();
         /**@}*/
@@ -183,14 +174,6 @@ public:
         uint32_t getLastMessageTime() {
                 return lastMessageTime;
         };
-
-        /**
-         * Used to call your own function when the controller is successfully initialized.
-         * @param funcOnInit Function to call.
-         */
-        void attachOnInit(void (*funcOnInit)(void)) {
-                pFuncOnInit = funcOnInit;
-        };
         /**@}*/
 
         /** Variable used to indicate if the normal Playstation controller is successfully connected. */
@@ -200,32 +183,39 @@ public:
         /** Variable used to indicate if the Navigation controller is successfully connected. */
         bool PS3NavigationConnected;
 
-private:
-        /* Mandatory members */
-        BTD *pBtd;
-
+protected:
+        /** @name BluetoothService implementation */
+        /**
+         * Used to pass acldata to the services.
+         * @param ACLData Incoming acldata.
+         */
+        virtual void ACLData(uint8_t* ACLData);
+        /** Used to run part of the state machine. */
+        virtual void Run();
+        /** Use this to reset the service. */
+        virtual void Reset();
         /**
          * Called when the controller is successfully initialized.
          * Use attachOnInit(void (*funcOnInit)(void)) to call your own function.
          * This is useful for instance if you want to set the LEDs in a specific way.
          */
-        void onInit();
-        void (*pFuncOnInit)(void); // Pointer to function called in onInit()
+        virtual void onInit();
+        /**@}*/
+
+private:
 
         void L2CAP_task(); // L2CAP state machine
 
         /* Variables filled from HCI event management */
-        int16_t hci_handle;
         uint8_t remote_name[30]; // First 30 chars of remote name
         bool activeConnection; // Used to indicate if it's already has established a connection
 
         /* Variables used by high level L2CAP task */
         uint8_t l2cap_state;
-        uint32_t l2cap_event_flag; // L2CAP flags of received Bluetooth events
 
         uint32_t lastMessageTime; // Variable used to store the millis value of the last message.
 
-        unsigned long timer;
+        uint32_t timer;
 
         uint32_t ButtonState;
         uint32_t OldButtonState;
@@ -243,7 +233,6 @@ private:
         uint8_t control_dcid[2]; // 0x0040
         uint8_t interrupt_scid[2]; // L2CAP source CID for HID_Interrupt
         uint8_t interrupt_dcid[2]; // 0x0041
-        uint8_t identifier; // Identifier for connection
 
         /* HID Commands */
         void HID_Command(uint8_t* data, uint8_t nbytes);
diff --git a/PS3USB.h b/PS3USB.h
index ca343a81..d2d7d14e 100644
--- a/PS3USB.h
+++ b/PS3USB.h
@@ -19,6 +19,7 @@
 #define _ps3usb_h_
 
 #include "Usb.h"
+#include "Hid.h"
 #include "PS3Enums.h"
 
 /* PS3 data taken from descriptors */
@@ -35,13 +36,6 @@
 #define PS3NAVIGATION_PID       0x042F  // Navigation controller
 #define PS3MOVE_PID             0x03D5  // Motion controller
 
-// Used in control endpoint header for HID Commands
-#define bmREQ_HID_OUT           USB_SETUP_HOST_TO_DEVICE|USB_SETUP_TYPE_CLASS|USB_SETUP_RECIPIENT_INTERFACE
-#define bmREQ_HID_IN            USB_SETUP_DEVICE_TO_HOST|USB_SETUP_TYPE_CLASS|USB_SETUP_RECIPIENT_INTERFACE
-
-#define HID_REQUEST_GET_REPORT  0x01
-#define HID_REQUEST_SET_REPORT  0x09
-
 #define PS3_MAX_ENDPOINTS       3
 
 /**
diff --git a/PS4BT.h b/PS4BT.h
index ff3168a3..6be6463e 100644
--- a/PS4BT.h
+++ b/PS4BT.h
@@ -46,14 +46,6 @@ public:
                 return BTHID::connected;
         };
 
-        /**
-         * Used to call your own function when the device is successfully initialized.
-         * @param funcOnInit Function to call.
-         */
-        void attachOnInit(void (*funcOnInit)(void)) {
-                pFuncOnInit = funcOnInit;
-        };
-
 protected:
         /** @name BTHID implementation */
         /**
@@ -125,7 +117,5 @@ private:
         void HID_Command(uint8_t *data, uint8_t nbytes) {
                 pBtd->L2CAP_Command(hci_handle, data, nbytes, control_scid[0], control_scid[1]);
         };
-
-        void (*pFuncOnInit)(void); // Pointer to function called in onInit()
 };
 #endif
\ No newline at end of file
diff --git a/PSBuzz.cpp b/PSBuzz.cpp
index 56ae618d..4efd2b55 100644
--- a/PSBuzz.cpp
+++ b/PSBuzz.cpp
@@ -78,5 +78,5 @@ void PSBuzz::setLedRaw(bool value, uint8_t controller) {
 
 void PSBuzz::PSBuzz_Command(uint8_t *data, uint16_t nbytes) {
         // bmRequest = Host to device (0x00) | Class (0x20) | Interface (0x01) = 0x21, bRequest = Set Report (0x09), Report ID (0x00), Report Type (Output 0x02), interface (0x00), datalength, datalength, data)
-        pUsb->ctrlReq(bAddress, epInfo[0].epAddr, bmREQ_HIDOUT, HID_REQUEST_SET_REPORT, 0x00, 0x02, 0x00, nbytes, nbytes, data, NULL);
+        pUsb->ctrlReq(bAddress, epInfo[0].epAddr, bmREQ_HID_OUT, HID_REQUEST_SET_REPORT, 0x00, 0x02, 0x00, nbytes, nbytes, data, NULL);
 };
\ No newline at end of file
diff --git a/SPP.cpp b/SPP.cpp
index 562a68ca..3262a3e3 100644
--- a/SPP.cpp
+++ b/SPP.cpp
@@ -43,7 +43,7 @@ const uint8_t rfcomm_crc_table[256] PROGMEM = {/* reversed, 8-bit, poly=0x07 */
 };
 
 SPP::SPP(BTD *p, const char* name, const char* pin) :
-pBtd(p) // Pointer to BTD class instance - mandatory
+BluetoothService(p) // Pointer to BTD class instance - mandatory
 {
         if(pBtd)
                 pBtd->registerServiceClass(this); // Register it as a Bluetooth service
@@ -69,6 +69,7 @@ void SPP::Reset() {
         l2cap_rfcomm_state = L2CAP_RFCOMM_WAIT;
         l2cap_event_flag = 0;
         sppIndex = 0;
+        creditSent = false;
 }
 
 void SPP::disconnect() {
@@ -397,10 +398,7 @@ void SPP::ACLData(uint8_t* l2capinbuf) {
 #ifdef DEBUG_USB_HOST
                                         Notify(PSTR("\r\nRFCOMM Connection is now established\r\n"), 0x80);
 #endif
-                                        waitForLastCommand = false;
-                                        creditSent = false;
-                                        connected = true; // The RFCOMM channel is now established
-                                        sppIndex = 0;
+                                        onInit();
                                 }
 #ifdef EXTRADEBUG
                                 else if(rfcommChannelType != RFCOMM_DISC) {
@@ -430,14 +428,20 @@ void SPP::Run() {
 #ifdef DEBUG_USB_HOST
                 Notify(PSTR("\r\nRFCOMM Connection is now established - Automatic\r\n"), 0x80);
 #endif
-                creditSent = false;
-                waitForLastCommand = false;
-                connected = true; // The RFCOMM channel is now established
-                sppIndex = 0;
+                onInit();
         }
         send(); // Send all bytes currently in the buffer
 }
 
+void SPP::onInit() {
+        creditSent = false;
+        waitForLastCommand = false;
+        connected = true; // The RFCOMM channel is now established
+        sppIndex = 0;
+        if(pFuncOnInit)
+                pFuncOnInit(); // Call the user function
+};
+
 void SPP::SDP_task() {
         switch(l2cap_sdp_state) {
                 case L2CAP_SDP_WAIT:
diff --git a/SPP.h b/SPP.h
index d9f6761c..b20e43e5 100644
--- a/SPP.h
+++ b/SPP.h
@@ -68,6 +68,11 @@ public:
          */
         SPP(BTD *p, const char *name = "Arduino", const char *pin = "0000");
 
+        /** @name BluetoothService implementation */
+        /** Used this to disconnect the virtual serial port. */
+        virtual void disconnect();
+        /**@}*/
+
         /**
          * Used to provide Boolean tests for the class.
          * @return Return true if SPP communication is connected.
@@ -78,20 +83,6 @@ public:
         /** Variable used to indicate if the connection is established. */
         bool connected;
 
-        /** @name BluetoothService implementation */
-        /**
-         * Used to pass acldata to the services.
-         * @param ACLData Incoming acldata.
-         */
-        virtual void ACLData(uint8_t* ACLData);
-        /** Used to establish the connection automatically. */
-        virtual void Run();
-        /** Use this to reset the service. */
-        virtual void Reset();
-        /** Used this to disconnect the virtual serial port. */
-        virtual void disconnect();
-        /**@}*/
-
         /** @name Serial port profile (SPP) Print functions */
         /**
          * Get number of bytes waiting to be read.
@@ -154,20 +145,33 @@ public:
         void send(void);
         /**@}*/
 
-private:
-        /* Bluetooth dongle library pointer */
-        BTD *pBtd;
+protected:
+        /** @name BluetoothService implementation */
+        /**
+         * Used to pass acldata to the services.
+         * @param ACLData Incoming acldata.
+         */
+        virtual void ACLData(uint8_t* ACLData);
+        /** Used to establish the connection automatically. */
+        virtual void Run();
+        /** Use this to reset the service. */
+        virtual void Reset();
+        /**
+         * Called when a device is successfully initialized.
+         * Use attachOnInit(void (*funcOnInit)(void)) to call your own function.
+         * This is useful for instance if you want to set the LEDs in a specific way.
+         */
+        virtual void onInit();
+        /**@}*/
 
+private:
         /* Set true when a channel is created */
         bool SDPConnected;
         bool RFCOMMConnected;
 
-        uint16_t hci_handle; // The HCI Handle for the connection
-
         /* Variables used by L2CAP state machines */
         uint8_t l2cap_sdp_state;
         uint8_t l2cap_rfcomm_state;
-        uint32_t l2cap_event_flag; // l2cap flags of received Bluetooth events
 
         uint8_t l2capoutbuf[BULK_MAXPKTSIZE]; // General purpose buffer for l2cap out data
         uint8_t rfcommbuf[10]; // Buffer for RFCOMM Commands
@@ -177,7 +181,6 @@ private:
         uint8_t sdp_dcid[2]; // 0x0050
         uint8_t rfcomm_scid[2]; // L2CAP source CID for RFCOMM
         uint8_t rfcomm_dcid[2]; // 0x0051
-        uint8_t identifier; // Identifier for command
 
         /* RFCOMM Variables */
         uint8_t rfcommChannel;
@@ -187,7 +190,7 @@ private:
         uint8_t rfcommChannelType;
         uint8_t rfcommPfBit;
 
-        unsigned long timer;
+        uint32_t timer;
         bool waitForLastCommand;
         bool creditSent;
 
diff --git a/Wii.cpp b/Wii.cpp
index 8106666b..25a760f7 100755
--- a/Wii.cpp
+++ b/Wii.cpp
@@ -83,7 +83,7 @@ const uint32_t WII_PROCONTROLLER_BUTTONS[] PROGMEM = {
 };
 
 WII::WII(BTD *p, bool pair) :
-pBtd(p) // pointer to USB class instance - mandatory
+BluetoothService(p) // Pointer to USB class instance - mandatory
 {
         if(pBtd)
                 pBtd->registerServiceClass(this); // Register it as a Bluetooth service
diff --git a/Wii.h b/Wii.h
index 848dec93..ed500dec 100755
--- a/Wii.h
+++ b/Wii.h
@@ -55,15 +55,6 @@ public:
         WII(BTD *p, bool pair = false);
 
         /** @name BluetoothService implementation */
-        /**
-         * Used to pass acldata to the services.
-         * @param ACLData Incoming acldata.
-         */
-        virtual void ACLData(uint8_t* ACLData);
-        /** Used to run part of the state machine. */
-        virtual void Run();
-        /** Use this to reset the service. */
-        virtual void Reset();
         /** Used this to disconnect any of the controllers. */
         virtual void disconnect();
         /**@}*/
@@ -189,14 +180,6 @@ public:
         uint8_t getWiiState() {
                 return wiiState;
         };
-
-        /**
-         * Used to call your own function when the controller is successfully initialized.
-         * @param funcOnInit Function to call.
-         */
-        void attachOnInit(void (*funcOnInit)(void)) {
-                pFuncOnInit = funcOnInit;
-        };
         /**@}*/
 
         /**@{*/
@@ -392,26 +375,34 @@ public:
         /**@}*/
 #endif
 
-private:
-        BTD *pBtd; // Pointer to BTD instance
-
+protected:
+        /** @name BluetoothService implementation */
+        /**
+         * Used to pass acldata to the services.
+         * @param ACLData Incoming acldata.
+         */
+        virtual void ACLData(uint8_t* ACLData);
+        /** Used to run part of the state machine. */
+        virtual void Run();
+        /** Use this to reset the service. */
+        virtual void Reset();
         /**
          * Called when the controller is successfully initialized.
          * Use attachOnInit(void (*funcOnInit)(void)) to call your own function.
          * This is useful for instance if you want to set the LEDs in a specific way.
          */
-        void onInit();
-        void (*pFuncOnInit)(void); // Pointer to function called in onInit()
+        virtual void onInit();
+        /**@}*/
+
+private:
 
         void L2CAP_task(); // L2CAP state machine
 
         /* Variables filled from HCI event management */
-        uint16_t hci_handle;
         bool activeConnection; // Used to indicate if it's already has established a connection
 
         /* Variables used by high level L2CAP task */
         uint8_t l2cap_state;
-        uint32_t l2cap_event_flag; // L2CAP flags of received Bluetooth events
         uint8_t wii_event_flag; // Used for Wii flags
 
         uint32_t ButtonState;
@@ -432,7 +423,6 @@ private:
         uint8_t control_dcid[2]; // 0x0060
         uint8_t interrupt_scid[2]; // L2CAP source CID for HID_Interrupt
         uint8_t interrupt_dcid[2]; // 0x0061
-        uint8_t identifier; // Identifier for connection
 
         /* HID Commands */
         void HID_Command(uint8_t* data, uint8_t nbytes);
@@ -457,7 +447,7 @@ private:
 
         bool activateNunchuck;
         bool motionValuesReset; // This bool is true when the gyro values has been reset
-        unsigned long timer;
+        uint32_t timer;
 
         uint8_t wiiState; // Stores the value in l2capinbuf[12] - (0x01: Battery is nearly empty), (0x02:  An Extension Controller is connected), (0x04: Speaker enabled), (0x08: IR enabled), (0x10: LED1, 0x20: LED2, 0x40: LED3, 0x80: LED4)
         uint8_t batteryLevel;
diff --git a/XBOXOLD.h b/XBOXOLD.h
index f5f95dce..dfb06df6 100644
--- a/XBOXOLD.h
+++ b/XBOXOLD.h
@@ -19,6 +19,7 @@
 #define _xboxold_h_
 
 #include "Usb.h"
+#include "Hid.h"
 #include "controllerEnums.h"
 
 /* Data Xbox taken from descriptors */
@@ -39,10 +40,6 @@
 #define XBOX_OLD_PID3                           0x0287 // Microsoft Microsoft Xbox Controller S
 #define XBOX_OLD_PID4                           0x0289 // Smaller Microsoft Xbox controller (US)
 
-// Used in control endpoint header for HID Commands
-#define bmREQ_HID_OUT USB_SETUP_HOST_TO_DEVICE|USB_SETUP_TYPE_CLASS|USB_SETUP_RECIPIENT_INTERFACE
-#define HID_REQUEST_SET_REPORT      0x09
-
 #define XBOX_MAX_ENDPOINTS   3
 
 /** This class implements support for a the original Xbox controller via USB. */
diff --git a/XBOXUSB.h b/XBOXUSB.h
index 8d5ecb2a..f79523c1 100644
--- a/XBOXUSB.h
+++ b/XBOXUSB.h
@@ -19,6 +19,7 @@
 #define _xboxusb_h_
 
 #include "Usb.h"
+#include "Hid.h"
 #include "xboxEnums.h"
 
 /* Data Xbox 360 taken from descriptors */
@@ -45,10 +46,6 @@
 
 #define XBOX_REPORT_BUFFER_SIZE 14 // Size of the input report buffer
 
-// Used in control endpoint header for HID Commands
-#define bmREQ_HID_OUT USB_SETUP_HOST_TO_DEVICE|USB_SETUP_TYPE_CLASS|USB_SETUP_RECIPIENT_INTERFACE
-#define HID_REQUEST_SET_REPORT      0x09
-
 #define XBOX_MAX_ENDPOINTS   3
 
 /** This class implements support for a Xbox wired controller via USB. */
diff --git a/hid.cpp b/hid.cpp
index c3c425b8..174215df 100644
--- a/hid.cpp
+++ b/hid.cpp
@@ -24,7 +24,7 @@ uint8_t HID::GetReportDescr(uint8_t ep, USBReadParser *parser) {
         const uint8_t constBufLen = 64;
         uint8_t buf[constBufLen];
 
-        uint8_t rcode = pUsb->ctrlReq(bAddress, ep, bmREQ_HIDREPORT, USB_REQUEST_GET_DESCRIPTOR, 0x00,
+        uint8_t rcode = pUsb->ctrlReq(bAddress, ep, bmREQ_HID_REPORT, USB_REQUEST_GET_DESCRIPTOR, 0x00,
                 HID_DESCRIPTOR_REPORT, 0x0000, 128, constBufLen, buf, (USBReadParser*)parser);
 
         //return ((rcode != hrSTALL) ? rcode : 0);
@@ -35,7 +35,7 @@ uint8_t HID::GetReportDescr(uint16_t wIndex, USBReadParser *parser) {
         const uint8_t constBufLen = 64;
         uint8_t buf[constBufLen];
 
-        uint8_t rcode = pUsb->ctrlReq(bAddress, 0x00, bmREQ_HIDREPORT, USB_REQUEST_GET_DESCRIPTOR, 0x00,
+        uint8_t rcode = pUsb->ctrlReq(bAddress, 0x00, bmREQ_HID_REPORT, USB_REQUEST_GET_DESCRIPTOR, 0x00,
                 HID_DESCRIPTOR_REPORT, wIndex, 128, constBufLen, buf, (USBReadParser*)parser);
 
         //return ((rcode != hrSTALL) ? rcode : 0);
@@ -48,27 +48,27 @@ uint8_t HID::GetReportDescr(uint16_t wIndex, USBReadParser *parser) {
 //}
 
 uint8_t HID::SetReport(uint8_t ep, uint8_t iface, uint8_t report_type, uint8_t report_id, uint16_t nbytes, uint8_t* dataptr) {
-        return ( pUsb->ctrlReq(bAddress, ep, bmREQ_HIDOUT, HID_REQUEST_SET_REPORT, report_id, report_type, iface, nbytes, nbytes, dataptr, NULL));
+        return ( pUsb->ctrlReq(bAddress, ep, bmREQ_HID_OUT, HID_REQUEST_SET_REPORT, report_id, report_type, iface, nbytes, nbytes, dataptr, NULL));
 }
 
 uint8_t HID::GetReport(uint8_t ep, uint8_t iface, uint8_t report_type, uint8_t report_id, uint16_t nbytes, uint8_t* dataptr) {
-        return ( pUsb->ctrlReq(bAddress, ep, bmREQ_HIDIN, HID_REQUEST_GET_REPORT, report_id, report_type, iface, nbytes, nbytes, dataptr, NULL));
+        return ( pUsb->ctrlReq(bAddress, ep, bmREQ_HID_IN, HID_REQUEST_GET_REPORT, report_id, report_type, iface, nbytes, nbytes, dataptr, NULL));
 }
 
 uint8_t HID::GetIdle(uint8_t iface, uint8_t reportID, uint8_t* dataptr) {
-        return ( pUsb->ctrlReq(bAddress, 0, bmREQ_HIDIN, HID_REQUEST_GET_IDLE, reportID, 0, iface, 0x0001, 0x0001, dataptr, NULL));
+        return ( pUsb->ctrlReq(bAddress, 0, bmREQ_HID_IN, HID_REQUEST_GET_IDLE, reportID, 0, iface, 0x0001, 0x0001, dataptr, NULL));
 }
 
 uint8_t HID::SetIdle(uint8_t iface, uint8_t reportID, uint8_t duration) {
-        return ( pUsb->ctrlReq(bAddress, 0, bmREQ_HIDOUT, HID_REQUEST_SET_IDLE, reportID, duration, iface, 0x0000, 0x0000, NULL, NULL));
+        return ( pUsb->ctrlReq(bAddress, 0, bmREQ_HID_OUT, HID_REQUEST_SET_IDLE, reportID, duration, iface, 0x0000, 0x0000, NULL, NULL));
 }
 
 uint8_t HID::SetProtocol(uint8_t iface, uint8_t protocol) {
-        return ( pUsb->ctrlReq(bAddress, 0, bmREQ_HIDOUT, HID_REQUEST_SET_PROTOCOL, protocol, 0x00, iface, 0x0000, 0x0000, NULL, NULL));
+        return ( pUsb->ctrlReq(bAddress, 0, bmREQ_HID_OUT, HID_REQUEST_SET_PROTOCOL, protocol, 0x00, iface, 0x0000, 0x0000, NULL, NULL));
 }
 
 uint8_t HID::GetProtocol(uint8_t iface, uint8_t* dataptr) {
-        return ( pUsb->ctrlReq(bAddress, 0, bmREQ_HIDIN, HID_REQUEST_GET_PROTOCOL, 0x00, 0x00, iface, 0x0001, 0x0001, dataptr, NULL));
+        return ( pUsb->ctrlReq(bAddress, 0, bmREQ_HID_IN, HID_REQUEST_GET_PROTOCOL, 0x00, 0x00, iface, 0x0001, 0x0001, dataptr, NULL));
 }
 
 void HID::PrintEndpointDescriptor(const USB_ENDPOINT_DESCRIPTOR* ep_ptr) {
diff --git a/hid.h b/hid.h
index a61cfb90..88b8ad3f 100644
--- a/hid.h
+++ b/hid.h
@@ -60,9 +60,9 @@ e-mail   :  support@circuitsathome.com
 #define TAG_LOCAL_USAGEMAX                      0x20
 
 /* HID requests */
-#define bmREQ_HIDOUT                            USB_SETUP_HOST_TO_DEVICE|USB_SETUP_TYPE_CLASS|USB_SETUP_RECIPIENT_INTERFACE
-#define bmREQ_HIDIN                             USB_SETUP_DEVICE_TO_HOST|USB_SETUP_TYPE_CLASS|USB_SETUP_RECIPIENT_INTERFACE
-#define bmREQ_HIDREPORT                         USB_SETUP_DEVICE_TO_HOST|USB_SETUP_TYPE_STANDARD|USB_SETUP_RECIPIENT_INTERFACE
+#define bmREQ_HID_OUT                           USB_SETUP_HOST_TO_DEVICE|USB_SETUP_TYPE_CLASS|USB_SETUP_RECIPIENT_INTERFACE
+#define bmREQ_HID_IN                            USB_SETUP_DEVICE_TO_HOST|USB_SETUP_TYPE_CLASS|USB_SETUP_RECIPIENT_INTERFACE
+#define bmREQ_HID_REPORT                        USB_SETUP_DEVICE_TO_HOST|USB_SETUP_TYPE_STANDARD|USB_SETUP_RECIPIENT_INTERFACE
 
 /* HID constants. Not part of chapter 9 */
 /* Class-Specific Requests */