diff --git a/src/main.cpp b/src/main.cpp
index 8725909..db393c8 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -1,88 +1,117 @@
 #include <Arduino.h>
 #include <Encoder.h>
 #include <LiquidCrystal_I2C.h>
+#include <cstdint>
 
-#define encoder_pin_a 2
-#define encoder_pin_b 3
-#define a_sec 1000000
+// Initialize Encoder Variables
+static constexpr uint8_t ENCODER_PIN_A{2};
+static constexpr uint8_t ENCODER_PIN_B{3};
+Encoder encoder(ENCODER_PIN_A, ENCODER_PIN_B);
 
-// LCD Address is 0x27
-LiquidCrystal_I2C lcd(0x27, 16, 2);
-
-Encoder encoder(encoder_pin_a, encoder_pin_b);
-unsigned long encoder_timer = 0;
-
-float encoder_new = 0;
+uint32_t encoder_timer = 0;
 float encoder_old = 0;
 float frequency = 0;
-unsigned int on_time = 100;
-boolean is_on = false;
-unsigned long timer;
+
+// Initialize LCD Variables
+static constexpr uint8_t I2C_LCD_ADDRESS{0x27};
+static constexpr uint8_t LCD_COLUMNS{16};
+static constexpr uint8_t LCD_ROWS{16};
+LiquidCrystal_I2C lcd(I2C_LCD_ADDRESS, LCD_COLUMNS, LCD_ROWS);
+
+// Initialize LED Variables
+static constexpr uint8_t LED_PIN{5};
+static constexpr uint32_t ON_TIME{100};
+uint32_t led_timer;
+
+/**
+ * @brief Update a display with the frequency and RPM information
+ * @param display a reference to the display option to update
+ * @param frequency the current frequency the LED is blinking at
+ */
+void UpdateDisplay(LiquidCrystal_I2C &display, float new_frequency) {
+  display.setCursor(0, 0); // Set cursor to character 2 on line 0
+  display.print("Frequency:");
+  display.print(new_frequency);
+
+  display.setCursor(0, 1); // Move cursor to character 2 on line 1
+  display.print("RPM:");
+  display.print(new_frequency * 60);
+}
+
+template <uint8_t pinNumber> inline void SetPin5() {
+  PORTD |= (1U << pinNumber);
+}
+
+template <uint8_t pinNumber> inline void ClearPin5() {
+  PORTD &= (0xFF - (1U << pinNumber));
+}
+
+float UpdateFrequency(Encoder &encoder, uint32_t &encoderTimer,
+                      float oldFrequency, float &encoderOld) {
+  float newFrequency = oldFrequency;
+  float encoderNew = float(encoder.read()) / 8;
+  if (encoderNew != encoderOld && millis() - encoderTimer > 100) {
+    float difference = encoderNew - encoderOld;
+
+    newFrequency = oldFrequency + pow(difference, 3);
+    encoderOld = encoderNew;
+
+    if (newFrequency < 0 || newFrequency > 2000 || encoderNew < 0) {
+      newFrequency = 0;
+      encoderNew = 0;
+      encoderOld = 0;
+    }
+    UpdateDisplay(lcd, newFrequency);
+
+    encoderTimer = millis();
+  }
+  return newFrequency;
+}
+
+void Flash(bool &isOn, uint32_t &timer, float frequency, uint32_t onTime) {
+  static constexpr float MICROSECONDS_PER_SECOND{1000000};
+  // Don't bother flashing if the frequency is 0
+  if (frequency == 0) {
+    return;
+  }
+
+  if (isOn == true) {
+    // Wait a set amount of time and then turn the LED off
+    if (micros() - timer >= onTime) {
+      ClearPin5();
+    }
+    // wait the rest of the off period then reset the clock
+    if ((micros() - timer) >= MICROSECONDS_PER_SECOND / frequency - onTime) {
+      isOn = false;
+      timer = micros();
+    }
+  } else {
+    // Turn on the LED
+    SetPin5();
+    isOn = true;
+  }
+}
 
 void setup() {
   // put your setup code here, to run once:
   Serial.begin(9600);
-  pinMode(5, OUTPUT);
+  pinMode(LED_PIN, OUTPUT);
   // Turn off pin 5
-  PORTD = PORTD & B11011111;
-  encoder_new = encoder.read();
+  SetPin5();
   encoder_old = encoder.read();
-  timer = micros();
+  led_timer = micros();
   encoder_timer = millis();
 
   lcd.init();
-  lcd.clear();         
+  lcd.clear();
   lcd.backlight();
-  lcd.setCursor(0,0);   //Set cursor to character 2 on line 0
-  lcd.print("Frequency:");
-  lcd.print(frequency);
-  
-  lcd.setCursor(0,1);   //Move cursor to character 2 on line 1
-  lcd.print("RPM:");
-  lcd.print(frequency*60);
+  UpdateDisplay(lcd, frequency);
 }
 
 void loop() {
-  // Get encoder data
-  float encoder_new = float(encoder.read())/8;
-  if (encoder_new != encoder_old && millis() - encoder_timer > 100) {
-    float difference = encoder_new - encoder_old;
-    
-    frequency = frequency + pow(difference, 3);
-    encoder_old = encoder_new;
+  // Get the new frequency from the encoder
+  frequency = UpdateFrequency(encoder, encoder_timer, frequency, encoder_old);
 
-  if(frequency < 0 || frequency > 2000 || encoder_new < 0){
-    frequency = 0; 
-    encoder_new = 0; 
-    encoder_old = 0;
-  }
-    lcd.setCursor(0,0);   //Set cursor to character 2 on line 0
-    lcd.print("Frequency:");
-    lcd.print(frequency);
-  
-    lcd.setCursor(0,1);   //Move cursor to character 2 on line 1
-    lcd.print("RPM:");
-    lcd.print(frequency*60);
-
-    encoder_timer = millis();
-  }
-
-  if(frequency != 0){
-    if(is_on == true){
-      // Wait a set amount of time and then turn the LED off
-      if(micros()-timer >= on_time){
-        PORTD = PORTD & B11011111;
-      }
-      // wait the rest of the off period then reset the clock
-      if((micros()-timer) >= a_sec/frequency - on_time){
-        is_on = false;
-        timer = micros();
-      }
-    }
-    else{
-      // Turn on the LED
-      PORTD = PORTD | B00100000;
-      is_on = true;
-    }
-  }
+  // Turn on or off the LED at the right time
+  Flash(frequency, led_timer, ON_TIME);
 }
\ No newline at end of file