diff --git a/src/hmi.cpp b/src/hmi.cpp
index 05e3be7..20506c0 100644
--- a/src/hmi.cpp
+++ b/src/hmi.cpp
@@ -17,48 +17,48 @@
 
 class SwitchPort2 {
 public:
-  SwitchPort2(uint16_t bit) : m_bit(bit), m_state(0), m_cnt(0) {
-    P2REN |= m_bit;
-  };
-  bool get() {
-    bool result = false;
-    switch (m_state) {
-    case 0:
-      if (P2IN & m_bit) {
-        m_cnt = 0;
-      } else {
-        m_cnt++;
-      }
-      if (m_cnt > 100) {
-        m_state = 1;
-        m_cnt = 0;
-      }
-      break;
-    case 1:
-      result = true;
-      m_state = 2;
-      break;
-    case 2:
-      if (P2IN & m_bit) {
-        m_cnt++;
-      } else {
-        m_cnt = 0;
-      }
-      if (m_cnt > 20) {
-        m_state = 0;
-        m_cnt = 0;
-      }
-      break;
-    default:
-      m_state = 0;
-      break;  
-    }
-    return result;
-  };
+	SwitchPort2(uint16_t bit) : m_bit(bit), m_state(0), m_cnt(0) {
+		P2REN |= m_bit;
+	};
+	bool get() {
+		bool result = false;
+		switch (m_state) {
+		case 0:
+			if (P2IN & m_bit) {
+				m_cnt = 0;
+			} else {
+				m_cnt++;
+			}
+			if (m_cnt > 100) {
+				m_state = 1;
+				m_cnt = 0;
+			}
+			break;
+		case 1:
+			result = true;
+			m_state = 2;
+			break;
+		case 2:
+			if (P2IN & m_bit) {
+				m_cnt++;
+			} else {
+				m_cnt = 0;
+			}
+			if (m_cnt > 20) {
+				m_state = 0;
+				m_cnt = 0;
+			}
+			break;
+		default:
+			m_state = 0;
+			break;
+		}
+		return result;
+	};
 private:
-  uint16_t m_bit;
-  uint8_t m_state;
-  uint16_t m_cnt;
+	uint16_t m_bit;
+	uint8_t m_state;
+	uint16_t m_cnt;
 };
 
 
@@ -67,21 +67,36 @@ SwitchPort2 switch2 = SwitchPort2(BIT1);
 SwitchPort2 switch3 = SwitchPort2(BIT2);
 
 void hmiInit() {
-    spiInit();
-    dispInit();
+	spiInit();
+	dispInit();
 }
 
 void hmiExec() {
-  if (switch1.get()) {
-    setUDes(0);
-  }
-  if (switch2.get()) {
-    setUDes(getUDes() - 1.0);
-  }
-  if (switch3.get()) {
-    setUDes(getUDes() + 1.0);
-  }
-  
-  dispSetFloat(0, getUDes());
-  dispSetFloat(1, getUCur());
+	static float savedVoltage = 0.0;
+	static bool enabled = false;
+	if (switch1.get()) {
+		if (enabled) {
+			enabled = false;
+			setUDes(0);
+		} else {
+			enabled = true;
+			setUDes(savedVoltage);
+		}
+	}
+	if (enabled && switch2.get()) {
+		savedVoltage = getUDes() - 1.0;
+		setUDes(savedVoltage);
+	}
+	if (enabled && switch3.get()) {
+		savedVoltage = getUDes() + 1.0;
+		setUDes(savedVoltage);
+	}
+
+	static uint32_t cnt = 0;
+	cnt++;
+	if (cnt >= 10000) {
+		dispSetFloat(0, getUCur());
+		dispSetFloat(1, getUDes());
+		cnt = 0;
+	}
 }
diff --git a/src/pwm.cpp b/src/pwm.cpp
index db55e79..3503ba5 100644
--- a/src/pwm.cpp
+++ b/src/pwm.cpp
@@ -16,7 +16,7 @@
 
 volatile Control ctrl((float)PWM_MIN, (float)PWM_MAX, Ctrl_P, Ctrl_I, Ctrl_D);
 
-volatile float u_des = 5.0;
+volatile float u_des = 0.0;
 volatile float u_curr = 0;
 volatile uint16_t newPwm = 0;