diff --git a/Makefile b/Makefile
index 67d1481..90d16ea 100644
--- a/Makefile
+++ b/Makefile
@@ -125,6 +125,7 @@ Test:
 	g++ -c -Ilibopeninv/include/ -Iinclude -o params.o libopeninv/src/params.cpp
 	g++ -c -Ilibopeninv/include/ -Iinclude -o errormessage.o libopeninv/src/errormessage.cpp
 	g++ -c -Ilibopeninv/include/ -Iinclude -o printf.o libopeninv/src/printf.cpp
+	g++ -c -Ilibopeninv/include/ -Iinclude -o picontroller.o libopeninv/src/picontroller.cpp
 	g++ -c -Ilibopeninv/include/ -Iinclude -o charger.o src/charger.cpp -D TEST_COMMON_H
 	g++ -c -Ilibopeninv/include/ -Iinclude -o digio_mock.o test/digio_mock.cpp
 	g++ -c -Ilibopeninv/include/ -Iinclude -o timer_mock.o test/timer_mock.cpp
diff --git a/include/charger.h b/include/charger.h
index e921599..81de23a 100644
--- a/include/charger.h
+++ b/include/charger.h
@@ -1,7 +1,9 @@
 #ifndef CHARGER_H
 #define CHARGER_H
+#include "../libopeninv/include/picontroller.h"
 #include "../libopeninv/include/params.h"
 extern uint32_t startTime;
+extern PiController dcCurController;
 bool IsEvseInput();
 bool CheckUnplugged();
 bool CheckTimeout();
@@ -13,4 +15,8 @@ bool CheckStartCondition();
 bool CheckVoltage();
 void ResetValuesInOffMode();
 void CalcEnable();
+bool CheckChargerFaults();
+void ChargerStateMachine();
+void CalcAcCurrentLimit();
+void ChargerStateMachine();
 #endif
diff --git a/libopeninv/libopeninv.a b/libopeninv/libopeninv.a
index e7fc1de..fa7118d 100644
Binary files a/libopeninv/libopeninv.a and b/libopeninv/libopeninv.a differ
diff --git a/src/charger.cpp b/src/charger.cpp
index ade0736..afa2300 100644
--- a/src/charger.cpp
+++ b/src/charger.cpp
@@ -208,3 +208,167 @@ void CalcEnable()
    Param::SetInt(Param::enable, enable);
 }
 
