Exemplo n.º 1
0
bool UdpNetworkedPhysicsProcessor::processCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
{
	if(gVerboseNetworkMessagesClient)
	{
		printf("PhysicsClientUDP::processCommand\n");
	}
//	int sz = sizeof(SharedMemoryCommand);
	
	b3Clock clock;
	double startTime = clock.getTimeInSeconds();
	double timeOutInSeconds = m_data->m_timeOutInSeconds;

	m_data->m_cs->lock();
	m_data->m_clientCmd = clientCmd;
	m_data->m_hasCommand = true;
	m_data->m_cs->unlock();

	while ((m_data->m_hasCommand) && (clock.getTimeInSeconds()-startTime < timeOutInSeconds))
	{
		b3Clock::usleep(0);
	}

#if 0


	bool hasStatus = false;

	b3Clock clock;
	double startTime = clock.getTimeInSeconds();
	double timeOutInSeconds = m_data->m_timeOutInSeconds;

	const SharedMemoryStatus* stat = 0;
	while ((!hasStatus) && (clock.getTimeInSeconds() - startTime < timeOutInSeconds))
	{
		hasStatus = receiveStatus(serverStatusOut, bufferServerToClient, bufferSizeInBytes);
		b3Clock::usleep(100);
	}
	return hasStatus;

#endif

	return false;
}
Exemplo n.º 2
0
/*********************************************************************
 ;  F U N C T I O N    D E S C R I P T I O N
 ;---------------------------------------------------------------------
 ; NAME:
 ; DESCRIPTION:
 ;  Input:
 ;  Output:
 ;  Used global variables:
 ; REMARKS when using this function:
 ;*********************************************************************/
Err_t TurnMotor(uint8_t id, uint16_t angle)
{
  Err_t err = ERROR;
  uint8_t errp, param, parcount;
  int8_t i;
  uint8_t data[] = {0xFF, 0xFF, id, 5, WRITE_DATA, GOALPOSITION_L, 0, 0, 0};
  uint16_t checksum = 0;
  
  if (angle > MAX_ANGLE || angle < MIN_ANGLE)
    return INVALID_RANGE;
  
  data[6] = (uint8_t)AngleLookupTable[angle];
  data[7] = (uint8_t)(AngleLookupTable[angle] >> 8);
  for (i = 2; i < 9; i++)
    checksum += data[i];
  data[8] = ~((uint8_t)checksum);
  
  TRAEN
  ;
  // Enable transmit
  
  for (i = 0; i < 9; i++)
  {
    err = USARTTransmit(USART_SERVO, data[i]);
    if (err != ERROR_OK)
      return err;
  }
  while (!(UCSR0A & (1 << TXC0)))
    ;
  
  err = receiveStatus(id, &errp, &param, &parcount);
  
  if (err == STATUS_ERR)
  {
    
  }
  
  return err;
}