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; }
/********************************************************************* ; 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, ¶m, &parcount); if (err == STATUS_ERR) { } return err; }