diff --git a/CMakeLists.txt b/CMakeLists.txt
index 763268b..365786a 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -11,7 +11,7 @@ include_directories(/usr/lib/avr/include)
 set(CMAKE_C_FLAGS "-O3 -Wall")
 
 add_link_options(-Wl,--print-memory-usage)
-add_executable(${PROJECT_NAME} main.c)
+add_executable(${PROJECT_NAME} main.c PT6302.c PT6302.h)
 
 add_custom_target(
         FLASH
diff --git a/PT6302.c b/PT6302.c
index b1e0554..6c88433 100644
--- a/PT6302.c
+++ b/PT6302.c
@@ -1,7 +1,54 @@
 //
 // Created by lacko on 02/03/2022.
 //
+#define F_CPU 8000000UL
 #include <avr/io.h>
 #include <util/delay.h>
 #include "PT6302.h"
 
+inline void PT6302_startup (void)
+{
+  DDRB |= (CSBpin | CLKBpin | DINpin);
+  DDRC |= RSTpin;
+  PORTB |= (CSBpin | CLKBpin);
+  PORTB &= ~DINpin;
+  PORTC &= ~RSTpin;
+  _delay_us (TPRZ);
+  PORTC |= RSTpin;
+  _delay_us (TWRSTB);
+}
+
+void transmit_bytes (const uint8_t *payload, const uint8_t size)
+{
+  _delay_us (1);
+  PORTB &= ~CSBpin;
+  for (int i = 0; i < size; ++i)
+    {
+      uint8_t data = payload[i];
+      for (int j = 0; j < 8; ++j, data = (data >> 1))
+        {
+          PORTB &= ~CLKBpin;
+          if ((data & 0x01) == 0)
+            PORTB &= ~DINpin;
+          else
+            PORTB |= DINpin;
+          _delay_us (TCW);
+          PORTB |= CLKBpin;
+          _delay_us (TCW);
+        }
+      _delay_us (TDOFF);
+    }
+  _delay_us (DTCSH);
+  PORTB |= CSBpin;
+}
+
+
+void set_ports(uint8_t gp1, uint8_t gp2)
+{
+  uint8_t command = 0x40;
+  if(gp1 != 0 )
+    command |= 0x01;
+  if (gp2 != 0)
+    command |= 0x02;
+  transmit_bytes (&command, 1);
+}
\ No newline at end of file
diff --git a/PT6302.h b/PT6302.h
index 25891b6..1c1e904 100644
--- a/PT6302.h
+++ b/PT6302.h
@@ -5,7 +5,19 @@
 #ifndef _PT6302_H_
 #define _PT6302_H_
 
+/* PT6302 timing constants, all in usec */
+static const uint8_t TCW = 1;     //CLKB pulse width
+static const uint8_t TDOFF = 16;  //Data processing time
+static const uint8_t DTCSH = 16;  //time difference between tDOFF and tCSH
+static const uint8_t TPRZ = 100;  //VDD rise time
+static const uint8_t TWRSTB = 1;  //RSTB pulse width
 
+enum pins {
+  RSTpin = (1 << PC2),
+  CSBpin = (1 << PB0),
+  CLKBpin = (1 << PB6),
+  DINpin = (1 << PB7),
+};
 
 /* -------------------------------------------------------------
  * Initial setup for the VFD controller interface
@@ -21,4 +33,10 @@ void PT6302_startup(void);
 void transmit_bytes (const uint8_t* payload, uint8_t size);
 
 
+/* ---------------------------------------------------------
+ * Sets GP1 and GP2 to the values given in the function call
+ * Handles the entire communication
+ * --------------------------------------------------------- */
+void set_ports(uint8_t gp1, uint8_t gp2);
+
 #endif //_PT6302_H_
diff --git a/main.c b/main.c
index 8520a1d..0c8b6b8 100644
--- a/main.c
+++ b/main.c
@@ -1,69 +1,19 @@
 #define F_CPU 8000000UL
 #include <avr/io.h>
 #include <util/delay.h>
+#include "PT6302.h"
 
-/* PT6302 timing constants, all in usec */
-static const uint8_t TCW = 1;     //CLKB pulse width
-static const uint8_t TDOFF = 8;   //Data processing time
-static const uint8_t DTCSH = 16;  //time difference between tDOFF and tCSH
-static const uint8_t TPRZ = 100;  //VDD rise time
-static const uint8_t TWRSTB = 1;  //RSTB pulse width
+/* PT6302 commands */
+static const uint8_t lighton = 0x72;
+static const uint8_t lightoff = 0x71;
 
-enum pins {
-  RSTpin = (1 << PC2),
-  CSBpin = (1 << PB0),
-  CLKBpin = (1 << PB6),
-  DINpin = (1 << PB7),
-};
-
-static inline void PT6302_startup (void)
-{
-  DDRB |= (CSBpin | CLKBpin | DINpin);
-  DDRC |= RSTpin;
-  PORTB |= (CSBpin | CLKBpin);
-  PORTB &= ~DINpin;
-  PORTC |= RSTpin;
-  _delay_us (TPRZ);
-  PORTC &= ~RSTpin;
-  _delay_us (TWRSTB);
-}
-
-static void transmit_bytes (const uint8_t *payload, const uint8_t size)
-{
-  PORTB &= ~CSBpin;
-  for (int i = 0; i < size; ++i)
-    {
-      uint8_t data = payload[i];
-      for (int j = 0; j < 8; ++j, data >>= 1)
-        {
-          PORTB &= ~CLKBpin;
-          if ((data & 0x01) == 0)
-            {
-              PORTB |= DINpin;
-            }
-          else
-            {
-              PORTB &= ~DINpin;
-            }
-          _delay_us (TCW);
-          PORTB |= CLKBpin;
-          _delay_us (TCW);
-        }
-      _delay_us (TDOFF);
-    }
-  _delay_us (DTCSH);
-  PORTB |= CSBpin;
-}
 
 int main ()
 {
   PT6302_startup ();
-  DDRC |= (1 << PC4);
+  set_ports (1, 1);
   while (1)
     {
-      PORTC |= (1 << PC4);
-      _delay_ms (1000);
-      PORTC &= ~(1 << PC4);
-      _delay_ms (1000);
+
     }
 }