diff --git a/master/master/config.h b/master/master/config.h
--- a/master/master/config.h
+++ b/master/master/config.h
@@ -30,6 +30,9 @@
 #define ERROR_SD_PARTITION 3
 #define ERROR_SD_FILE 4
 
+#define ERROR_XBEETIMEOUT 5
+#define ERROR_NOXBEE 6
+
 #define ERROR_CRAP 15
 
 // --------------------------------------------------------------------------
diff --git a/master/master/lib/slavesensors.c b/master/master/lib/slavesensors.c
--- a/master/master/lib/slavesensors.c
+++ b/master/master/lib/slavesensors.c
@@ -76,33 +76,30 @@ void slavesensors_network_scan() {
 	serial0_sendString("Beginning network scan...\r\n\r\n");
 	
 	_delay_ms(500); // xbee warmup
-	
-	// Enter AT mode
-	serial0_sendChar('+'); // Enter AT mode
-	serial0_sendChar('+');
-	serial0_sendChar('+');
+	wdt_reset();
 	
-	// Wait 1ms until we get "OK"
-	while(!serial0_hasChar());
-	
-	bufPtr = serial0_readLine();
-	
+	atOK = slavesensors_enterAT();
+		
 	char nameString[20] = "NONE";
 	
 	char slaveNames[6][16]; // Hold 16-char addresses of max 6 nodes, we only need them for debug so they are local 
 	
-	int nodeCount = 0;
+	uint8_t nodeCount = 0;
 	
 	// wait for OK
 	//todo
-	if(strcmp(bufPtr, "OK") == 0)
+	if(atOK == 0)
 	{
-		atOK = 1;
-		
 		serial0_sendString("ATND");
 		serial0_sendChar(0x0D);
+		
 		// wait for scan to complete
+		uint16_t scanStart = time_millis(); 		
 		while(!serial0_hasChar()) {
+			if(time_millis() - scanStart > 5000) {
+				led_errorcode(ERROR_XBEETIMEOUT);
+				return;
+			}
 			wdt_reset();
 		}
 		
@@ -138,16 +135,12 @@ void slavesensors_network_scan() {
 				lineCount++;
 			}
 
-			
 		}		
-		// Exit AT
-		serial0_sendString("ATCN");
-		serial0_sendChar(0x0D);
+
+		slavesensors_exitAT();
 
 	}
-	else {
-		atOK = 0;
-	}
+	
 	led_on(LED_SIDEBOARD);
 	_delay_ms(200);
 	led_off(LED_SIDEBOARD);
@@ -158,8 +151,8 @@ void slavesensors_network_scan() {
 		snprintf(debugBuf, 64, "  %s - %s%s\r\n", slaveNames[i],slaveAddressHigh,slaveAddressLow[i]);
 		serial0_sendString(debugBuf);
 	}
-	
-	if(atOK != 1) {
+	serial0_sendString("\r\n");
+	if(atOK != 0) {
 		serial0_sendString("AT mode failed \r\n");
 	}
 	#endif
@@ -170,9 +163,87 @@ void slavesensors_network_scan() {
 	// 9 data lines per node, <CR> terminated, followed by a new line with only <CR> at the end of all nodes.
 	
 	// <CR> followed by another <CR> signifies end of scan data
+	
+	slavesensors_selectnode(1);
+	
 	serial0_ion();
 }
 
+void slavesensors_exitAT() 
+{
+	// Exit AT
+	serial0_sendString("ATCN");
+	serial0_sendChar(0x0D);
+	xbeeIsOk(); // wait for OK
+}
+
+// Enter AT mode. Leaves "OK" on the buffer.
+int slavesensors_enterAT() 
+{
+	// Delay guard time
+	_delay_ms(1);
+	
+	// Enter AT mode
+	serial0_sendChar('+'); // Enter AT mode
+	serial0_sendChar('+');
+	serial0_sendChar('+');
+	
+	// Wait 1ms until we get "OK"
+	int atStart = time_millis();
+	while(!serial0_hasChar()) {
+		if(time_millis() - atStart > 500) {
+			led_errorcode(ERROR_XBEETIMEOUT);
+			wdt_reset();
+			return 1;
+		}
+	};	
+	
+	return xbeeIsOk();
+
+}
+
+int xbeeIsOk() 
+{
+	char* tmppntr = serial0_readLine();
+	if(strcmp(tmppntr, "OK") == 0)
+	{
+		return 0;
+	}
+	else
+	{
+		led_errorcode(ERROR_NOXBEE);
+		return 1;
+	}
+}
+
+void slavesensors_selectnode(uint8_t nodeIndex) 
+{
+	char tmpBuf[23];
+	
+	// If we can get into AT mode
+	if(slavesensors_enterAT() == 0) {
+		
+		snprintf(tmpBuf, 23, "ATDH %s%c",slaveAddressHigh[nodeIndex], 0x0D);
+		serial0_sendString(tmpBuf);
+		
+		if(xbeeIsOk() != 0) {
+			led_errorcode(ERROR_NOXBEE);
+			return;
+		}
+		
+		snprintf(tmpBuf, 23, "ATDL %s%c",slaveAddressLow[nodeIndex], 0x0D);
+		serial0_sendString(tmpBuf);
+		
+		if(xbeeIsOk() != 0) {
+			led_errorcode(ERROR_NOXBEE);
+			return;
+		}
+		
+		slavesensors_exitAT();
+	}
+	return;
+}
+
 bool slavesensors_isrequesting() 
 {
 	return requesting;