robodyn_mechanisms: JointControlActualGripper.cpp Source File

Go to the documentation of this file.
00001 
00005 #include "robodyn_mechanisms/JointControlActualGripper.h"
00006 /***************************************************************************/
00011 JointControlActualGripper::JointControlActualGripper(const std::string& mechanism, IoFunctions ioFunctions, NodeRegisterManagerPtr nodeRegisterManager)
00012     : JointControlActualInterface(mechanism, ioFunctions), nodeRegisterManager(nodeRegisterManager), logCategory("gov.nasa.JointControlActualGripper")
00013 {
00014     setParameters();
00015 }
00016 
00017 JointControlActualGripper::~JointControlActualGripper()
00018 {
00019     // Nothing
00020 }
00021 
00022 void JointControlActualGripper::setParameters()
00023 {
00024     std::string parameterFile = io.getControlFile(mechanism);
00025 
00027     TiXmlDocument file(parameterFile.c_str());
00028     bool loadOkay = file.LoadFile();
00029 
00030     if (!loadOkay)
00031     {
00032         std::stringstream err;
00033         err << "Failed to load file [" << parameterFile << "]";
00034         NasaCommonLogging::Logger::log(logCategory, log4cpp::Priority::FATAL, err.str());
00035         throw std::runtime_error(err.str());
00036     }
00037 
00038     TiXmlHandle doc(&file);
00039     NasaCommonLogging::Logger::log(logCategory, log4cpp::Priority::INFO, "File [" + parameterFile + "] successfully loaded.");
00040 
00041     // Print the XML file's contents
00042     //cout << "Successfully loaded file: " << parameterFile << endl;
00043     //TiXmlPrinter printer;
00044     //printer.SetIndent("\t");
00045     //file.Accept(&printer);
00046     //cout << printer.Str() << endl;
00047 
00048     // Check for ApiMap
00049     TiXmlHandle parametersElement(doc.FirstChildElement("ApiMap"));
00050 
00051     if (parametersElement.ToElement())
00052     {
00053         // Status bit names
00054         ProcAliveStatusName       = ApiMap::getXmlElementValue(parametersElement, "ProcAliveStatus");
00055         CommAliveStatusName       = ApiMap::getXmlElementValue(parametersElement, "CommAliveStatus");
00056         JointFaultStatusName      = ApiMap::getXmlElementValue(parametersElement, "JointFaultStatus");
00057         CoeffsLoadedStatusName    = ApiMap::getXmlElementValue(parametersElement, "CoeffsLoadedStatus");
00058         MotorEnableStatusName     = ApiMap::getXmlElementValue(parametersElement, "MotorEnableStatus");
00059         BrakeReleaseStatusName    = ApiMap::getXmlElementValue(parametersElement, "BrakeReleaseStatus");
00060         BridgeEnableStatusName    = ApiMap::getXmlElementValue(parametersElement, "BridgeEnableStatus");
00061         MotComSourceStatusName    = ApiMap::getXmlElementValue(parametersElement, "MotComSourceStatus");
00062         ControlModeStatusName     = ApiMap::getXmlElementValue(parametersElement, "ControlModeStatus");
00063         CalibrationModeStatusName = ApiMap::getXmlElementValue(parametersElement, "CalibrationModeStatus");
00064         ClearFaultStatusName      = ApiMap::getXmlElementValue(parametersElement, "ClearFaultStatus");
00065         NotInBootloaderStatusName = ApiMap::getXmlElementValue(parametersElement, "NotInBootloaderStatus");
00066         BridgeFaultStatusName     = ApiMap::getXmlElementValue(parametersElement, "BridgeFaultStatus");
00067         BusVoltageFaultStatusName = ApiMap::getXmlElementValue(parametersElement, "BusVoltageFaultStatus");
00068         ApsFaultStatusName        = ApiMap::getXmlElementValue(parametersElement, "ApsFaultStatus");
00069         Aps1TolFaultStatusName    = ApiMap::getXmlElementValue(parametersElement, "Aps1TolFaultStatus");
00070         Aps2TolFaultStatusName    = ApiMap::getXmlElementValue(parametersElement, "Aps2TolFaultStatus");
00071         EncDriftFaultStatusName   = ApiMap::getXmlElementValue(parametersElement, "EncDriftFaultStatus");
00072         VelocityFaultStatusName   = ApiMap::getXmlElementValue(parametersElement, "VelocityFaultStatus");
00073         LimitFaultStatusName      = ApiMap::getXmlElementValue(parametersElement, "LimitFaultStatus");
00074         CurrentFaultStatusName    = ApiMap::getXmlElementValue(parametersElement, "CurrentFaultStatus");
00075         ATIStaticFaultStatusName  = ApiMap::getXmlElementValue(parametersElement, "ATIStaticFaultStatus");
00076     }
00077     else
00078     {
00079         std::stringstream err;
00080         err << "The file " << parameterFile << " has no element named [ApiMap]";
00081         NasaCommonLogging::Logger::log("gov.nasa.JointControlActualFsmGripper", log4cpp::Priority::ERROR, err.str());
00082         throw std::runtime_error(err.str());
00083     }
00084 }
00085 
00086 /***************************************************************************/
00092 void JointControlActualGripper::getStates(r2_msgs::JointControlData& actualStates)
00093 {
00094     updateCommandModeState(); // Must come before updateControlModeState()
00095     updateControlModeState();
00096     updateCalibrationModeState();
00097     updateClearFaultModeState();
00098     updateCoeffState();
00099     actualStates = states;
00100 }
00101 /***************************************************************************/
00106 void JointControlActualGripper::updateControlModeState(void)
00107 {
00108     // Check for FPGA FAULTED bits
00109     if (nodeRegisterManager->getStatusValue(mechanism, ProcAliveStatusName)   == 0)
00110     {
00111         if (states.controlMode.state != r2_msgs::JointControlMode::FAULTED)
00112         {
00113             NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::DEBUG << "Proccessor not alive .. automatic transition to JointControlMode::FAULTED on joint: " << mechanism;
00114             states.controlMode.state = r2_msgs::JointControlMode::FAULTED;
00115         }
00116 
00117         return;
00118     }
00119 
00120     if (nodeRegisterManager->getStatusValue(mechanism, CommAliveStatusName)   == 0)
00121     {
00122         if (states.controlMode.state != r2_msgs::JointControlMode::FAULTED)
00123         {
00124             NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::INFO << "Comm stats not alive.. automatic transition to JointControlMode::FAULTED on joint: " << mechanism;
00125             states.controlMode.state = r2_msgs::JointControlMode::FAULTED;
00126         }
00127 
00128         return;
00129     }
00130 
00131     // Check for BOOTLOADER state
00132     if (nodeRegisterManager->getStatusValue(mechanism, NotInBootloaderStatusName) == 0)
00133     {
00134         if (states.controlMode.state != r2_msgs::JointControlMode::BOOTLOADER)
00135         {
00136             NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::DEBUG << "Transition to JointControlMode::BOOTLOADER on joint: " << mechanism;
00137             states.controlMode.state = r2_msgs::JointControlMode::BOOTLOADER;
00138         }
00139 
00140         return;
00141     }
00142 
00143     // Check for processor FAULTED bits
00144     if (nodeRegisterManager->getStatusValue(mechanism, JointFaultStatusName) == 1)
00145     {
00146         if (states.controlMode.state != r2_msgs::JointControlMode::FAULTED)
00147         {
00148             NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::INFO << "Transition to JointControlMode::FAULTED on joint: " << mechanism;
00149             states.controlMode.state = r2_msgs::JointControlMode::FAULTED;
00150         }
00151 
00152         return;
00153     }
00154 
00155     uint16_t currentMotorEnable  = nodeRegisterManager->getStatusValue(mechanism, MotorEnableStatusName);
00156     uint16_t currentBrakeRelease = nodeRegisterManager->getStatusValue(mechanism, BrakeReleaseStatusName);
00157     uint16_t currentBridgeEnable = nodeRegisterManager->getStatusValue(mechanism, BridgeEnableStatusName);
00158 
00159 //    if(currentBridgeEnable == 0)
00160 //    {
00161 //      if(states.controlMode.state != r2_msgs::JointControlMode::INVALID)
00162 //        {
00163 //            NasaCommonLogging::Logger::getCategory("gov.nasa.JointControlActualFsmSeriesElastic") << log4cpp::Priority::INFO << mechanism << ": INVALID CONTROL MODE STATE DETECTED";
00164 //            NasaCommonLogging::Logger::getCategory("gov.nasa.JointControlActualFsmSeriesElastic") << log4cpp::Priority::DEBUG << "Transition to JointControlMode::INVALID on joint: " << mechanism;
00165 //            states.controlMode.state = r2_msgs::JointControlMode::INVALID;
00166 //        }
00167 //        return;
00168 //  }
00169 
00170     // Check for OFF
00171     if ( (currentMotorEnable  == 0) &&
00172             (currentBridgeEnable == 1) &&
00173             (currentBrakeRelease == 0) )
00174     {
00175         if (states.controlMode.state != r2_msgs::JointControlMode::OFF)
00176         {
00177             NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::DEBUG << "Transition to JointControlMode::OFF on joint: " << mechanism;
00178             states.controlMode.state = r2_msgs::JointControlMode::OFF;
00179         }
00180 
00181         return;
00182     }
00183 
00184     // Check for PARK
00185     if ( (currentMotorEnable  == 1) &&
00186             (currentBridgeEnable == 1) &&
00187             (currentBrakeRelease == 0) )
00188     {
00189         if (states.controlMode.state != r2_msgs::JointControlMode::PARK)
00190         {
00191             NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::DEBUG << "Transition to JointControlMode::PARK on joint: " << mechanism;
00192             states.controlMode.state = r2_msgs::JointControlMode::PARK;
00193         }
00194 
00195         return;
00196     }
00197 
00198     // Check for NEUTRAL
00199     if ( (currentMotorEnable  == 0) &&
00200             (currentBridgeEnable == 1) &&
00201             (currentBrakeRelease == 1) )
00202     {
00203         if (states.controlMode.state != r2_msgs::JointControlMode::NEUTRAL)
00204         {
00205             NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::DEBUG << "Transition to JointControlMode::NEUTRAL on joint: " << mechanism;
00206             states.controlMode.state = r2_msgs::JointControlMode::NEUTRAL;
00207         }
00208 
00209         return;
00210     }
00211 
00212     // Check for DRIVE
00213     if ( (currentMotorEnable  == 1) &&
00214             (currentBridgeEnable == 1) &&
00215             (currentBrakeRelease == 1) )
00216     {
00217         if (states.controlMode.state != r2_msgs::JointControlMode::DRIVE)
00218         {
00219             NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::DEBUG << "Transition to JointControlMode::DRIVE on joint: " << mechanism;
00220             states.controlMode.state = r2_msgs::JointControlMode::DRIVE;
00221         }
00222 
00223         return;
00224     }
00225 
00226     // All valid states have been checked, what's left is invalid
00227     if (states.controlMode.state != r2_msgs::JointControlMode::INVALID)
00228     {
00229         NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::INFO << "Transition to JointControlMode::INVALID on joint: " << mechanism;
00230         states.controlMode.state = r2_msgs::JointControlMode::INVALID;
00231     }
00232 }
00233 /***************************************************************************/
00238 void JointControlActualGripper::updateCommandModeState(void)
00239 {
00240     // Check for MOTCOM
00241     if (nodeRegisterManager->getStatusValue(mechanism, MotComSourceStatusName) == 0)
00242     {
00243         if (states.commandMode.state != r2_msgs::JointControlCommandMode::MOTCOM)
00244         {
00245             NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::DEBUG << "Transition to JointControlCommandMode::MOTCOM on joint: " << mechanism;
00246             states.commandMode.state = r2_msgs::JointControlCommandMode::MOTCOM;
00247         }
00248 
00249         return;
00250     }
00251 
00252     uint16_t currentCoeffsLoaded = nodeRegisterManager->getStatusValue(mechanism, CoeffsLoadedStatusName);
00253     uint16_t currentControlMode  = nodeRegisterManager->getStatusValue(mechanism, ControlModeStatusName);
00254 
00255 //    // Check for transition from MULTI to MOTCOM
00256 //    if ((currentControlMode == 0) &&
00257 //        (states.commandMode.state == r2_msgs::JointControlCommandMode::MULTILOOPSTEP))
00258 //    {
00259 //        return;
00260 //    }
00261 
00262     // Check for STALLMODE
00263     if ((currentCoeffsLoaded == 1) &&
00264             (currentControlMode  == 1))
00265     {
00266         if (states.commandMode.state != r2_msgs::JointControlCommandMode::STALLMODE)
00267         {
00268             NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::DEBUG << "Transition to JointControlCommandMode::STALLMODE on joint: " << mechanism;
00269             states.commandMode.state = r2_msgs::JointControlCommandMode::STALLMODE;
00270         }
00271 
00272         return;
00273     }
00274 
00275     // Check for MULTILOOPSTEP
00276     if ((currentCoeffsLoaded   == 1) &&
00277             (currentControlMode    == 3))
00278     {
00279         if (states.commandMode.state != r2_msgs::JointControlCommandMode::MULTILOOPSTEP)
00280         {
00281             NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::DEBUG << "Transition to JointControlCommandMode::MULTILOOPSTEP on joint: " << mechanism;
00282             states.commandMode.state = r2_msgs::JointControlCommandMode::MULTILOOPSTEP;
00283         }
00284 
00285         return;
00286     }
00287 
00288     // All valid states have been checked, what's left is invalid
00289     if (states.commandMode.state != r2_msgs::JointControlCommandMode::INVALID)
00290     {
00291         NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::INFO << mechanism << ": INVALID COMMAND MODE STATE DETECTED";
00292         NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::DEBUG << "Transition to JointControlCommandMode::INVALID on joint: " << mechanism;
00293         states.commandMode.state = r2_msgs::JointControlCommandMode::INVALID;
00294     }
00295 }
00296 /***************************************************************************/
00301 void JointControlActualGripper::updateCalibrationModeState(void)
00302 {
00303     // Check for DISABLE
00304     if (nodeRegisterManager->getStatusValue(mechanism, CalibrationModeStatusName) == 0)
00305     {
00306         if (states.calibrationMode.state != r2_msgs::JointControlCalibrationMode::DISABLE)
00307         {
00308             NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::DEBUG << "Transition to JointControlCalibrationMode::DISABLE on joint: " << mechanism;
00309             states.calibrationMode.state = r2_msgs::JointControlCalibrationMode::DISABLE;
00310         }
00311 
00312         return;
00313     }
00314 
00315     // If not DISABLE then ENABLE
00316     if (states.calibrationMode.state != r2_msgs::JointControlCalibrationMode::ENABLE)
00317     {
00318         NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::DEBUG << "Transition to JointControlCalibrationMode::ENABLE on joint: " << mechanism;
00319         states.calibrationMode.state = r2_msgs::JointControlCalibrationMode::ENABLE;
00320     }
00321 }
00322 /***************************************************************************/
00327 void JointControlActualGripper::updateClearFaultModeState(void)
00328 {
00329     // Check for DISABLE
00330     if (nodeRegisterManager->getStatusValue(mechanism, ClearFaultStatusName) == 0)
00331     {
00332         if (states.clearFaultMode.state != r2_msgs::JointControlClearFaultMode::DISABLE)
00333         {
00334             NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::DEBUG << "Transition to JointControlClearFaultMode::DISABLE on joint: " << mechanism;
00335             states.clearFaultMode.state = r2_msgs::JointControlClearFaultMode::DISABLE;
00336         }
00337 
00338         return;
00339     }
00340 
00341     // If not DISABLE then ENABLE
00342     if (states.clearFaultMode.state != r2_msgs::JointControlClearFaultMode::ENABLE)
00343     {
00344         NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::DEBUG << "Transition to JointControlClearFaultMode::ENABLE on joint: " << mechanism;
00345         states.clearFaultMode.state = r2_msgs::JointControlClearFaultMode::ENABLE;
00346     }
00347 }
00348 /***************************************************************************/
00353 void JointControlActualGripper::updateCoeffState(void)
00354 {
00355     // Check for NOTLOADED
00356     if (nodeRegisterManager->getStatusValue(mechanism, CoeffsLoadedStatusName) == 0)
00357     {
00358         if (states.coeffState.state != r2_msgs::JointControlCoeffState::NOTLOADED)
00359         {
00360             NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::DEBUG << "Transition to JointControlCoeffState::NOTLOADED on joint: " << mechanism;
00361             states.coeffState.state = r2_msgs::JointControlCoeffState::NOTLOADED;
00362         }
00363 
00364         return;
00365     }
00366 
00367     // If not NOTLOADED then LOADED
00368     if (states.coeffState.state != r2_msgs::JointControlCoeffState::LOADED)
00369     {
00370         NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::DEBUG << "Transition to JointControlCoeffState::LOADED on joint: " << mechanism;
00371         states.coeffState.state = r2_msgs::JointControlCoeffState::LOADED;
00372     }
00373 }
00374 
00375 void JointControlActualGripper::getFaults(diagnostic_msgs::DiagnosticStatus& faultStatus)
00376 {
00377     faultStatus.name = mechanism.c_str();
00378     faultStatus.hardware_id = mechanism.c_str();
00379     faultStatus.message = "OK";
00380     faultStatus.level = diagnostic_msgs::DiagnosticStatus::OK;
00381 
00382     if (states.controlMode.state == r2_msgs::JointControlMode::FAULTED)
00383     {
00384         faultStatus.level = diagnostic_msgs::DiagnosticStatus::WARN;
00385         std::stringstream message;
00386         message << "FAULT state detected on " << mechanism << ": FAULT";
00387 
00388         if (nodeRegisterManager->getStatusValue(mechanism, ProcAliveStatusName)       == 0)
00389         {
00390             faultStatus.level = diagnostic_msgs::DiagnosticStatus::ERROR;
00391             message << ", ProcNotAlive";
00392         }
00393 
00394         if (nodeRegisterManager->getStatusValue(mechanism, BridgeFaultStatusName)     == 1)
00395         {
00396             message << ", BridgeFault";
00397         }
00398 
00399         if (nodeRegisterManager->getStatusValue(mechanism, CommAliveStatusName)       == 0)
00400         {
00401             faultStatus.level = diagnostic_msgs::DiagnosticStatus::ERROR;
00402             message << ", CommNotAlive";
00403         }
00404 
00405         if (nodeRegisterManager->getStatusValue(mechanism, JointFaultStatusName)      == 1)
00406         {
00407             message << ", JointFault";
00408         }
00409 
00410         if (nodeRegisterManager->getStatusValue(mechanism, BusVoltageFaultStatusName) == 1)
00411         {
00412             message << ", BusVoltageFault";
00413         }
00414 
00415         if (nodeRegisterManager->getStatusValue(mechanism, ApsFaultStatusName)        == 1)
00416         {
00417             message << ", ApsFault";
00418         }
00419 
00420         if (nodeRegisterManager->getStatusValue(mechanism, Aps1TolFaultStatusName)    == 1)
00421         {
00422             message << ", Aps1TolFault";
00423         }
00424 
00425         if (nodeRegisterManager->getStatusValue(mechanism, Aps2TolFaultStatusName)    == 1)
00426         {
00427             message << ", Aps2TolFault";
00428         }
00429 
00430         if (nodeRegisterManager->getStatusValue(mechanism, EncDriftFaultStatusName)   == 1)
00431         {
00432             message << ", EncDriftFault";
00433         }
00434 
00435         if (nodeRegisterManager->getStatusValue(mechanism, VelocityFaultStatusName)   == 1)
00436         {
00437             message << ", VelocityFault";
00438         }
00439 
00440         if (nodeRegisterManager->getStatusValue(mechanism, LimitFaultStatusName)      == 1)
00441         {
00442             message << ", LimitFault";
00443         }
00444 
00445         if (nodeRegisterManager->getStatusValue(mechanism, CoeffsLoadedStatusName)    == 0)
00446         {
00447             message << ", CoeffsNotLoaded";
00448         }
00449 
00450         if (nodeRegisterManager->getStatusValue(mechanism, CurrentFaultStatusName)    == 1)
00451         {
00452             message << ", CurrentFault";
00453         }
00454 
00455         if (nodeRegisterManager->getStatusValue(mechanism, ATIStaticFaultStatusName)  == 1)
00456         {
00457             faultStatus.level = diagnostic_msgs::DiagnosticStatus::ERROR;
00458             message << ", ATIStaticFault";
00459         }
00460 
00461         faultStatus.message = message.str().c_str();
00462     }
00463 }