// this sets the property of a puck to a value osaPuck::Errno osaPuck::SetProperty( Barrett::ID propid, Barrett::Value propval, bool verify){ // empty CAN frame osaCANBusFrame frame; // pack the property ID and value in a "set" CAN frame if( PackProperty( frame, Barrett::SET, propid, propval )!=osaPuck::ESUCCESS ){ CMN_LOG_RUN_WARNING << LogPrefix() << "Failed to pack property " << propid << std::endl; return osaPuck::EFAILURE; } // send the CAN frame if( canbus->Send( frame ) != osaCANBus::ESUCCESS ){ CMN_LOG_RUN_ERROR << LogPrefix() << "Failed to send the CAN frame." << std::endl; return osaPuck::EFAILURE; } // do we double check that the value was set? if( verify ){ // If we just changed the status of the puck, give it a bit of time to // initialize itself if( propid == Barrett::STATUS && propval == osaPuck::STATUS_READY ) osaSleep( 1.0 ); else osaSleep( 0.01 ); // query the puck to make sure that the property is set Barrett::Value recvpropval = rand(); if( GetProperty( propid, recvpropval ) != osaPuck::ESUCCESS ){ CMN_LOG_RUN_WARNING << LogPrefix()<<"Failed to get puck property" << std::endl; return osaPuck::EFAILURE; } if( propval != recvpropval ){ CMN_LOG_RUN_WARNING << LogPrefix() << "Oop! Unexpected property value. " << "Expected " << propval << " got " << recvpropval << std::endl; return osaPuck::EFAILURE; } } return osaPuck::ESUCCESS; }
// Get a property from the puck. this sends a query to the puck and wait for // its reply osaPuck::Errno osaPuck::GetProperty( Barrett::ID propid, Barrett::Value& propvalue ){ // empty CAN frame osaCANBusFrame sendframe; // pack the query in a can frame if( PackProperty( sendframe, Barrett::GET, propid ) != osaPuck::ESUCCESS ){ CMN_LOG_RUN_ERROR << LogPrefix() << "Failed to pack property" << std::endl; return osaPuck::EFAILURE; } // send the CAN frame if( canbus->Send( sendframe ) != osaCANBus::ESUCCESS ){ CMN_LOG_RUN_ERROR << LogPrefix() << "Failed to querry puck" << std::endl; return osaPuck::EFAILURE; } // empty CAN frame osaCANBusFrame recvframe; // receive the response in a CAN frame if( canbus->Recv( recvframe ) != osaCANBus::ESUCCESS ){ CMN_LOG_RUN_ERROR << LogPrefix() << "Failed to receive property" << std::endl; return osaPuck::EFAILURE; } // unpack the can frame Barrett::ID recvpropid; if(UnpackCANFrame( recvframe, recvpropid, propvalue ) != osaPuck::ESUCCESS){ CMN_LOG_RUN_ERROR << LogPrefix() << "Failed to unpack CAN frame." << std::endl; return osaPuck::EFAILURE; } // make sure that the property received is the one we asked for if( propid != recvpropid ){ CMN_LOG_RUN_ERROR << LogPrefix() << "Oop! Unexpected property ID. " << "Expected " << propid << " got " << recvpropid << std::endl; return osaPuck::EFAILURE; } return osaPuck::ESUCCESS; }
void Log(char * message) { if (logFile != NULL) { LogPrefix(); fprintf(logFile, message); fprintf(logFile, "\n"); fflush(logFile); } }
void LogFormatted(char * message, ...) { if (logFile != NULL) { va_list varargs; va_start(varargs, message); LogPrefix(); vfprintf(logFile, message, varargs); fprintf(logFile, "\n"); fflush(logFile); } }