robodyn_mechanisms: JointControlActualSeriesElastic.cpp Source File

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