static void *syslogger(void* arg) { va_list ap; while (1) { /* wait for a filled buffer */ buffer_msg* pbuffer = getReadBuffer(&buffers); if (likely(pbuffer)) { __vsyslogex_chk(pbuffer->prio, -1, 0, pbuffer->caller_tid, pbuffer->msg, ap); signalEmptyBufferAvailable(&buffers); } } logger = 0; pthread_exit(NULL); }
int main(void){ int log = 0; //Start Switch // DDRA = 0x00; // PORTA = 0x12; //Start PORT A for switch and IR sensors DDRA = 0xFC; PORTA = 0xFE; //LED Initial DDRC = 0x7F; PORTC = 0x7E; DDRD = 0x70; PORTD = 0x11; MotorInit(); initSerial(); char * readData = NULL; int isFinish = 0; sensorInit(); if (isCaptureMode ==1) dxl_write_byte( BROADCAST_ID, P_TORQUE_ENABLE, 0 ); while(1){ sensorTest(0); sensorTest(1); sensorTest(2); setMode(); if( checkSerialRead() > 0 ){ readData = getReadBuffer(); if( readData != NULL ){ // printf( "readData=%s\n", &readData[0] ); split( &readData[0] ); switch( serCmd[0] ){ case EVT_ACTION: ServoControl( serCmd[1] ); // setSpeedTest( serCmd[1] ); sendAck(1); break; case EVT_START_MOTION: startMotion( serCmd[1], serCmd[2] ); PORTC = ~(1 << (LED_MAX - 2)); sendAck(1); break; case EVT_STOP_MOTION: stopMotion(); sendAck(1); break; case EVT_FORCE_MOTION: forceMotion( serCmd[1], serCmd[2] ); break; case EVT_GET_NOW_ANGLE: getAngle(); break; case EVT_SET_ANGLE: setAngle(); case EVT_GET_ACT_ANGLE: if( serCmd[1] >= ACT_MAX ){ sendAck(0); }else{ sendActAngle(serCmd[1]); } break; case EVT_GET_LOAD: getLoad(); // printf( "%d\n", movingTime ); break; case EVT_GET_VOLTAGE: getVoltage(); break; case EVT_TORQUE_DISABLE: dxl_write_byte( BROADCAST_ID, P_TORQUE_ENABLE, 0 ); break; case EVT_WATCH_DOG: watchDogCnt = 0; break; case EVT_MOTION_EDIT: break; case 999: // printf( "finish\n"); sendAck(999); isFinish = 1; break; default: sendAck(0); } if( isFinish > 0 ){ MotorControl( 0, 0 ); break; } memset( readData, 0x00, SERIAL_BUFFER_SIZE ); } } memset( &serCmd[0], 0x00, sizeof(int) * SERIAL_BUFFER_SIZE ); if (~PINA & SW_START ) { if (log == 1) printf( "main() 0\n"); if( iStart > 0 ){ iStart = 0; PORTC = LED_BAT|LED_TxD|LED_RxD|LED_AUX|LED_MANAGE|LED_PROGRAM|LED_PLAY; if (isCaptureMode != 1) ServoControl( 0 ); } }else{ if( iStart == 0 ){ PORTC &= ~LED_PLAY; iStart = 1; } if( modeWait <= 0 ){ if (log == 1) printf( "main() 1\n"); setModeAction(); if (isMovetest == 1) { moveTest(); } else { move(); } }else{ if (log == 1) printf( "main() 2\n"); modeWait -= MAIN_DELAY; } } if (sensorValue[0] == 0 && sensorValueOld[0] != sensorValue[0]) { if (log == 1) printf( "### main() sensorValue[0] == 0\n"); PORTC |= LED_PROGRAM; //edit }else if (sensorValueOld[0] != sensorValue[0]){ if (log == 1) printf( "### main() sensorValue[0] == 1\n"); PORTC &= ~LED_PROGRAM; //edit } if (sensorValue[1] == 0 && sensorValueOld[1] != sensorValue[1]) { if (log == 1) printf( "### main() sensorValue[1] == 0\n"); PORTC |= LED_MANAGE; //mon }else if (sensorValueOld[1] != sensorValue[1]){ if (log == 1) printf( "### main() sensorValue[1] == 1\n"); PORTC &= ~LED_MANAGE; //mon } if (sensorValue[2] == 0 && sensorValueOld[2] != sensorValue[2]) { if (log == 1) printf( "### main() sensorValue[2] == 0\n"); PORTC |= LED_AUX; //AUX }else if (sensorValueOld[2] != sensorValue[2]){ if (log == 1) printf( "### main() sensorValue[2] == 1\n"); PORTC &= ~LED_AUX; //AUX } sensorValueOld[0] = sensorValue[0]; sensorValueOld[1] = sensorValue[1]; sensorValueOld[2] = sensorValue[2]; // walk pattern LED // brinkLED(); _delay_ms(MAIN_DELAY); watchDogCnt++; caputureCount1++; if (caputureCount1 == 25){ if (isCaptureMode == 1) getAngle(); caputureCount1 = 0; } } }
void AsioSessionState::onReflectedReadWrite( const boost::system::error_code& error, size_t bytesTransferred) { RCF2_TRACE("")(this); RCF_UNUSED_VARIABLE(error); RCF_ASSERT( mState == ReadingData || mState == ReadingDataCount || mState == WritingData) (mState); RCF_ASSERT(mReflecting); if (bytesTransferred == 0) { // Previous operation was aborted for some reason (probably because // of a thread exiting). Reissue the operation. mState = (mState == WritingData) ? ReadingData : WritingData; } if (mState == WritingData) { mState = ReadingData; std::vector<char> &readBuffer = getReadBuffer(); readBuffer.resize(8*1024); char *buffer = &readBuffer[0]; std::size_t bufferLen = readBuffer.size(); Lock lock(mMutex); if (!mHasBeenClosed) { implRead(buffer, bufferLen); } } else if ( mState == ReadingData || mState == ReadingDataCount) { mState = WritingData; std::vector<char> &readBuffer = getReadBuffer(); char *buffer = &readBuffer[0]; std::size_t bufferLen = bytesTransferred; // mReflecteePtr will be nulled in onWriteCompletion(), otherwise // we could easily end up with a cycle RCF_ASSERT(!mReflecteePtr); mReflecteePtr = mReflecteeWeakPtr.lock(); if (mReflecteePtr) { RCF_ASSERT(mReflecteePtr->mReflecting); Lock lock(mReflecteePtr->mMutex); if (!mReflecteePtr->mHasBeenClosed) { // TODO: if this can throw, then we need a scope_guard // to reset mReflecteePtr mReflecteePtr->implWrite(*this, buffer, bufferLen); } } } }
void AsioSessionState::onReadWrite( size_t bytesTransferred, const boost::system::error_code& error) { RCF_ASSERT(!error); RCF_ASSERT(!mReflecting); { switch(mState) { case ReadingDataCount: case ReadingData: RCF_ASSERT( bytesTransferred <= mReadBufferRemaining) (bytesTransferred)(mReadBufferRemaining); mReadBufferRemaining -= bytesTransferred; if (mReadBufferRemaining > 0) { invokeAsyncRead(); } else { RCF_ASSERT(mReadBufferRemaining == 0)(mReadBufferRemaining); if (mState == ReadingDataCount) { std::vector<char> &readBuffer = getReadBuffer(); RCF_ASSERT(readBuffer.size() == 4)(readBuffer.size()); unsigned int packetLength = 0; memcpy(&packetLength, &readBuffer[0], 4); networkToMachineOrder(&packetLength, 4, 1); if (packetLength <= mTransport.getMaxMessageLength()) { readBuffer.resize(packetLength); mReadBufferRemaining = packetLength; mState = ReadingData; invokeAsyncRead(); } else { sendServerError(RcfError_ServerMessageLength); } } else if (mState == ReadingData) { mState = Ready; mTransport.getSessionManager().onReadCompleted( getSessionPtr()); if (mTransport.mInterrupt) { mTransport.mInterrupt = false; mTransport.mDemuxerPtr->stop(); } } } break; case WritingData: RCF_ASSERT( bytesTransferred <= mWriteBufferRemaining) (bytesTransferred)(mWriteBufferRemaining); mWriteBufferRemaining -= bytesTransferred; if (mWriteBufferRemaining > 0) { invokeAsyncWrite(); } else { if (mCloseAfterWrite) { int fd = implGetNative(); const int BufferSize = 8*1024; char buffer[BufferSize]; while (recv(fd, buffer, BufferSize, 0) > 0); } else { mState = Ready; mSlicedWriteByteBuffers.resize(0); mWriteByteBuffers.resize(0); mTransport.getSessionManager().onWriteCompleted( getSessionPtr()); if (mTransport.mInterrupt) { mTransport.mInterrupt = false; mTransport.mDemuxerPtr->stop(); } } } break; default: RCF_ASSERT(0); } } }