C++ (Cpp) dxl_hal_set_timeout Examples

Example #1

File: dynamixel_syncread.c Project: MathieuR/ros-bioloid

void dxl_tx_packet()
{
	unsigned char i;
	unsigned char TxNumByte, RealTxNumByte;
	unsigned char checksum = 0;

	if( giBusUsing == 1 )
		return;
	
	giBusUsing = 1;

	if( gbInstructionPacket[LENGTH] > (MAXNUM_TXPARAM+2) )
	{
		gbCommStatus = COMM_TXERROR;
		giBusUsing = 0;
		return;
	}
	
	if( gbInstructionPacket[INSTRUCTION] != INST_PING
		&& gbInstructionPacket[INSTRUCTION] != INST_READ
		&& gbInstructionPacket[INSTRUCTION] != INST_WRITE
		&& gbInstructionPacket[INSTRUCTION] != INST_REG_WRITE
		&& gbInstructionPacket[INSTRUCTION] != INST_ACTION
		&& gbInstructionPacket[INSTRUCTION] != INST_RESET
		&& gbInstructionPacket[INSTRUCTION] != INST_SYNC_WRITE
		&& gbInstructionPacket[INSTRUCTION] != INST_SYNC_READ)
	{
		gbCommStatus = COMM_TXERROR;
		giBusUsing = 0;
		return;
	}
	
	gbInstructionPacket[0] = 0xff;
	gbInstructionPacket[1] = 0xff;
	for( i=0; i<(gbInstructionPacket[LENGTH]+1); i++ )
		checksum += gbInstructionPacket[i+2];
	gbInstructionPacket[gbInstructionPacket[LENGTH]+3] = ~checksum;
	
	if( gbCommStatus == COMM_RXTIMEOUT || gbCommStatus == COMM_RXCORRUPT )
		dxl_hal_clear();

	TxNumByte = gbInstructionPacket[LENGTH] + 4;
	RealTxNumByte = dxl_hal_tx( (unsigned char*)gbInstructionPacket, TxNumByte );

	if( TxNumByte != RealTxNumByte )
	{
		gbCommStatus = COMM_TXFAIL;
		giBusUsing = 0;
		return;
	}

	if( gbInstructionPacket[INSTRUCTION] == INST_READ )
		dxl_hal_set_timeout( gbInstructionPacket[PARAMETER+1] + 6 );
	else if ( gbInstructionPacket[INSTRUCTION] == INST_SYNC_READ )
        dxl_hal_set_timeout( gbInstructionPacket[PARAMETER+1] + 6 );
    else
		dxl_hal_set_timeout( 6 );

	gbCommStatus = COMM_TXSUCCESS;
}

Example #2

File: communications.cpp Project: ccccjason/IEEE2015

// Caller is resonsible for the received_data buffer's appropriate size.
// Returns the total numberSet bytes received for packet.
// If receive timed out, value of num_bytes_received is undefined.
bool RXPacket(uint8_t* received_data, int packet_length, int* num_bytes_received) {
  dxl_hal_set_timeout(packet_length);

  // Repeatedly attempt to read until either we have all of the bytes
  // requested or the timeout expires.
  // TODO replace this logic with something that isn't so CPU intensive.
  // like interrupt driven...
  int bytes_read = 0; 
  while (bytes_read < packet_length) {
    if (dxl_hal_timeout() == 1) {
      return false;
    }
    assert(packet_length < MAX_PACKET_BYTES);
    bytes_read += dxl_hal_rx(&(received_data[bytes_read]), packet_length);
  }

  // PrintPacket(data, num_bytes);

  // Now attempt to match the header to the data received.
  int header_match_count = 0;
  int buffer_index = 0;
  while (header_match_count < HEADER_SIZE) {
    if (buffer_index >= bytes_read) {
      return false;
    }
    if (received_data[buffer_index++] == HEADER[header_match_count]) {
      header_match_count++;
    } else {
      header_match_count = 0;
    }
  }

  // Now, the header is matched
  // Move bytes down to the beginning of the data buffer.
  int packet_start_index = buffer_index - HEADER_SIZE;
  bytes_read -= packet_start_index;
  memmove(received_data, &(received_data[packet_start_index]), bytes_read);

  // Now read the remaining bytes if necessary.
  while (bytes_read < packet_length) {
    assert((packet_length - bytes_read) < MAX_PACKET_BYTES);
    bytes_read += dxl_hal_rx(&(received_data[bytes_read]), packet_length - bytes_read);
    if (dxl_hal_timeout() == 1) {
      return false;
    }
  }

  uint16_t expected_crc = UpdateCRC(0, received_data, bytes_read - CHECKSUM_SIZE);
  uint16_t received_crc = MakeWord(received_data[bytes_read - 2], received_data[bytes_read - 1]);
  if (expected_crc != received_crc) {
    // CRC check failed.
    return false;
  }

  *num_bytes_received = bytes_read;
  return true;
}

Example #3

File: dynamixel.cpp Project: KurtE/Raspberry_Pi

