diff --git a/code/main.c b/code/main.c
index 6839569..526c71b 100644
--- a/code/main.c
+++ b/code/main.c
@@ -50,6 +50,24 @@ void btn_init(){
     PCICR |= (1<<PCIE0);
 }
 
+void sdi_shield_write(uint16_t addr, uint8_t val){
+    i2c_start_wait(SDI_SHIELD_ADDR + I2C_WRITE);     // set device address and write mode
+    i2c_write(addr & 0xFF);
+    i2c_write(addr >> 8);
+    i2c_write(val);
+    i2c_stop();
+}
+
+uint8_t sdi_shield_read(uint16_t addr){
+    i2c_start_wait(SDI_SHIELD_ADDR + I2C_WRITE);     // set device address and write mode
+    i2c_write(addr & 0xFF);
+    i2c_write(addr >> 8);
+    i2c_rep_start(SDI_SHIELD_ADDR + I2C_READ);       // set device address and read mode
+    uint8_t data = i2c_readNak();
+    i2c_stop();
+    return data;
+}
+
 int main(void){
     DDRB |= 1<<VF_BACK_PVW_TALLY_PIN;
     DDRB |= 1<<VF_BACK_PGM_TALLY_PIN;
@@ -123,31 +141,11 @@ int main(void){
     sei();
 
     while(1){
-	//shield_set_flags(0x5000, 0x01);
-	i2c_start_wait(SDI_SHIELD_ADDR + I2C_WRITE);     // set device address and write mode
-	i2c_write(0x00);
-	i2c_write(0x50);
-	i2c_write(0x01);
-	i2c_stop();
+	sdi_shield_write(0x5000, 1);
  
-	i2c_start_wait(SDI_SHIELD_ADDR + I2C_WRITE);     // set device address and write mode
-	i2c_write(0x01);
-	i2c_write(0x50);
-	i2c_rep_start(SDI_SHIELD_ADDR + I2C_READ);       // set device address and read mode
-	uint8_t data_length = i2c_readNak();
-	i2c_stop();
-
-	//uint8_t data_length;
-	//shield_read_uint8(0x5001, &data_length);
+	uint8_t data_length = sdi_shield_read(0x5001);
 	if(data_length){
-	    i2c_start_wait(SDI_SHIELD_ADDR + I2C_WRITE);     // set device address and write mode
-	    i2c_write(menu.settings.cam);
-	    i2c_write(0x51);
-	    i2c_rep_start(SDI_SHIELD_ADDR + I2C_READ);       // set device address and read mode
-	    uint8_t tally = i2c_readNak();
-	    i2c_stop();
-	    //uint8_t tally;
-	    //shield_read_uint8(0x5100+menu.settings.cam, &tally);
+	    uint8_t tally = sdi_shield_read(0x5100 + menu.settings.cam);
 	    pgm = !!(tally & (1<<0));
 	    pvw = !!(tally & (1<<1));
 	}