diff --git a/src/debug.cpp b/src/debug.cpp
new file mode 100644
index 0000000..15c213c
--- /dev/null
+++ b/src/debug.cpp
@@ -0,0 +1,21 @@
+#include "debug.h"
+
+
+Debug::Debug() {
+    P1DIR |= BIT0 | BIT1 | BIT2 | BIT3;
+}
+
+void Debug::write(uint16_t v) {
+  P1OUT |= BIT1;
+  for (uint8_t i = 0; i < 16; i++) {
+    P1OUT |= BIT3;
+    uint16_t x = v >> i;
+    if ((x & 1) == 1) {
+      P1OUT |= BIT2;
+    } else {
+      P1OUT &= ~BIT2;
+    }
+    P1OUT &= ~BIT3;
+  }
+  P1OUT &= ~BIT1;
+}
\ No newline at end of file
diff --git a/src/debug.h b/src/debug.h
new file mode 100644
index 0000000..3e88eac
--- /dev/null
+++ b/src/debug.h
@@ -0,0 +1,8 @@
+#include <stdint.h>
+
+
+class Debug {
+public:
+  Debug();
+  void write(uint16_t v);
+}
diff --git a/src/debug.txt b/src/debug.txt
deleted file mode 100644
index 1014473..0000000
--- a/src/debug.txt
+++ /dev/null
@@ -1,12 +0,0 @@
-//    P1OUT |= BIT1;
-//    for (uint8_t i = 0; i < 16; i++) {
-//      P1OUT |= BIT3;
-//      uint16_t x = newPwm >> i;
-//      if ((x & 1) == 1) {
-//        P1OUT |= BIT2;
-//      } else {
-//        P1OUT &= ~BIT2;
-//      }
-//      P1OUT &= ~BIT3;
-//    }
-//    P1OUT &= ~BIT1;
diff --git a/src/hmi.cpp b/src/hmi.cpp
index 95c27c3..dfb4931 100644
--- a/src/hmi.cpp
+++ b/src/hmi.cpp
@@ -60,11 +60,14 @@ private:
 };
 
 
-SwitchPort2 switch1 = SwitchPort2(BIT0);
-SwitchPort2 switch2 = SwitchPort2(BIT1);
-SwitchPort2 switch3 = SwitchPort2(BIT2);
+SwitchPort2 switch1;
+SwitchPort2 switch2;
+SwitchPort2 switch3;
 
 void hmiInit() {
+  switch1 = SwitchPort2(BIT0);
+  switch2 = SwitchPort2(BIT1);
+  switch3 = SwitchPort2(BIT2);
 }
 
 void hmiExec() {
diff --git a/src/main.cpp b/src/main.cpp
index 5699fc6..974738a 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -6,7 +6,6 @@
  */
 
 #include <intrinsics.h>
-//#include <isr_compat.h>
 #include <msp430g2553.h>
 #include <stdint.h>
 
@@ -25,8 +24,6 @@ void init() {
   BCSCTL1 = XT2OFF | RSEL0 | RSEL1 | RSEL2 | RSEL3;
   BCSCTL2 = 0;
   BCSCTL3 = 0;
-
-  P1DIR |= BIT0 | BIT1 | BIT2 | BIT3;
 }
 
 int main() {
diff --git a/src/params.h b/src/params.h
index 9366710..63302a4 100644
--- a/src/params.h
+++ b/src/params.h
@@ -5,6 +5,9 @@
 const uint32_t ADC_TIME_OUT = 100000;
 const uint16_t ADC_MAX = 511;
 
+const uint16_t PWM_MIN = 0;
+const uint16_t PWM_MAX = 511;
+
 const uint8_t DAMPING = 5;
 
 const float Ctrl_P = 10.0;
@@ -15,7 +18,6 @@ const float U_ref = 3.3;
 const float R_top = 100000.0;
 const float R_bottom = 6200.0;
 
-const float U_des = 5.0;
 
 
 #endif /* PARAMS_H_ */
diff --git a/src/pwm.h b/src/pwm.h
index a88b12a..4a26644 100644
--- a/src/pwm.h
+++ b/src/pwm.h
@@ -3,9 +3,6 @@
 
 
 
-const uint16_t PWM_MIN = 0;
-const uint16_t PWM_MAX = 511;
-const uint16_t CYCLE_DELAY = 1;
 
 
 void pwmInit();