insidem300.cc

机器人仿真软件

		// test if we are supposed to cancel
		pthread_testcancel ();
		
		// Process any pending messages
		ProcessMessages();
		
		// Interact with the device, and push out the resulting data.
		this->RefreshData ();
		
		nanosleep (&sleepTime, NULL);
	}
}

////////////////////////////////////////////////////////////////////////////////
// ProcessMessage function
int InsideM300::ProcessMessage (MessageQueue* resp_queue, 
				   player_msghdr * hdr,
				   void * data)
{	
	if (Message::MatchMessage (hdr, PLAYER_MSGTYPE_REQ, PLAYER_RFID_REQ_POWER,
		device_addr))
	{
		// Power up/down the RFID reader (NACK for now)
		Publish (device_addr, resp_queue, PLAYER_MSGTYPE_RESP_NACK, 
			 hdr->subtype);
	}
	
	else if (Message::MatchMessage (hdr, PLAYER_MSGTYPE_REQ, 
			 PLAYER_RFID_REQ_READTAG, device_addr))
	{
		// Read RFID tag data (NACK for now)
		Publish (device_addr, resp_queue, PLAYER_MSGTYPE_RESP_NACK, 
			 hdr->subtype);
	}
	
	else if (Message::MatchMessage (hdr, PLAYER_MSGTYPE_REQ, 
			 PLAYER_RFID_REQ_WRITETAG, device_addr))
	{
		// Write data to the RFID tag (NACK for now)
		Publish (device_addr, resp_queue, PLAYER_MSGTYPE_RESP_NACK, 
			 hdr->subtype);
	}
	
	else if (Message::MatchMessage (hdr, PLAYER_MSGTYPE_REQ, 
			 PLAYER_RFID_REQ_LOCKTAG, device_addr))
	{
		// Lock a RFID tag (NACK for now)
		Publish (device_addr, resp_queue, PLAYER_MSGTYPE_RESP_NACK, 
			 hdr->subtype);
	}
	
	else
	{
		return -1;
	}
	
	return 0;
}

////////////////////////////////////////////////////////////////////////////////
// RefreshData function
void InsideM300::RefreshData ()
{
	memset (&this->Data, 0, sizeof (player_rfid_data_t));
	
	// Get the time at which we started reading
	// This will be a pretty good estimate of when the phenomena occured
	struct timeval time;
	GlobalTime->GetTime(&time);
	
	// Anticollision mode
	SelectTags ();

	// Write the RFID data
	Publish (device_addr, NULL, PLAYER_MSGTYPE_DATA, PLAYER_RFID_DATA, 
			&Data, sizeof (player_rfid_data_t), NULL);
	
	return;
}

////////////////////////////////////////////////////////////////////////////////
// WriteByte function
void InsideM300::WriteByte (unsigned char buf)
{
	int wresult;
	wresult = write (this->fd, &buf, 1);
	if (wresult < 0)
		PLAYER_WARN (">> Error while writing 1 byte !");
}	

////////////////////////////////////////////////////////////////////////////////
// ReadByte function
unsigned char InsideM300::ReadByte ()
{
	unsigned char buf;
	int bresult = read (this->fd, &buf, 1);
	if (bresult == 0)
		return (0);
	return buf;
}

////////////////////////////////////////////////////////////////////////////////
// ReadStatusWord function
unsigned int InsideM300::ReadStatusWord ()
{
	unsigned int status;

	status  = (ReadByte () << 8);
	status |=  ReadByte ();

	return status;
}
////////////////////////////////////////////////////////////////////////////////
// CouplerSynchronize function
// Return 0 if synchronization succeeded, -1 otherwise.
int InsideM300::CouplerSynchronize ()
{
	unsigned int status = 0;
	int          i;

	// try a couple of times (12 attempts) to synchronize with the coupler
	for (i = 0; i < 7; i++)
	{
		WriteByte (0x00);              // send 0x00 to the coupler
		status = ReadStatusWord ();    // read the status word

		// check to see if the status word matches one of the known error codes
		if ((status == P1_P2_INCORRECT)            ||
			(status == INSTRUCTION_NOT_RECOGNIZED) ||
			(status == CLASS_NOT_RECOGNIZED)       ||
			(status == COMMAND_FLOW_INCORRECT)     ||
			(status == CARD_NOT_FOUND))
			return (0);                // synchronization succeeded
	} 

	return (-1);
}