+// TODO: no unit tests below here (see test_logic.h and implement there)
+
+bool CheckChargerFaults()
+{
+   const int acPresentThresh = 70;
+   const int timeout = 20;
+   static int counters[3] = { timeout, timeout, timeout };
+   int configuredChargers = Param::GetInt(Param::chargerena);
+   bool timeouts[3];
+   bool active1 = (configuredChargers & 1) && (Param::GetInt(Param::c1uac) > acPresentThresh);
+   bool active2 = (configuredChargers & 2) && (Param::GetInt(Param::c2uac) > acPresentThresh);
+   bool active3 = (configuredChargers & 4) && (Param::GetInt(Param::c3uac) > acPresentThresh);
+
+   timeouts[0] = (Param::GetInt(Param::c1flag) & FLAG_CHECK) != 0;
+   timeouts[1] = (Param::GetInt(Param::c2flag) & FLAG_CHECK) != 0;
+   timeouts[2] = (Param::GetInt(Param::c3flag) & FLAG_CHECK) != 0;
+
+   for (int i = 0; i < 3; i++)
+   {
+      if (timeouts[i])
+      {
+         if (counters[i] > 0)
+         {
+            counters[i]--;
+            timeouts[i] = false;
+         }
+         else
+         {
+            ErrorMessage::Post(ERR_CHARGERCAN);
+         }
+      }
+      else
+      {
+         counters[i] = timeout;
+      }
+   }
+
+   //Set check flag. By the next call this should be deleted by the CAN module
+   Param::SetInt(Param::c1flag, Param::GetInt(Param::c1flag) | FLAG_CHECK);
+   Param::SetInt(Param::c2flag, Param::GetInt(Param::c2flag) | FLAG_CHECK);
+   Param::SetInt(Param::c3flag, Param::GetInt(Param::c3flag) | FLAG_CHECK);
+
+   return (active1 && ((Param::GetInt(Param::c1flag) & FLAG_FAULT) || timeouts[0])) ||
+          (active2 && ((Param::GetInt(Param::c2flag) & FLAG_FAULT) || timeouts[1])) ||
+          (active3 && ((Param::GetInt(Param::c3flag) & FLAG_FAULT) || timeouts[2]));
+}
+
+void CalcAcCurrentLimit()
+{
+   int configuredChargers = Param::GetInt(Param::chargerena);
+   float iacLim = Param::GetFloat(Param::iaclim);
+   float hwaclim = Param::GetFloat(Param::hwaclim);
+   float evseLim = Param::GetFloat(Param::evselim);
+   float cableLim = Param::GetFloat(Param::cablelim);
+   int activeModules = ((configuredChargers & 1) > 0) + ((configuredChargers & 2) > 0) + ((configuredChargers & 4) > 0);
+
+   if (IsEvseInput())
+   {
+      iacLim = MIN(iacLim, MIN(evseLim, cableLim));
+   }
+
+   if (Param::GetInt(Param::opmode) == 0)
+   {
+      dcCurController.ResetIntegrator();
+      iacLim = 0;
+   }
+   else
+   {
+      dcCurController.SetMinMaxY(0, iacLim);
+      iacLim = dcCurController.Run(Param::Get(Param::idc));
+   }
+
+   if (Param::GetInt(Param::inputype) == INP_MANUAL ||
+       Param::GetInt(Param::inputype) == INP_TYPE1  ||
+       Param::GetInt(Param::inputype) == INP_TYPE2  ||
+      (Param::GetInt(Param::inputype) == INP_TYPE2_AUTO && !DigIo::threep_in.Get()))
+   {
+      iacLim /= (float)activeModules;
+   }
+
+   iacLim = MIN(iacLim, hwaclim);
+
+   Param::SetFloat(Param::aclim, iacLim);
+}
+
+void ChargerStateMachine()
+{
+   static states state = OFF;
+   int configuredChargers = Param::GetInt(Param::chargerena);
+
+   if (!Param::GetBool(Param::enable))
+   {
+      state = OFF;
+   }
+
+   switch (state)
+   {
+      default:
+      case OFF:
+         Param::SetInt(Param::opmode, 0);
+         DisableAll();
+
+         if (CheckStartCondition())
+         {
+            startTime = rtc_get_counter_val();
+            state = WAITSTART;
+         }
+         break;
+      case WAITSTART:
+         if (CheckDelay())
+            state = ENABLE;
+         break;
+      case ENABLE:
+         DigIo::hvena_out.Set();
+         if (configuredChargers & 1)
+            DigIo::ch1ena_out.Set();
+         if (configuredChargers & 2)
+            DigIo::ch2ena_out.Set();
+         if (configuredChargers & 4)
+            DigIo::ch3ena_out.Set();
+         state = ACTIVATE;
+         break;
+      case ACTIVATE:
+         Param::SetInt(Param::opmode, 1);
+
+         if (configuredChargers & 1)
+            DigIo::ch1act_out.Set();
+         if (configuredChargers & 2)
+            DigIo::ch2act_out.Set();
+         if (configuredChargers & 4)
+            DigIo::ch3act_out.Set();
+
+         startTime = rtc_get_counter_val();
+         state = EVSEACTIVATE;
+         break;
+      case EVSEACTIVATE:
+         DigIo::evseact_out.Set();
+         DigIo::acpres_out.Set();
+
+         if (CheckVoltage() || CheckTimeout())
+            state = STOP;
+         if (CheckUnplugged())
+         {
+            DigIo::acpres_out.Clear();
+            DigIo::evseact_out.Clear();
+            state = OFF;
+         }
+         if (CheckChargerFaults())
+         {
+            DigIo::acpres_out.Clear();
+            state = OFF;
+         }
+         break;
+      case STOP:
+         DisableAll();
+         Param::SetInt(Param::opmode, 0);
+
+         if (CheckUnplugged())
+            state = OFF;
+         break;
+   }
+
+   Param::SetInt(Param::state, state);
+}
diff --git a/src/main.cpp b/src/main.cpp
index 2e45c9a..3dcec61 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -45,172 +45,9 @@
 
 static Stm32Scheduler* scheduler;
 static Can* can;
