diff --git a/.vscode/settings.json b/.vscode/settings.json
new file mode 100644
index 0000000..b4cb1b7
--- /dev/null
+++ b/.vscode/settings.json
@@ -0,0 +1,6 @@
+{
+    "files.associations": {
+        "random": "cpp",
+        "limits": "cpp"
+    }
+}
\ No newline at end of file
diff --git a/include/CCUTasks.h b/include/CCUTasks.h
index bcfaa7c..c1d1aa9 100644
--- a/include/CCUTasks.h
+++ b/include/CCUTasks.h
@@ -1 +1 @@
-#include <ht_sched.hpp>
\ No newline at end of file
+#include <ht_sched.hpp>
diff --git a/lib/constants/CCUParams.h b/lib/constants/CCUParams.h
new file mode 100644
index 0000000..8e9e1fa
--- /dev/null
+++ b/lib/constants/CCUParams.h
@@ -0,0 +1,8 @@
+#ifndef __CCUPARAMS_H__
+#define __CCUPARAMS_H__
+namespace CCUParams{
+  const double target_voltage = 3.1; //per cell
+  const float max_AC_current = 15; //need to add the DC current setpoint probably 30 amps
+  const float ideal_cell_temp = 40;
+};
+#endif
\ No newline at end of file
diff --git a/lib/constants/pidParams.h b/lib/constants/pidParams.h
new file mode 100644
index 0000000..70c45a4
--- /dev/null
+++ b/lib/constants/pidParams.h
@@ -0,0 +1,8 @@
+#ifndef __PIDPARAMS_H__
+#define __PIDPARAMS_H__
+namespace PidParams {
+  const float Kp = 1;
+  const float Ki = 1;
+  const float Kd = 1;
+};
+#endif
\ No newline at end of file
diff --git a/lib/interfaces/include/EthernetInterface.h b/lib/interfaces/include/EthernetInterface.h
new file mode 100644
index 0000000..36da88b
--- /dev/null
+++ b/lib/interfaces/include/EthernetInterface.h
@@ -0,0 +1,27 @@
+#ifndef __ETHERNETINTERFACE_H__
+#define __ETHERNETINTERFACE_H__
+
+//WHAT ETHERERNET INTERFACE NEEDS TO DO
+//1. get the protobuf message 2. map the values to a struct 3. send out those values to main charge sys   
+
+struct CCUInputValues //I think this is correct but i dont know if i need to account for all the values in the protobuf message struct
+{
+    float maxCellTemp; //in degress C
+    float maxCellV; //in Volts
+};
+
+struct CCUOutputValues
+{
+    bool currentFlow;
+};
+
+class EthernetInterface{
+public:
+    EthernetInterface();
+    CCUInputValues UpdatingValues();
+
+private:
+    CCUInputValues inputValues;
+    CCUInputValues val;
+};
+#endif
\ No newline at end of file
diff --git a/lib/interfaces/src/EthernetInterface.cpp b/lib/interfaces/src/EthernetInterface.cpp
new file mode 100644
index 0000000..01be449
--- /dev/null
+++ b/lib/interfaces/src/EthernetInterface.cpp
@@ -0,0 +1,11 @@
+#include "EthernetInterface.h"
+
+
+
+/*
+CCUInputValues EthernetInterface::UpdatingValues(){
+  //here is where the protobuf message gets parsed into the struct
+  CCUInputValues val;
+  return CCUInputValues;
+}
+*/
\ No newline at end of file
diff --git a/lib/systems/DerateFactorSystem.h b/lib/systems/DerateFactorSystem.h
deleted file mode 100644
index 38d78ec..0000000
--- a/lib/systems/DerateFactorSystem.h
+++ /dev/null
@@ -1,17 +0,0 @@
-# include <PID_RT.h>
-
-#define PIDrunInterval
-
-float BMS_temps;
-
-/*
-float derateCalculation(){ //need to flesh out what the derate factor actually shoud be
-        //make work using PID of cell temps and board temps - prioritize board temps
-        //if PID rising a lot have max stop where derate goes to 0 / stop charging
-        setInterval(PIDrunInterval);
-        compute(BMS_temps);
-        float factor = getOutput();
-        double final_derate_factor = 1.0;
-        return final_derate_factor; 
-    };
-    */
\ No newline at end of file
diff --git a/lib/systems/MainChargeSystem.h b/lib/systems/MainChargeSystem.h
deleted file mode 100644
index c2d6213..0000000
--- a/lib/systems/MainChargeSystem.h
+++ /dev/null
@@ -1,43 +0,0 @@
-/* Imports */
-#include <Arduino.h> 
-//#include "EthernetInterface.h" // this is not created yet but will take info from acu and give out usable values 
-#include "DerateFactorSystem.h" // not made yet need to decide the scaling based on temp of cells
-//#include "Charger_data.h"
-
-
-/* TO-DO 
-Taking in the message from ACU protobuf recieving 
-_sys becomes System
-*/
-/* Constants */
-
-double target_voltage = 12.4; //currently arbitrary
-bool current_flow = false;
-int max_AC_current = 5;
-
-/* Class Instance Creation */
-
-
-/* Function Init */
-
-int set_charge_current();
-
-/* Logic */
-
-void charge_cycle() {
-  double max_cell_V = 1; // ethernet_message.get_max_cell_V(); returns the max cell Voltage to allow for decision whether or not to continue charging or not (not yet created) the code for individual cell temps is in the AMS code that already exists
-
-  if (max_cell_V >= target_voltage) { //checks whether or not any of the cells are charged yet
-      current_flow = false; //this should be a function that actually turns on/off current flow 
-  }
-  else { 
-      set_charge_current(); //sets the charge current to the PID adjusted value
-      current_flow = true; //same as line 32
-  }
-}
-
-
-int set_charge_current() { //returns an int value for current
-  int current_setpoint = max_AC_current * 1; //derateCalculation(); derate calculation actually needs to be thought of
-  return current_setpoint;
-}
diff --git a/lib/systems/include/MainChargeSystem.h b/lib/systems/include/MainChargeSystem.h
new file mode 100644
index 0000000..7bcccf0
--- /dev/null
+++ b/lib/systems/include/MainChargeSystem.h
@@ -0,0 +1,25 @@
+#ifndef __MAINCHARGE_H__
+#define __MAINCHARGE_H__
+
+
+#include "CCUPID.h"
+#include "EthernetInterface.h"
+#include "CCUParams.h"
+
+
+
+/* Object Instance Creation */ //should I be creating these instances in this headder file??
+
+/* TO-DO 
+Taking in the message from ACU protobuf recieving 
+*/
+class MainChargeSystem {
+  public:
+    MainChargeSystem() : pid() {}
+    float charge_cycle(CCUInputValues inputValues, CCUOutputValues& outputValues); //add safe start that ramps up to max over 10 seconds
+  private:
+    float current_setpoint;
+    PID pid;
+    };
+
+#endif
diff --git a/lib/systems/src/MainChargeSystem.cpp b/lib/systems/src/MainChargeSystem.cpp
new file mode 100644
index 0000000..8658c7d
--- /dev/null
+++ b/lib/systems/src/MainChargeSystem.cpp
@@ -0,0 +1,30 @@
+/* Imports */
+#include "MainChargeSystem.h"
+
+//need to add a way to create the PID object so the same one is used for the charging so that the prev_error works
+namespace calc {
+  float min(float v1, float v2){
+      if (v1 < v2){
+          return v1;
+      }
+      else{
+          return v2;
+      }
+  };
+};
+
+float MainChargeSystem::charge_cycle(CCUInputValues inputValues, CCUOutputValues &outputValues){
+    if (inputValues.maxCellV >= CCUParams::target_voltage || inputValues.maxCellTemp > CCUParams::ideal_cell_temp) { //checks whether or not any of the cells are charged yet
+      outputValues.currentFlow = false; //this should be a function that actually turns on/off current flow 
+      current_setpoint = 0;
+      return current_setpoint;
+    }
+    else { 
+      float pidTemp = pid.compute(CCUParams::ideal_cell_temp, inputValues.maxCellTemp); 
+      float pidVolts = pid.compute(CCUParams::target_voltage, inputValues.maxCellV);
+      float scalar = calc::min(pidTemp, pidVolts);
+      outputValues.currentFlow = true; //needs to be replaced with the actual thing to set current flow to the correct value
+      current_setpoint = CCUParams::max_AC_current * scalar; //currently the PID returns a value greater than 1
+      return current_setpoint;      
+    }
+}
diff --git a/lib/utilities/CCUPID.h b/lib/utilities/CCUPID.h
new file mode 100644
index 0000000..8d7efae
--- /dev/null
+++ b/lib/utilities/CCUPID.h
@@ -0,0 +1,31 @@
+#ifndef __CCUPID_H__
+#define __CCUPID_H__
+
+
+#include "PIDParams.h"
+class PID {
+public:
+    PID(float kp = PidParams::Kp, float ki = PidParams::Ki, float kd = PidParams::Kd) : Kp(kp), Ki(ki), Kd(kd), prev_error(0), integral(0) {}
+    
+    float compute(double setpoint, double measured_value) {
+        float error = setpoint - measured_value;
+        integral += error;
+        float derivative = error - prev_error;
+        prev_error = error;
+        return (Kp * error) + (Ki * integral) + (Kd * derivative);
+    }
+
+    void setTunings(double kp, double ki, double kd) {
+        Kp = kp;
+        Ki = ki;
+        Kd = kd;
+    }
+
+private:
+    float Kp, Ki, Kd;
+    float prev_error;
+    float integral;
+};
+
+
+#endif
\ No newline at end of file
diff --git a/platformio.ini b/platformio.ini
index cb92caa..7824c10 100644
--- a/platformio.ini
+++ b/platformio.ini
@@ -1,65 +1,54 @@
-; Hytech CCU PlatformIO configuration file.
+; PlatformIO Project Configuration File
+;
+;   Build options: build flags, source filter
+;   Upload options: custom upload port, speed and extra flags
+;   Library options: dependencies, extra library storages
+;   Advanced options: extra scripting
 ;
 ; Please visit documentation for the other options and examples
 ; https://docs.platformio.org/page/projectconf.html
 
 [common]
