diff --git a/src/bus/twi.c b/src/bus/twi.c
index 402c799..051737d 100644
--- a/src/bus/twi.c
+++ b/src/bus/twi.c
@@ -13,6 +13,9 @@
 #define  TW_MR_SLA_NACK  0x48
 #define  TW_MR_DATA_ACK  0x50
 
+// XXX: Handle interrupts in TWI_vect ISR? This may not
+// actually be much better than the blocking approach
+
 int TWI_Init(void)
 {
 	// Set SCL bit rate to 400kHz
@@ -23,7 +26,7 @@ int TWI_Init(void)
 	return 0;
 }
 
-int TWI_Start(char addr, char mode)
+int TWI_Start(unsigned char addr, unsigned char mode)
 {
 	unsigned int status;
 
@@ -60,7 +63,7 @@ int TWI_Start(char addr, char mode)
 	return 0;
 }
 
-int TWI_Write(char data)
+int TWI_Write(unsigned char data)
 {
 	TWDR = data;
 	TWCR = (1 << TWEN)    // Enable TWI
diff --git a/src/bus/twi.h b/src/bus/twi.h
index c675a26..f7bc15b 100644
--- a/src/bus/twi.h
+++ b/src/bus/twi.h
@@ -2,8 +2,8 @@
 #define MAD_CORE_BUS_TWI_H
 
 int TWI_Init(void);
-int TWI_Start(char addr, char mode);
-int TWI_Write(char data);
+int TWI_Start(unsigned char addr, unsigned char mode);
+int TWI_Write(unsigned char data);
 int TWI_Wait_ACK(void);
 int TWI_Stop(void);
 
diff --git a/src/main.c b/src/main.c
index c4570d0..ba655c8 100644
--- a/src/main.c
+++ b/src/main.c
@@ -3,15 +3,33 @@
 #include "bus/twi.h"
 
 #include <avr/interrupt.h>
+#include <assert.h>
+
+// ADS1115 (ADC)
+// PCA9546 (Multiplexer TWI)
+// ATH20 (3x)
+// BMP280 (3x)
+
+// PB0 Mosfet 1 Peltier
+// PB1 Mosfet 2 Heating
+// PB2 Mosfet 3 Lights
+
+// PD4 PWM Peltier Hot Side
+// PD5 PWM Peltier Cold Side
+// PD7 PWM Heating
+
+// PC0 TWI Multiplexer SCL
+// PC1 TWI Multiplexer SDA
+
+// PD0 Serial USART RX
+// PD1 Serial USART TX
+
+static void SetPinDefaults(void);
+static void SetTWIChannel(int channel);
 
 int main(void)
 {
-	char crb;
-
-	// ADS1115 (ADC)
-	// PCA9546 (Multiplexer TWI)
-	// ATH20 (3x)
-	// BMP280 (3x)
+	SetPinDefaults();
 
 	USART_Init();
 	TWI_Init();
@@ -19,11 +37,42 @@ int main(void)
 	sei();
 
 	Info("Initializing...");
+	SetTWIChannel(0);
+
+	Sleep(500);
+
+	return 0;
+}
+
+static void SetPinDefaults(void)
+{
+	// Initialize Pin Outputs
+	// ======================
+
+	// PB0: MOSFET #1 (Peltier)
+	DDRB |= (1 << PB0);    // Out
+	PORTB &= ~(1 << PB0);  // Low
+
+	// PB1: MOSFET #2 (Heating)
+	DDRB |= (1 << PB1);    // Out
+	PORTB &= ~(1 << PB1);  // Low
+
+	// PB2: MOSFET #3 (Lights)
+	DDRB |= (1 << PB2);    // Out
+	PORTB &= ~(1 << PB2);  // Low
+}
+
+static void SetTWIChannel(int channel)
+{
+	unsigned char crb;
+
+	assert(channel >= 0);
+	assert(channel <= 3);
 
 	// PCA9546 I2C Multiplexer
 	// =======================
 
-	TWI_Stop();
+	Info("Switching TWI channel to %d...", channel);
 
 	// Excerpts taken from the PCA9546A datasheet:
 	// https://www.ti.com/lit/ds/symlink/pca9546a.pdf
@@ -45,12 +94,14 @@ int main(void)
 
 	//  7  6  5  4   |   3   2   1   0
 	//  X  X  X  X   |  B3  B2  B1  B0
-	//               |  CHANNEL_______
+	//  UNUSED____   |  CHANNEL_______
 
-	crb = 0x01; //  Channel 0  |  00000001
-	//  = 0x02  //  Channel 1  |  00000010
-	//  = 0x04  //  Channel 2  |  00000100
-	//  = 0x08  //  Channel 3  |  00001000
+	crb = (1 << channel);
+
+	// crb = 0x01; //  Channel 0  |  00000001
+	// crb = 0x02  //  Channel 1  |  00000010
+	// crb = 0x04  //  Channel 2  |  00000100
+	// crb = 0x08  //  Channel 3  |  00001000
 
 	TWI_Write(crb);
 
@@ -62,8 +113,5 @@ int main(void)
 	TWI_Wait_ACK();
 	TWI_Stop();
 
-	Info("Switched to TWI channel 0.");
-	Sleep(500);
-
-	return 0;
+	Info("TWI channel was switched to %d.", channel);
 }