-static PiController dcCurController;
+PiController dcCurController;
 uint32_t startTime;
 
-static bool CheckChargerFaults()
-{
-   const int acPresentThresh = 70;
-   const int timeout = 20;
-   static int counters[3] = { timeout, timeout, timeout };
-   int configuredChargers = Param::GetInt(Param::chargerena);
-   bool timeouts[3];
-   bool active1 = (configuredChargers & 1) && (Param::GetInt(Param::c1uac) > acPresentThresh);
-   bool active2 = (configuredChargers & 2) && (Param::GetInt(Param::c2uac) > acPresentThresh);
-   bool active3 = (configuredChargers & 4) && (Param::GetInt(Param::c3uac) > acPresentThresh);
-
-   timeouts[0] = (Param::GetInt(Param::c1flag) & FLAG_CHECK) != 0;
-   timeouts[1] = (Param::GetInt(Param::c2flag) & FLAG_CHECK) != 0;
-   timeouts[2] = (Param::GetInt(Param::c3flag) & FLAG_CHECK) != 0;
-
-   for (int i = 0; i < 3; i++)
-   {
-      if (timeouts[i])
-      {
-         if (counters[i] > 0)
-         {
-            counters[i]--;
-            timeouts[i] = false;
-         }
-         else
-         {
-            ErrorMessage::Post(ERR_CHARGERCAN);
-         }
-      }
-      else
-      {
-         counters[i] = timeout;
-      }
-   }
-
-   //Set check flag. By the next call this should be deleted by the CAN module
-   Param::SetInt(Param::c1flag, Param::GetInt(Param::c1flag) | FLAG_CHECK);
-   Param::SetInt(Param::c2flag, Param::GetInt(Param::c2flag) | FLAG_CHECK);
-   Param::SetInt(Param::c3flag, Param::GetInt(Param::c3flag) | FLAG_CHECK);
-
-   return (active1 && ((Param::GetInt(Param::c1flag) & FLAG_FAULT) || timeouts[0])) ||
-          (active2 && ((Param::GetInt(Param::c2flag) & FLAG_FAULT) || timeouts[1])) ||
-          (active3 && ((Param::GetInt(Param::c3flag) & FLAG_FAULT) || timeouts[2]));
-}
-
-static void CalcAcCurrentLimit()
-{
-   int configuredChargers = Param::GetInt(Param::chargerena);
-   float iacLim = Param::GetFloat(Param::iaclim);
-   float hwaclim = Param::GetFloat(Param::hwaclim);
-   float evseLim = Param::GetFloat(Param::evselim);
-   float cableLim = Param::GetFloat(Param::cablelim);
-   int activeModules = ((configuredChargers & 1) > 0) + ((configuredChargers & 2) > 0) + ((configuredChargers & 4) > 0);
-
-   if (IsEvseInput())
-   {
-      iacLim = MIN(iacLim, MIN(evseLim, cableLim));
-   }
-
-   if (Param::GetInt(Param::opmode) == 0)
-   {
-      dcCurController.ResetIntegrator();
-      iacLim = 0;
-   }
-   else
-   {
-      dcCurController.SetMinMaxY(0, iacLim);
-      iacLim = dcCurController.Run(Param::Get(Param::idc));
-   }
-
-   if (Param::GetInt(Param::inputype) == INP_MANUAL ||
-       Param::GetInt(Param::inputype) == INP_TYPE1  ||
-       Param::GetInt(Param::inputype) == INP_TYPE2  ||
-      (Param::GetInt(Param::inputype) == INP_TYPE2_AUTO && !DigIo::threep_in.Get()))
-   {
-      iacLim /= (float)activeModules;
-   }
-
-   iacLim = MIN(iacLim, hwaclim);
-
-   Param::SetFloat(Param::aclim, iacLim);
-}
-
-static void ChargerStateMachine()
-{
-   static states state = OFF;
-   int configuredChargers = Param::GetInt(Param::chargerena);
-
-   if (!Param::GetBool(Param::enable))
-   {
-      state = OFF;
-   }
-
-   switch (state)
-   {
-      default:
-      case OFF:
-         Param::SetInt(Param::opmode, 0);
-         DisableAll();
-
-         if (CheckStartCondition())
-         {
-            startTime = rtc_get_counter_val();
-            state = WAITSTART;
-         }
-         break;
-      case WAITSTART:
-         if (CheckDelay())
-            state = ENABLE;
-         break;
-      case ENABLE:
-         DigIo::hvena_out.Set();
-         if (configuredChargers & 1)
-            DigIo::ch1ena_out.Set();
-         if (configuredChargers & 2)
-            DigIo::ch2ena_out.Set();
-         if (configuredChargers & 4)
-            DigIo::ch3ena_out.Set();
-         state = ACTIVATE;
-         break;
-      case ACTIVATE:
-         Param::SetInt(Param::opmode, 1);
-
-         if (configuredChargers & 1)
-            DigIo::ch1act_out.Set();
-         if (configuredChargers & 2)
-            DigIo::ch2act_out.Set();
-         if (configuredChargers & 4)
-            DigIo::ch3act_out.Set();
-
-         startTime = rtc_get_counter_val();
-         state = EVSEACTIVATE;
-         break;
-      case EVSEACTIVATE:
-         DigIo::evseact_out.Set();
-         DigIo::acpres_out.Set();
-
-         if (CheckVoltage() || CheckTimeout())
-            state = STOP;
-         if (CheckUnplugged())
-         {
-            DigIo::acpres_out.Clear();
-            DigIo::evseact_out.Clear();
-            state = OFF;
-         }
-         if (CheckChargerFaults())
-         {
-            DigIo::acpres_out.Clear();
-            state = OFF;
-         }
-         break;
-      case STOP:
-         DisableAll();
-         Param::SetInt(Param::opmode, 0);
-
-         if (CheckUnplugged())
-            state = OFF;
-         break;
-   }
-
-   Param::SetInt(Param::state, state);
-}
-
 //sample 100ms task
 static void Ms100Task(void)
 {
diff --git a/test/test.out b/test/test.out
index baf4831..6594a49 100755
Binary files a/test/test.out and b/test/test.out differ
diff --git a/test/test_common.h b/test/test_common.h
index 41f96cf..e67bbd5 100644
--- a/test/test_common.h
+++ b/test/test_common.h
@@ -3,4 +3,6 @@
 #include "timer_mock.h"
 #include "digio_mock.h"
 #include "charger.h"
+uint32_t startTime;
+PiController dcCurController;
 #endif
diff --git a/test/test_logic.h b/test/test_logic.h
index 3ad5e5b..175e2ec 100644
--- a/test/test_logic.h
+++ b/test/test_logic.h
@@ -3,7 +3,6 @@
 #endif
 #include <assert.h>
 #include "test_common.h"
-uint32_t startTime;
 
 void test_calc_totals()
 {
@@ -238,3 +237,7 @@ void test_calc_enable(){
   //}
   //assert(3 == canenable);
 }
+
+// TODO: test_charger_state_machine(){}
+// TODO: test_calc_ac_current_limit(){}
+// TODO: test_check_charger_faults(){}