////////////////////////////////////////////////////////////////////////////////
// SendISOCommand
unsigned int InsideM300::SendISOCommand (unsigned char typeISO, int dataOutLen,
					 unsigned char commandType, 
					 unsigned char p1,
					 unsigned char p2, unsigned char p3,
					 unsigned char *dataIn, 
					 unsigned char *dataOut)
{
	unsigned char ISOCommand[5];
	int           dataInLen = 0;

	if ((typeISO == ISO_IN) || (typeISO == ISO_IN_OUT)) 
		dataInLen = p3;

	// Prepare the command
	ISOCommand[CMD_CLASS] = 0x80;           // command class
	ISOCommand[CMD_INSTR] = commandType;    // command type
	ISOCommand[CMD_P1_P]  = p1;
	ISOCommand[CMD_P2_P]  = p2;
	ISOCommand[CMD_P3_P]  = p3;

	// Send ISO Command
	return (IsoExchange (typeISO, ISOCommand, dataInLen, dataIn, dataOutLen, 
			dataOut));
}

////////////////////////////////////////////////////////////////////////////////
// IsoExchange
unsigned int InsideM300::IsoExchange (unsigned char typeISO, 
				    unsigned char *ISOCommand,
				    int dataInLen, unsigned char *dataIn,
				    int dataOutLen, unsigned char *dataOut)
{
	// Local variable declaration
	unsigned char ACK;
	unsigned int  status;

	int i; //for counter

	// Send command
	WriteByte (ISOCommand[CMD_CLASS]);
	//printf ("Writing .%x", p_abCommand[CMD_CLASS]);
	WriteByte (ISOCommand[CMD_INSTR]);
	//printf (".%x", p_abCommand[CMD_INSTR]);
	WriteByte (ISOCommand[CMD_P1_P]);
	//printf (".%x", p_abCommand[CMD_P1_P]);
	WriteByte (ISOCommand[CMD_P2_P]);
	//printf (".%x", p_abCommand[CMD_P2_P]);
	WriteByte (ISOCommand[CMD_P3_P]);
	//printf (".%x [5 bytes]\n", p_abCommand[CMD_P3_P]);

	switch (typeISO)
	{
		case ISO_NONE :                     // Neither data sent nor received
			return ReadStatusWord ();

		case ISO_IN :
		// Verify ACK
			ACK = ReadByte ();
			if (ACK != ISOCommand[CMD_INSTR])
			{
				status = (ACK<<8);
				status = status | ReadByte ();
				return status;
			}
			for (i = 0; i < dataInLen ; i++)
			{
				WriteByte (dataIn[i]);      // Send buffer data
			}
			return ReadStatusWord ();

		case ISO_OUT :
		// Verify ACK
			ACK = ReadByte ();
			if (ACK != ISOCommand[CMD_INSTR])
			{
				status  = (ACK << 8);
				status |= ReadByte ();
				return status;
			}
			for (i = 0; i < dataOutLen; i++)
				dataOut[i] = ReadByte ();   // Grab buffer data
			return ReadStatusWord ();

		case ISO_IN_OUT :
		// Verify ACK
			ACK = ReadByte ();
			if (ACK != ISOCommand[CMD_INSTR])
			{
				status = (ACK<<8);
				status = status | ReadByte ();
				return status;
			}
			for (i = 0; i < dataInLen ; i++)
				WriteByte (dataIn[i]);      // Send buffer data in
		// Verify ACK
			ACK = ReadByte ();
			if (ACK != ISOCommand[CMD_INSTR])
			{
				status = (ACK<<8);
				status = status | ReadByte ();
				return status;
			}
			for (i = 0; i < dataOutLen; i++)
				dataOut[i] = ReadByte ();   // Grab buffer data out
			return ReadStatusWord();
		default: 
			return (0);
	}
}