+lib_deps_shared = 
+	https://github.com/ssilverman/QNEthernet#v0.26.0
+	https://github.com/hytech-racing/HT_SCHED
 
-  
-lib_deps_shared =
-  https://github.com/ssilverman/QNEthernet#v0.26.0
-  https://github.com/hytech-racing/HT_SCHED
-  https://github.com/RobTillaart/PID_RT.git
-
-; Teensy41 Environment. This environment is the primary environment for uploading code to the car.
-; * Build to verify the on-charger software
-; * UPLOAD to compile and upload the on-charger software
-; * TEST is not yet configured.
 [env:teensy41]
 check_tool = clangtidy
-check_flags =
-  clangtidy: --config-file=./.clang-tidy
+check_flags = 
+	clangtidy: --config-file=./.clang-tidy
 check_severity = medium, high
-
-test_framework=googletest
 build_unflags = -std=gnu++11
 build_flags = 
-  -std=c++17
-  -D TEENSY_OPT_SMALLEST_CODE
-check_src_filters =
-  +<include/*>
-  +<lib/*>
-  +<src/*>
-  -<src/old_main.cpp>
+	-std=c++17
+	-D TEENSY_OPT_SMALLEST_CODE
+check_src_filters = 
+	+<include/*>
+	+<lib/*>
+	+<src/*>
 platform = teensy
 board = teensy41
 framework = arduino
 monitor_speed = 115200
 upload_protocol = teensy-cli
-test_ignore =
-  test_interfaces
-  test_systems
-lib_deps =
-  ${common.lib_deps_shared}
-  https://github.com/hytech-racing/shared_firmware_interfaces.git
-  
-; Test Systems Environment. This is only for compiling and uploading the hardware-abstracted code.
-; * BUILD to verify the SYSTEMS compile.
-; * DO NOT UPLOAD. This is a test-only environment.
-; * TEST to run all unit tests in test_systems on the existing system code.
+test_ignore = 
+	test*
+lib_deps = 
+	${common.lib_deps_shared}
+
 [env:test_systems_env]
 platform = native
 test_framework = googletest
-build_src_filter =
-  -<**/*.c>
-  -<**/*.cpp>
+build_src_filter = 
+	-<**/*.c>
+	-<**/*.cpp>
 build_unflags = -std=gnu++11
 build_flags = 
-  -std=c++17
-  -g
-  -D TESTING_SYSTEMS
-lib_ignore =
-test_ignore=
-  test_interfaces*
-lib_deps =
-  ${common.lib_deps_shared}
\ No newline at end of file
+	-std=c++17
+	-g
+	-D TESTING_SYSTEMS
+lib_ignore = 
+test_filter =
+	test/*
+lib_deps = 
+	google/googletest@^1.15.2
diff --git a/src/main.cpp b/src/main.cpp
index 0a06dca..0659630 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -1,8 +1,7 @@
 /* Libraries */
 #include <Arduino.h>
