diff --git a/main.cpp b/main.cpp
index 17aa44f..bd6eb26 100644
--- a/main.cpp
+++ b/main.cpp
@@ -216,7 +216,6 @@ int powerDispatch(char* token, Serial* serial)
 	return -1;
 }
 
-
 void serialDispatch(Serial* serial)
 {
 	static uint8_t lastItemToResend = 0;
diff --git a/mfrc522.cpp b/mfrc522.cpp
index 3e44f72..ee64c2b 100644
--- a/mfrc522.cpp
+++ b/mfrc522.cpp
@@ -98,8 +98,17 @@ uint8_t Mfrc522::calculateCrc(uint8_t *data, uint8_t length, uint16_t *result)
 		return ERR;
 	write(CommandReg, IDLE);
 	
+	if(serial)
+	{
+		serial->write_p(PSTR("Calculated CRC "));
+		serial->write((int)read(CRCResultRegL));
+		serial->putChar(' ');
+		serial->write((int)read(CRCResultRegH));
+		serial->putChar('\n');
+	}
+	
 	*result = read(CRCResultRegL);
-	*result = read(CRCResultRegH) << 8;
+	*result |= read(CRCResultRegH) << 8;
 	return 0;
 }
 
@@ -439,7 +448,7 @@ uint8_t Mfrc522::selectTag(Uid *uid)
 	} // End of while ( ! uidComplete)
 	
 	// Set correct uid->size
-	uid->size = 3 * cascadeLevel + 2;
+	uid->size = 3 * (cascadeLevel+1) + 1;
 
 	return 0;
 }
diff --git a/nfcbord.cpp b/nfcbord.cpp
index a25dcb1..fd86b05 100644
--- a/nfcbord.cpp
+++ b/nfcbord.cpp
@@ -48,46 +48,7 @@ void NfcBoard::printNfcDevices(Serial* serial)
 
 int NfcBoard::dispatch(char* inBuffer, Serial* serial)
 {
-	if(strcmp(inBuffer, "tsta") == 0 )
-	{
-		serial->write_p(PSTR("Runing shift test\n"));
-		while(!serial->dataIsWaiting())
-		{
-			csReg.clear(false);
-			_delay_us(100);
-		}
-		serial->write_p(PSTR("Finished\n"));
-		csReg.clear(true);
-		return 0;
-	}
-	else if(strcmp(inBuffer, "tstb") == 0 )
-	{
-		serial->write_p(PSTR("Runing input shift test b\n"));
-		while(!serial->dataIsWaiting())
-		{
-			serial->write((int)(*irqReg.read()));
-			serial->putChar('\n');
-			_delay_us(100);
-		}
-		serial->write_p(PSTR("Finished\n"));
-		return 0;
-	}
-	else if(strcmp(inBuffer, "tstc") == 0 )
-	{
-		serial->write_p(PSTR("Runing spi test\n"));
-		while(!serial->dataIsWaiting())
-		{
-			csReg.setBit(1, false);
-			spim.readWrite((0x37 << 1) | (1 << 7));
-			serial->write((int)spim.readWrite());
-			serial->putChar('\n');
-			csReg.setBit(1, true);
-			_delay_us(100);
-		}
-		serial->write_p(PSTR("Finished\n"));
-		return 0;
-	}
-	else if(strcmp(inBuffer, "tstd") == 0 )
+	else if(strcmp(inBuffer, "detect") == 0 )
 	{
 		serial->write_p(PSTR("Runing tag detection test\n"));
 		bool oldPresent = false;
@@ -98,10 +59,8 @@ int NfcBoard::dispatch(char* inBuffer, Serial* serial)
 			{
 				oldPresent = present;
 
-				serial->write_p(PSTR("Tag found, selecting\n"));
-
 				Mfrc522::Uid uid;
-				Mfrc522::serial = serial;
+				//Mfrc522::serial = serial;
 				uint8_t res = readers[0].selectTag(&uid);
 				Mfrc522::serial = nullptr;
 				if(res != 0)
@@ -111,9 +70,14 @@ int NfcBoard::dispatch(char* inBuffer, Serial* serial)
 					serial->putChar('\n');
 					continue;
 				}
-				/*snprintf(buffer, SNPRINTF_BUFFER_SIZE, "Uid: %x %x %x %x %x %x %x %x %x %x\n",
-						 uid.uidByte[0], uid.uidByte[1], uid.uidByte[2], uid.uidByte[3], uid.uidByte[4],
-						 uid.uidByte[5], uid.uidByte[6], uid.uidByte[7], uid.uidByte[8], uid.uidByte[9]);*/
+				serial->write_p(PSTR("Uid: "));
+				for(uint8_t i = 0; i < uid.size; ++i)
+				{
+					serial->write((int)uid.uidByte[i]);
+					serial->putChar(':');
+				}
+				serial->putChar('\n');
+				break;
 			}
 			else if(!present && oldPresent)
 			{