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);
}
}
}