-#include "MainCharge_sys.h"
 #include <QNEthernet.h>
-
+#include "MainChargeSystem.h"
 /* Parameters */
 bool charge_enable = false;
 
@@ -11,13 +10,13 @@ namespace qn = qindesign::network; //setup of qn namespace
 qn::EthernetUDP udp; //setup of qn namespace
 
 /* Functions */
-void configure_charging();
+
 
 void setup() {
   //Serial.begin(115200); //I dont know if this is the baudrate I also don't know if this is even needed
   qn::Ethernet.begin(); //begins QNEthernet
+  
 }
 
 void loop() {
-  charge_cycle();
 }
\ No newline at end of file
diff --git a/test/test_charge_cycle.h b/test/test_charge_cycle.h
new file mode 100644
index 0000000..ba0274b
--- /dev/null
+++ b/test/test_charge_cycle.h
@@ -0,0 +1,48 @@
+#ifndef __CHARGECYCLE_T__
+#define __CHARGECYCLE_T__
+
+#include "MainChargeSystem.h"
+#include "EthernetInterface.h"
+#include <gtest/gtest.h>
+
+MainChargeSystem mainChargeLoop;
+
+
+TEST(mainChargeTest, bothTooHigh){  
+    CCUInputValues test1inputs;
+    CCUOutputValues test1outputs;
+    test1inputs.maxCellTemp = 70;
+    test1inputs.maxCellV =  4;
+    mainChargeLoop.charge_cycle(test1inputs, test1outputs);
+    EXPECT_EQ(test1outputs.currentFlow,  false);
+};
+
+TEST(mainChargeTest, TempTooHigh){  
+    CCUInputValues test2inputs;
+    CCUOutputValues test2outputs;
+    test2inputs.maxCellTemp = 70;
+    test2inputs.maxCellV =  1.27;
+    mainChargeLoop.charge_cycle(test2inputs, test2outputs);
+    EXPECT_EQ(test2outputs.currentFlow,  false);
+};
+
+TEST(mainChargeTest, VoltsTooHigh){  
+    CCUInputValues test3inputs;
+    CCUOutputValues test3outputs;
+    test3inputs.maxCellTemp = 52.1;
+    test3inputs.maxCellV =  4;
+    mainChargeLoop.charge_cycle(test3inputs, test3outputs);
+    EXPECT_EQ(test3outputs.currentFlow,  false);
+};
+
+TEST(mainChargeTest, shouldBeTrue){  
+    CCUInputValues test4inputs;
+    CCUOutputValues test4outputs;
+    test4inputs.maxCellTemp = 30;
+    test4inputs.maxCellV =  2.3;
+    mainChargeLoop.charge_cycle(test4inputs, test4outputs);
+    EXPECT_EQ(test4outputs.currentFlow,  true);
+};
+
+
+#endif
\ No newline at end of file
diff --git a/test/test_maincharge.cpp b/test/test_maincharge.cpp
new file mode 100644
index 0000000..cabd475
--- /dev/null
+++ b/test/test_maincharge.cpp
@@ -0,0 +1,13 @@
+#include <gtest/gtest.h>
+#include "test_charge_cycle.h"
+
+
+
+int main(int argc, char **argv)
+{
+    testing::InitGoogleTest(&argc, argv);
+	if(RUN_ALL_TESTS()) {
+        // Do nothing (always return 0 and allow PlatformIO to parse result)
+    }
+	return 0;
+}
\ No newline at end of file