diff --git a/src/kaleidoscope/hardware/ez/ErgoDox.cpp b/src/kaleidoscope/hardware/ez/ErgoDox.cpp
index 7150ef29..46f1fdd6 100644
--- a/src/kaleidoscope/hardware/ez/ErgoDox.cpp
+++ b/src/kaleidoscope/hardware/ez/ErgoDox.cpp
@@ -97,13 +97,13 @@ void __attribute__((optimize(3))) ErgoDox::readMatrix() {
   scanner_.reattachExpanderOnError();
 
   for (uint8_t row = 0; row < ROWS / 2; row++) {
-    scanner_.selectRow(row);
-    scanner_.selectRow(row + ROWS / 2);
+    scanner_.selectExtenderRow(row);
+    scanner_.toggleATMegaRow(row);
 
     readMatrixRow(row);
     readMatrixRow(row + ROWS / 2);
 
-    scanner_.unselectRows();
+    scanner_.toggleATMegaRow(row);
   }
 }
 
diff --git a/src/kaleidoscope/hardware/ez/ErgoDox/ErgoDoxScanner.cpp b/src/kaleidoscope/hardware/ez/ErgoDox/ErgoDoxScanner.cpp
index c2042c8f..e7fcb9e3 100644
--- a/src/kaleidoscope/hardware/ez/ErgoDox/ErgoDoxScanner.cpp
+++ b/src/kaleidoscope/hardware/ez/ErgoDox/ErgoDoxScanner.cpp
@@ -27,6 +27,7 @@
 
 #include "kaleidoscope/hardware/ez/ErgoDox/ErgoDoxScanner.h"
 #include <avr/wdt.h>
+#include "kaleidoscope/hardware/avr/pins_and_ports.h"
 #include "kaleidoscope/hardware/ez/ErgoDox/i2cmaster.h"
 
 #define I2C_ADDR        0b0100000
@@ -85,78 +86,42 @@ out:
   return status;
 }
 
-void
-ErgoDoxScanner::initCols() {
-  DDRF  &= ~(1 << 7 | 1 << 6 | 1 << 5 | 1 << 4 | 1 << 1 | 1 << 0);
-  PORTF |= (1 << 7 | 1 << 6 | 1 << 5 | 1 << 4 | 1 << 1 | 1 << 0);
-}
-
 void
 ErgoDoxScanner::begin() {
   expander_error_ = initExpander();
 
-  unselectRows();
-  initCols();
+  // Init columns
+  DDRF  &= ~(1 << 7 | 1 << 6 | 1 << 5 | 1 << 4 | 1 << 1 | 1 << 0);
+  PORTF |= (1 << 7 | 1 << 6 | 1 << 5 | 1 << 4 | 1 << 1 | 1 << 0);
+
+  // Init rows
+  DDRB  |= (1 << 0 | 1 << 1 | 1 << 2 | 1 << 3);
+  PORTB |= (1 << 0 | 1 << 1 | 1 << 2 | 1 << 3);
+  DDRD  |= (1 << 2 | 1 << 3);
+  PORTD |= (1 << 2 | 1 << 3);
+  DDRC  |= (1 << 6);
+  PORTC |= (1 << 6);
 }
 
-void __attribute__((optimize(3)))
-ErgoDoxScanner::selectRow(int row) {
-  if (row < 7) {
-    if (!expander_error_) {
-      expander_error_ = i2c_start(I2C_ADDR_WRITE);
-      if (expander_error_)
-        goto out;
-      expander_error_ = i2c_write(GPIOA);
-      if (expander_error_)
-        goto out;
-      expander_error_ = i2c_write(0xFF & ~(1 << row));
-      if (expander_error_)
-        goto out;
+void __attribute__((optimize(3))) ErgoDoxScanner::selectExtenderRow(int row) {
+  if (!expander_error_) {
+    expander_error_ = i2c_start(I2C_ADDR_WRITE);
+    if (expander_error_)
+      goto out;
+    expander_error_ = i2c_write(GPIOA);
+    if (expander_error_)
+      goto out;
+    expander_error_ = i2c_write(0xFF & ~(1 << row));
+    if (expander_error_)
+      goto out;
 out:
-      i2c_stop();
-    }
-  } else {
-    switch (row) {
-    case 7:
-      DDRB  |= (1 << 0);
-      PORTB &= ~(1 << 0);
-      break;
-    case 8:
-      DDRB  |= (1 << 1);
-      PORTB &= ~(1 << 1);
-      break;
-    case 9:
-      DDRB  |= (1 << 2);
-      PORTB &= ~(1 << 2);
-      break;
-    case 10:
-      DDRB  |= (1 << 3);
-      PORTB &= ~(1 << 3);
-      break;
-    case 11:
-      DDRD  |= (1 << 2);
-      PORTD &= ~(1 << 3);
-      break;
-    case 12:
-      DDRD  |= (1 << 3);
-      PORTD &= ~(1 << 3);
-      break;
-    case 13:
-      DDRC  |= (1 << 6);
-      PORTC &= ~(1 << 6);
-      break;
-    }
+    i2c_stop();
   }
 }
 
-void __attribute__((optimize(3)))
-ErgoDoxScanner::unselectRows() {
-  DDRB  &= ~(1 << 0 | 1 << 1 | 1 << 2 | 1 << 3);
-  PORTB &= ~(1 << 0 | 1 << 1 | 1 << 2 | 1 << 3);
-  DDRD  &= ~(1 << 2 | 1 << 3);
-  PORTD &= ~(1 << 2 | 1 << 3);
-  DDRC  &= ~(1 << 6);
-  PORTC &= ~(1 << 6);
+void __attribute__((optimize(3))) ErgoDoxScanner::toggleATMegaRow(int row) {
+  static uint8_t row_pins[] = { PIN_B0, PIN_B1, PIN_B2, PIN_B3, PIN_D2, PIN_D3, PIN_C6 };
+  OUTPUT_TOGGLE(row_pins[row]);
 }
 
 uint8_t __attribute__((optimize(3)))
diff --git a/src/kaleidoscope/hardware/ez/ErgoDox/ErgoDoxScanner.h b/src/kaleidoscope/hardware/ez/ErgoDox/ErgoDoxScanner.h
index c7613e31..b5249655 100644
--- a/src/kaleidoscope/hardware/ez/ErgoDox/ErgoDoxScanner.h
+++ b/src/kaleidoscope/hardware/ez/ErgoDox/ErgoDoxScanner.h
@@ -38,9 +38,8 @@ class ErgoDoxScanner {
   ErgoDoxScanner() {}
 
   void begin();
-  void initCols();
-  void selectRow(int row);
-  void unselectRows();
+  void toggleATMegaRow(int row);
+  void selectExtenderRow(int row);
   uint8_t readCols(int row);
 
   void reattachExpanderOnError();