void dxl_tx_packet()
{
	unsigned char i;
	unsigned char TxNumByte, RealTxNumByte;
	unsigned char checksum = 0;

	if( giBusUsing == 1 )
		return;
	
	giBusUsing = 1;

	if( gbInstructionPacket[LENGTH] > (MAXNUM_TXPARAM+2) )
	{
		gbCommStatus = COMM_TXERROR;
		giBusUsing = 0;
		return;
	}
	
	if( gbInstructionPacket[INSTRUCTION] != INST_PING
		&& gbInstructionPacket[INSTRUCTION] != INST_READ
		&& gbInstructionPacket[INSTRUCTION] != INST_WRITE
		&& gbInstructionPacket[INSTRUCTION] != INST_REG_WRITE
		&& gbInstructionPacket[INSTRUCTION] != INST_ACTION
		&& gbInstructionPacket[INSTRUCTION] != INST_RESET
		&& gbInstructionPacket[INSTRUCTION] != INST_SYNC_WRITE 
		&& gbInstructionPacket[INSTRUCTION] != INST_SYNC_READ )
	{
		gbCommStatus = COMM_TXERROR;
		giBusUsing = 0;
		return;
	}
	
	gbInstructionPacket[0] = 0xff;
	gbInstructionPacket[1] = 0xff;
	for( i=0; i<(gbInstructionPacket[LENGTH]+1); i++ )
		checksum += gbInstructionPacket[i+2];
	gbInstructionPacket[gbInstructionPacket[LENGTH]+3] = ~checksum;
	
	if( gbCommStatus == COMM_RXTIMEOUT || gbCommStatus == COMM_RXCORRUPT )
		dxl_hal_clear();

	TxNumByte = gbInstructionPacket[LENGTH] + 4;
#ifdef DEBUG_PRINT
	printf("TX(%d):", TxNumByte);
	for (int i=0; i < TxNumByte; i++) 
	{
		printf(" %2x", gbInstructionPacket[i]);
		if ((i & 0xff) == 0xff)
			printf("\n");
	}        
#endif    
	RealTxNumByte = dxl_hal_tx( (unsigned char*)gbInstructionPacket, TxNumByte );

#ifdef DEBUG_PRINT
	printf(" - %d\n", RealTxNumByte);
#endif
	if( TxNumByte != RealTxNumByte )
	{
		gbCommStatus = COMM_TXFAIL;
		giBusUsing = 0;
		return;
	}

	if( gbInstructionPacket[INSTRUCTION] == INST_READ )
		dxl_hal_set_timeout( gbInstructionPacket[PARAMETER+1] + 6 );
	else if( gbInstructionPacket[INSTRUCTION] == INST_SYNC_READ )   // BUGBUG:: probably need to expand more here
		dxl_hal_set_timeout((gbInstructionPacket[LENGTH] - 4) * gbInstructionPacket[PARAMETER+1] + 6 );
	else
		dxl_hal_set_timeout( 6 );

	gbCommStatus = COMM_TXSUCCESS;
}

Example #4

File: dynamixel.c Project: dsapandora/RoboFight

// Send an instruction packet
void dxl_tx_packet()
{
	unsigned char i;
	unsigned char TxNumByte, RealTxNumByte;
	unsigned char checksum = 0;

	// do nothing if bus is busy
	if( giBusUsing == 1 )
		return;
	
	// set bus as busy
	giBusUsing = 1;

	// check packet does not exceed maximum number of parameters
	if( gbInstructionPacket[LENGTH] > (MAXNUM_TXPARAM+2) )
	{
		gbCommStatus = COMM_TXERROR;
		giBusUsing = 0;
		return;
	}
	
	// check instruction is valid
	if( gbInstructionPacket[INSTRUCTION] != INST_PING
		&& gbInstructionPacket[INSTRUCTION] != INST_READ
		&& gbInstructionPacket[INSTRUCTION] != INST_WRITE
		&& gbInstructionPacket[INSTRUCTION] != INST_REG_WRITE
		&& gbInstructionPacket[INSTRUCTION] != INST_ACTION
		&& gbInstructionPacket[INSTRUCTION] != INST_RESET
		&& gbInstructionPacket[INSTRUCTION] != INST_SYNC_WRITE )
	{
		gbCommStatus = COMM_TXERROR;
		giBusUsing = 0;
		return;
	}
	
	// create the packet header (2x 0xFF)
	gbInstructionPacket[0] = 0xff;
	gbInstructionPacket[1] = 0xff;
	// calculate the checksum
	for( i=0; i<(gbInstructionPacket[LENGTH]+1); i++ )
		checksum += gbInstructionPacket[i+2];
	gbInstructionPacket[gbInstructionPacket[LENGTH]+3] = ~checksum;
	
	// if timeout or corrupt clear the buffer
	if( gbCommStatus == COMM_RXTIMEOUT || gbCommStatus == COMM_RXCORRUPT )
		dxl_hal_clear();

	// transfer the packet
	TxNumByte = gbInstructionPacket[LENGTH] + 4;
	RealTxNumByte = dxl_hal_tx( (unsigned char*)gbInstructionPacket, TxNumByte );

	// check that all bytes were sent 
	if( TxNumByte != RealTxNumByte )
	{
		// communication failure - not all bytes sent
		gbCommStatus = COMM_TXFAIL;
		giBusUsing = 0;
		return;
	}

	// for read instructions we expect a reply within the timeout period
	if( gbInstructionPacket[INSTRUCTION] == INST_READ )
		dxl_hal_set_timeout( gbInstructionPacket[PARAMETER+1] + 6 );
	else
		dxl_hal_set_timeout( 6 );

	gbCommStatus = COMM_TXSUCCESS;
}