JointControlCommandGripper.cpp
Go to the documentation of this file.
00001 
00005 #include "robodyn_mechanisms/JointControlCommandGripper.h"
00006 /***************************************************************************/
00011 JointControlCommandGripper::JointControlCommandGripper(const std::string& mechanism, IoFunctions ioFunctions, NodeRegisterManagerPtr nodeRegisterManager)
00012     : JointControlCommandInterface(mechanism, ioFunctions), nodeRegisterManager(nodeRegisterManager), logCategory("gov.nasa.JointControlCommandGripper")
00013 {
00014     flea3Enable = cbnEnable = led2Enable = 0;
00015     setParameters();
00016     bootLoader();
00017     motCom();
00018     disableCalibrationMode();
00019     disableClearFaultMode();
00020 }
00021 
00022 JointControlCommandGripper::~JointControlCommandGripper()
00023 {
00024     // Nothing
00025 }
00026 void JointControlCommandGripper::setParameters()
00027 {
00028     std::string parameterFile = io.getControlFile(mechanism);
00029 
00031     TiXmlDocument file(parameterFile.c_str());
00032 
00033     if (!file.LoadFile())
00034     {
00035         std::stringstream err;
00036         err << "Failed to load file [" << parameterFile << "]";
00037         NasaCommonLogging::Logger::log(logCategory, log4cpp::Priority::FATAL, err.str());
00038         throw std::runtime_error(err.str());
00039     }
00040 
00041     TiXmlHandle doc(&file);
00042     NasaCommonLogging::Logger::log(logCategory, log4cpp::Priority::INFO, "File [" + parameterFile + "] successfully loaded.");
00043 
00044     // Print the XML file's contents
00045     //cout << "Successfully loaded file: " << parameterFile << endl;
00046     //TiXmlPrinter printer;
00047     //printer.SetIndent("\t");
00048     //file.Accept(&printer);
00049     //cout << printer.Str() << endl;
00050 
00051     // Check for ApiMap
00052     TiXmlHandle parametersElement(doc.FirstChildElement("ApiMap"));
00053 
00054     if (parametersElement.ToElement())
00055     {
00056         // Control bit names
00057         BootEnableControlName      = ApiMap::getXmlElementValue(parametersElement, "BootEnableControl");
00058         BridgeEnableControlName    = ApiMap::getXmlElementValue(parametersElement, "BridgeEnableControl");
00059         BrakeReleaseControlName    = ApiMap::getXmlElementValue(parametersElement, "BrakeReleaseControl");
00060         MotorEnableControlName     = ApiMap::getXmlElementValue(parametersElement, "MotorEnableControl");
00061         MotComSourceControlName    = ApiMap::getXmlElementValue(parametersElement, "MotComSourceControl");
00062         ControlModeControlName     = ApiMap::getXmlElementValue(parametersElement, "ControlModeControl");
00063         CalibrationModeControlName = ApiMap::getXmlElementValue(parametersElement, "CalibrationModeControl");
00064         ClearFaultControlName      = ApiMap::getXmlElementValue(parametersElement, "ClearFaultControl");
00065         CrabEnableControlName      = ApiMap::getXmlElementValue(parametersElement, "CrabEnableControl");
00066         AtiEnableControlName       = ApiMap::getXmlElementValue(parametersElement, "AtiEnableControl");
00067         Flea3EnableControlName     = ApiMap::getXmlElementValue(parametersElement, "Flea3EnableControl");
00068         CBNEnableControlName       = ApiMap::getXmlElementValue(parametersElement, "CBNEnableControl");
00069         Led2EnableControlName      = ApiMap::getXmlElementValue(parametersElement, "Led2EnableControl");
00070     }
00071     else
00072     {
00073         std::stringstream err;
00074         err << "The file " << parameterFile << " has no element named [ApiMap]";
00075         NasaCommonLogging::Logger::log(logCategory, log4cpp::Priority::ERROR, err.str());
00076         throw std::runtime_error(err.str());
00077     }
00078 
00079     AtiEnableName   = StringUtilities::makeFullyQualifiedRoboDynElement(mechanism, AtiEnableControlName);
00080     Flea3EnableName = StringUtilities::makeFullyQualifiedRoboDynElement(mechanism, Flea3EnableControlName);
00081     CBNEnableName   = StringUtilities::makeFullyQualifiedRoboDynElement(mechanism, CBNEnableControlName);
00082     Led2EnableName  = StringUtilities::makeFullyQualifiedRoboDynElement(mechanism, Led2EnableControlName);
00083 
00084     // Set constant control register bits for this type of joint
00085     if (nodeRegisterManager->hasControlProperty(mechanism, CrabEnableControlName))
00086     {
00087         nodeRegisterManager->setControlValue(mechanism, CrabEnableControlName, 1);
00088     }
00089 
00090     if (nodeRegisterManager->hasControlProperty(mechanism, AtiEnableControlName))
00091     {
00092         atiEnable = 1;
00093         io.setIntParam(AtiEnableName, atiEnable);
00094         nodeRegisterManager->setControlValue(mechanism, AtiEnableControlName, atiEnable);
00095     }
00096 
00097     if (nodeRegisterManager->hasControlProperty(mechanism, Flea3EnableControlName))
00098     {
00099         flea3Enable = nodeRegisterManager->getControlValue(mechanism, Flea3EnableControlName);
00100         io.setIntParam(Flea3EnableName, flea3Enable);
00101     }
00102 
00103     if (nodeRegisterManager->hasControlProperty(mechanism, CBNEnableControlName))
00104     {
00105         cbnEnable = nodeRegisterManager->getControlValue(mechanism, CBNEnableControlName);
00106         io.setIntParam(CBNEnableName, cbnEnable);
00107     }
00108 
00109     if (nodeRegisterManager->hasControlProperty(mechanism, Led2EnableControlName))
00110     {
00111         led2Enable = nodeRegisterManager->getControlValue(mechanism, Led2EnableControlName);
00112         io.setIntParam(Led2EnableName, led2Enable);
00113     }
00114 
00115 
00116 }
00117 
00118 /***************************************************************************/
00124 void JointControlCommandGripper::getStates(r2_msgs::JointControlData& commandStates)
00125 {
00126     updateParameters();
00127     commandStates = states;
00128 }
00129 
00130 void JointControlCommandGripper::updateParameters()
00131 {
00132     if (nodeRegisterManager->hasControlProperty(mechanism, Flea3EnableControlName))
00133     {
00134         io.getIntParam(Flea3EnableName, flea3Enable);
00135         nodeRegisterManager->setControlValue(mechanism, Flea3EnableControlName, flea3Enable);
00136     }
00137 
00138     if (nodeRegisterManager->hasControlProperty(mechanism, CBNEnableControlName))
00139     {
00140         io.getIntParam(CBNEnableName, cbnEnable);
00141         nodeRegisterManager->setControlValue(mechanism, CBNEnableControlName, cbnEnable);
00142     }
00143 
00144     if (nodeRegisterManager->hasControlProperty(mechanism, Led2EnableControlName))
00145     {
00146         io.getIntParam(Led2EnableName, led2Enable);
00147         nodeRegisterManager->setControlValue(mechanism, Led2EnableControlName, led2Enable);
00148     }
00149 
00150     if (nodeRegisterManager->hasControlProperty(mechanism, AtiEnableControlName))
00151     {
00152         io.getIntParam(AtiEnableName, atiEnable);
00153         nodeRegisterManager->setControlValue(mechanism, AtiEnableControlName, atiEnable);
00154     }
00155 
00156 }
00157 
00158 // controlMode functions
00159 /***************************************************************************/
00164 void JointControlCommandGripper::bootLoader(void)
00165 {
00166     NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::DEBUG << "Command transition to JointControlMode::BOOTLOADER successful on joint: " << mechanism;
00167     nodeRegisterManager->setControlValue(mechanism, BootEnableControlName,   0);
00168     nodeRegisterManager->setControlValue(mechanism, BridgeEnableControlName, 1);
00169     nodeRegisterManager->setControlValue(mechanism, BrakeReleaseControlName, 0);
00170     nodeRegisterManager->setControlValue(mechanism, MotorEnableControlName,  0);
00171     states.controlMode.state = r2_msgs::JointControlMode::BOOTLOADER;
00172 }
00173 /***************************************************************************/
00178 void JointControlCommandGripper::off(void)
00179 {
00180     NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::DEBUG << "Command transition to JointControlMode::OFF successful on joint: " << mechanism;
00181     nodeRegisterManager->setControlValue(mechanism, BootEnableControlName,   1);
00182     nodeRegisterManager->setControlValue(mechanism, BridgeEnableControlName, 1);  // fixing init problems, per Harrison's suggestion
00183     nodeRegisterManager->setControlValue(mechanism, BrakeReleaseControlName, 0);
00184     nodeRegisterManager->setControlValue(mechanism, MotorEnableControlName,  0);
00185     states.controlMode.state = r2_msgs::JointControlMode::OFF;
00186 }
00187 /***************************************************************************/
00192 void JointControlCommandGripper::park(void)
00193 {
00194     NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::DEBUG << "Command transition to JointControlMode::PARK successful on joint: " << mechanism;
00195     nodeRegisterManager->setControlValue(mechanism, BridgeEnableControlName, 1);
00196     nodeRegisterManager->setControlValue(mechanism, BrakeReleaseControlName, 0);
00197     nodeRegisterManager->setControlValue(mechanism, MotorEnableControlName,  1);
00198     states.controlMode.state = r2_msgs::JointControlMode::PARK;
00199 }
00200 /***************************************************************************/
00205 void JointControlCommandGripper::neutral(void)
00206 {
00207     NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::DEBUG << "Command transition to JointControlMode::NEUTRAL successful on joint: " << mechanism;
00208     nodeRegisterManager->setControlValue(mechanism, BridgeEnableControlName, 1);
00209     nodeRegisterManager->setControlValue(mechanism, BrakeReleaseControlName, 1);
00210     nodeRegisterManager->setControlValue(mechanism, MotorEnableControlName,  0);
00211     states.controlMode.state = r2_msgs::JointControlMode::NEUTRAL;
00212 }
00213 /***************************************************************************/
00218 void JointControlCommandGripper::drive(void)
00219 {
00220     NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::DEBUG << "Command transition to JointControlMode::DRIVE successful on joint: " << mechanism;
00221     nodeRegisterManager->setControlValue(mechanism, BrakeReleaseControlName, 1);
00222     nodeRegisterManager->setControlValue(mechanism, MotorEnableControlName,  1);
00223     states.controlMode.state = r2_msgs::JointControlMode::DRIVE;
00224 }
00225 
00226 // commandMode functions
00227 /***************************************************************************/
00232 void JointControlCommandGripper::motCom(void)
00233 {
00234     NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::DEBUG << "Command transition to JointControlMode::MOTCOM successful on joint: " << mechanism;
00235     nodeRegisterManager->setControlValue(mechanism, MotComSourceControlName, 0); // Brainstem mot com
00236     nodeRegisterManager->setControlValue(mechanism, ControlModeControlName,  0); // MotCom control mode
00237     states.commandMode.state = r2_msgs::JointControlCommandMode::MOTCOM;
00238 }
00239 /***************************************************************************/
00244 void JointControlCommandGripper::stallMode(void)
00245 {
00246     NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::DEBUG << "Command transition to JointControlMode::STALLMODE successful on joint: " << mechanism;
00247 
00248     nodeRegisterManager->setControlValue(mechanism, MotComSourceControlName, 1); // Embedded mot com
00249     nodeRegisterManager->setControlValue(mechanism, ControlModeControlName,  1); // StallMode control mode
00250 
00251     states.commandMode.state = r2_msgs::JointControlCommandMode::STALLMODE;
00252     return;
00253 }
00254 /***************************************************************************/
00259 void JointControlCommandGripper::multiLoopStep(void)
00260 {
00261     NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::DEBUG << "Command transition to JointControlMode::MULTILOOPSTEP successful on joint: " << mechanism;
00262 
00263     nodeRegisterManager->setControlValue(mechanism, MotComSourceControlName,   1); // Embedded mot com
00264     nodeRegisterManager->setControlValue(mechanism, ControlModeControlName,    3); // MultiLoop control mode
00265 
00266     states.commandMode.state = r2_msgs::JointControlCommandMode::MULTILOOPSTEP;
00267     return;
00268 }
00269 
00270 /***************************************************************************/
00275 void JointControlCommandGripper::multiLoopSmooth(void)
00276 {
00277     NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::INFO << "MULTILOOPSMOOTH not defined for Grippers, trying MULTILOOPSTEP instead on joint: " << mechanism;
00278     multiLoopStep();
00279 }
00280 
00281 void JointControlCommandGripper::actuator(void)
00282 {
00283     NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::WARN << "ACTUATOR is not a valid JointControlCommandMode for Grippers , trying MULTILOOPSTEP instead on joint: " << mechanism;
00284     multiLoopStep();
00285 }
00286 
00287 // calibrationMode functions
00288 /***************************************************************************/
00293 void JointControlCommandGripper::disableCalibrationMode(void)
00294 {
00295     NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::DEBUG << "Command transition to JointControlCalibrationMode::DISABLE successful on joint: " << mechanism;
00296     nodeRegisterManager->setControlValue(mechanism, CalibrationModeControlName, 0);
00297     states.calibrationMode.state = r2_msgs::JointControlCalibrationMode::DISABLE;
00298 }
00299 /***************************************************************************/
00304 void JointControlCommandGripper::enableCalibrationMode(void)
00305 {
00306     NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::DEBUG << "Command transition to JointControlCalibrationMode::ENABLE successful on joint: " << mechanism;
00307     nodeRegisterManager->setControlValue(mechanism, CalibrationModeControlName, 1);
00308     states.calibrationMode.state = r2_msgs::JointControlCalibrationMode::ENABLE;
00309 }
00310 /***************************************************************************/
00315 void JointControlCommandGripper::resetCalibrationMode(void)
00316 {
00317     NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::DEBUG << "Command transition to JointControlCalibrationMode::DISABLE successful on joint: " << mechanism;
00318     nodeRegisterManager->setControlValue(mechanism, CalibrationModeControlName, 0);
00319     states.calibrationMode.state = r2_msgs::JointControlCalibrationMode::DISABLE;
00320 }
00321 // clearFaultMode functions
00322 /***************************************************************************/
00327 void JointControlCommandGripper::disableClearFaultMode(void)
00328 {
00329     NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::DEBUG << "Command transition to JointControlClearFaultMode::DISABLE successful on joint: " << mechanism;
00330     nodeRegisterManager->setControlValue(mechanism, ClearFaultControlName, 0);
00331     states.clearFaultMode.state = r2_msgs::JointControlClearFaultMode::DISABLE;
00332 }
00333 /***************************************************************************/
00338 void JointControlCommandGripper::enableClearFaultMode(void)
00339 {
00340     NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::DEBUG << "Command transition to JointControlClearFaultMode::ENABLE successful on joint: " << mechanism;
00341     nodeRegisterManager->setControlValue(mechanism, ClearFaultControlName, 1);
00342     states.clearFaultMode.state = r2_msgs::JointControlClearFaultMode::ENABLE;
00343 }

robodyn_mechanisms
Author(s):
autogenerated on Thu Jun 6 2019 21:22:48