////////////////////////////////////////////////////////////////////////////////
// ReadMultipleTags function
int InsideM300::SelectTags ()
{
	unsigned int  status, iStatus;
	unsigned char chipAnswer[24], globalChipAnswer[24];
	unsigned char ISOCommand[10];
	int           chipMask;
	
	status = ResetField ();
	if (status != STATUS_OK)
		PLAYER_WARN1 (">> Error 0x%x while resetting field !", status);
	
	ISOCommand[0] = 0x36; ISOCommand[1] = 0x01;
	ISOCommand[2] = 0x00; ISOCommand[3] = 0x00;
	// 0xC2 = TRANSMIT - Basic inventory command
	iStatus = SendISOCommand (ISO_IN, 0, 0xC2, 0xF3, 0x0A, 0x04, ISOCommand, 
				  chipAnswer);
	
	int tagsFound = 1;
	while (tagsFound != 0) {
		
		ISOCommand[0] = 0x00;
		// 0xC0 = GET_RESPONSE - get chip UID
		status = SendISOCommand (ISO_OUT, 10, 0xC0, 0x00, 0x00, 0x0A, 
					 ISOCommand, chipAnswer);
		chipMask = chipAnswer[2];
		
		switch (iStatus) {
			case STATUS_OK:
			{
				this->Data.tags[this->Data.tags_count].type = chipAnswer[1];
				this->Data.tags[this->Data.tags_count].guid_count = 8;
				int j;
				for (j = 0; j < 8; j++)
					this->Data.tags[this->Data.tags_count].guid[j] = 
							chipAnswer [9-j];
				this->Data.tags_count++;
			
				tagsFound = 0;
				break;
			}
			case CARD_NOT_IDENTIFIED:
			{
				InsideInventory (1, chipMask, globalChipAnswer);
			
				memset (&ISOCommand, 0,sizeof (ISOCommand));
				int k;
				for (k = 2; k < 10; k++)
					ISOCommand[k] = globalChipAnswer[k-2];
			
				ISOCommand[0] = 0x20; ISOCommand[1] = 0x02;
				// 0xC2 = TRANSMIT (Note: get Chip Answer)
				status = SendISOCommand (ISO_IN, 0, 0xC2, 0xB3, 0x00, 0x0A, 
							 ISOCommand, chipAnswer);
				memset (&ISOCommand, 0,sizeof (ISOCommand));
				ISOCommand[0] = 0x36; ISOCommand[1] = 0x01;
				ISOCommand[2] = 0x00; ISOCommand[3] = 0x00;
				// 0xC2 = TRANSMIT (Note: get Chip Answer)
				iStatus = SendISOCommand (ISO_IN, 0, 0xC2, 0xF3, 0x0A, 0x04, 
							  ISOCommand, chipAnswer);
				break;
			}
			default:
			{
				tagsFound = 0;
				break;
			}
		}
	}
	return (0);
}

////////////////////////////////////////////////////////////////////////////////
// InsideInventory function
void InsideM300::InsideInventory (int maskLength, int chipMask, 
				  unsigned char *globalChipAnswer)
{
	unsigned int  status, iStatus;
	unsigned char ISOCommand[5];
	unsigned char chipAnswer[24];
	int i;
	
	if (maskLength > 15)
		return;
	
	ISOCommand[0] = 0x36; ISOCommand[1] = 0x01;
	ISOCommand[2] = 0x00; ISOCommand[3] = 0x00 + maskLength;
	ISOCommand[4] = 0x00 + chipMask;
	
	// Masked Inventory command
	iStatus = SendISOCommand (ISO_IN, 0, 0xC2, 0xF3, 0x0A, 0x05, ISOCommand, 
				  chipAnswer);
	// Get UID
	status  = SendISOCommand (ISO_OUT, 10, 0xC0, 0x00, 0x00, 0x0A, NULL, 
				  chipAnswer);

	chipMask = chipAnswer[2];

	switch (iStatus)
	{
		case STATUS_OK:
		{
			for (i = 0; i < 8; i++)
				globalChipAnswer[i] = chipAnswer[i+2];
			
			this->Data.tags[this->Data.tags_count].type = chipAnswer[1];
			this->Data.tags[this->Data.tags_count].guid_count = 8;
			int j;
			for (j = 0; j < 8; j++)
				this->Data.tags[this->Data.tags_count].guid[j] = chipAnswer [9-j];
			this->Data.tags_count++;
			
			break;
		}
		case CARD_NOT_IDENTIFIED:
		{
			InsideInventory (maskLength + 1, chipMask, globalChipAnswer);
			break;
		}
	}
}

////////////////////////////////////////////////////////////////////////////////
// ResetField function
unsigned int InsideM300::ResetField ()
{
	unsigned char ISOCommand[5];
	unsigned char dataIn[2] = {0x00, 0x00}; // Data in initialisation

	// Prepare the command
	ISOCommand[CMD_CLASS] = 0x80;
	ISOCommand[CMD_INSTR] = 0xF4;
	ISOCommand[CMD_P1_P]  = 0x40;
	ISOCommand[CMD_P2_P]  = 0x00;
	ISOCommand[CMD_P3_P]  = 0x01;

	// Send ISO Command
	return (IsoExchange (ISO_IN, ISOCommand, 1, dataIn, 0, NULL));
}