ServoController.cpp
Go to the documentation of this file.
00001 // -*- C++ -*-
00010 #include <rtm/CorbaNaming.h>
00011 #include <hrpModel/Link.h>
00012 #include <hrpModel/ModelLoaderUtil.h>
00013 #include "ServoController.h"
00014 #include "hrpsys/util/VectorConvert.h"
00015 
00016 #include "ServoSerial.h"
00017 
00018 using namespace std;
00019 
00020 // Module specification
00021 // <rtc-template block="module_spec">
00022 static const char* nullcomponent_spec[] =
00023   {
00024     "implementation_id", "ServoController",
00025     "type_name",         "ServoController",
00026     "description",       "null component",
00027     "version",           HRPSYS_PACKAGE_VERSION,
00028     "vendor",            "AIST",
00029     "category",          "example",
00030     "activity_type",     "DataFlowComponent",
00031     "max_instance",      "10",
00032     "language",          "C++",
00033     "lang_type",         "compile",
00034     // Configuration variables
00035     ""
00036   };
00037 // </rtc-template>
00038 
00039 ServoController::ServoController(RTC::Manager* manager)
00040   : RTC::DataFlowComponentBase(manager),
00041     // <rtc-template block="initializer">
00042     m_ServoControllerServicePort("ServoControllerService")
00043     // </rtc-template>
00044 {
00045     m_service0.servo(this);
00046 }
00047 
00048 ServoController::~ServoController()
00049 {
00050 }
00051 
00052 
00053 
00054 RTC::ReturnCode_t ServoController::onInitialize()
00055 {
00056   std::cout << m_profile.instance_name << ": onInitialize()" << std::endl;
00057   // <rtc-template block="bind_config">
00058   // Bind variables and configuration variable
00059   
00060   // </rtc-template>
00061 
00062   // Registration: InPort/OutPort/Service
00063   // <rtc-template block="registration">
00064   // Set InPort buffers
00065 
00066   // Set OutPort buffer
00067   
00068   // Set service provider to Ports
00069   m_ServoControllerServicePort.registerProvider("service0", "ServoControllerService", m_service0);
00070   
00071   // Set service consumers to Ports
00072   
00073   // Set CORBA Service Ports
00074   addPort(m_ServoControllerServicePort);
00075   
00076   // </rtc-template>
00077 
00078   RTC::Properties& prop = getProperties();
00079 
00080   // get servo.devname
00081   std::string devname = prop["servo.devname"];
00082   if ( devname  == "" ) {
00083       std::cerr << "\e[1;31m[WARNING] " <<  m_profile.instance_name << ": needs servo.devname property\e[0m" << std::endl;
00084       std::cerr << "\e[1;31m[WARNING] " <<  m_profile.instance_name << ": running in dummy mode\e[0m" << std::endl;
00085       return RTC::RTC_OK;
00086   }
00087 
00088   // get servo.id
00089   coil::vstring servo_ids = coil::split(prop["servo.id"], ",");
00090   if ( servo_ids.size() == 0 ) {
00091       std::cerr << "\e[1;31m[ERROR] " <<  m_profile.instance_name << ": needs servo.id property\e[0m" << std::endl;
00092       return RTC::RTC_ERROR;
00093   }
00094   servo_id.resize(servo_ids.size());
00095   for(unsigned int i = 0; i < servo_ids.size(); i++) {
00096       coil::stringTo(servo_id[i], servo_ids[i].c_str());
00097   }
00098 
00099   std::cout << m_profile.instance_name << ": servo_id : ";
00100   for(unsigned int i = 0; i < servo_id.size(); i++) {
00101       std::cerr << servo_id[i] << " ";
00102   }
00103   std::cerr << std::endl;
00104 
00105   // get servo.offset
00106   coil::vstring servo_offsets = coil::split(prop["servo.offset"], ",");
00107   if ( servo_offsets.size() == 0 ) {
00108       servo_offset.resize(servo_ids.size());
00109   } else {
00110       servo_offset.resize(servo_offsets.size());
00111   }
00112   if ( servo_ids.size() != servo_offset.size() ) {
00113       std::cerr << "\e[1;31m[ERROR] " <<  m_profile.instance_name << ": servo.id and servo.offset property must have same length\e[0m" << std::endl;
00114       return RTC::RTC_ERROR;
00115   }
00116   for(unsigned int i = 0; i < servo_offsets.size(); i++) {
00117       coil::stringTo(servo_offset[i], servo_offsets[i].c_str());
00118   }
00119 
00120   std::cout << m_profile.instance_name << ": servo_offset : ";
00121   for(unsigned int i = 0; i < servo_offset.size(); i++) {
00122       std::cerr << servo_offset[i] << " ";
00123   }
00124   std::cerr << std::endl;
00125 
00126  // get servo.dir
00127   coil::vstring servo_dirs = coil::split(prop["servo.dir"], ",");
00128   if ( servo_dirs.size() == 0 ) {
00129       servo_dir.assign(servo_ids.size(), 1);
00130   } else {
00131       servo_dir.assign(servo_dirs.size(), 1);
00132   }
00133   if ( servo_ids.size() != servo_dir.size() ) {
00134       std::cerr << "\e[1;31m[ERROR] " <<  m_profile.instance_name << ": servo.id and servo.dir property must have same length\e[0m" << std::endl;
00135       return RTC::RTC_ERROR;
00136   }
00137   for(unsigned int i = 0; i < servo_dirs.size(); i++) {
00138       coil::stringTo(servo_dir[i], servo_dirs[i].c_str());
00139   }
00140 
00141   std::cout << m_profile.instance_name << ": servo_dir : ";
00142   for(unsigned int i = 0; i < servo_dir.size(); i++) {
00143       std::cerr << servo_dir[i] << " ";
00144   }
00145   std::cerr << std::endl;
00146 
00147   serial = new ServoSerial((char *)(devname.c_str()));
00148 
00149   return RTC::RTC_OK;
00150 }
00151 
00152 
00153 
00154 RTC::ReturnCode_t ServoController::onFinalize()
00155 {
00156     if ( serial ) delete serial;
00157     return RTC::RTC_OK;
00158 }
00159 
00160 /*
00161 RTC::ReturnCode_t ServoController::onStartup(RTC::UniqueId ec_id)
00162 {
00163   return RTC::RTC_OK;
00164 }
00165 */
00166 
00167 /*
00168 RTC::ReturnCode_t ServoController::onShutdown(RTC::UniqueId ec_id)
00169 {
00170   return RTC::RTC_OK;
00171 }
00172 */
00173 
00174 RTC::ReturnCode_t ServoController::onActivated(RTC::UniqueId ec_id)
00175 {
00176   std::cout << m_profile.instance_name<< ": onActivated(" << ec_id << ")" << std::endl;
00177   if ( ! serial ) return RTC::RTC_OK;
00178 
00179   return RTC::RTC_OK;
00180 }
00181 
00182 RTC::ReturnCode_t ServoController::onDeactivated(RTC::UniqueId ec_id)
00183 {
00184   std::cout << m_profile.instance_name<< ": onDeactivated(" << ec_id << ")" << std::endl;
00185   if ( ! serial ) return RTC::RTC_OK;
00186 
00187   return RTC::RTC_OK;
00188 }
00189 
00190 RTC::ReturnCode_t ServoController::onExecute(RTC::UniqueId ec_id)
00191 {
00192   return RTC::RTC_OK;
00193 }
00194 
00195 /*
00196 RTC::ReturnCode_t ServoController::onAborting(RTC::UniqueId ec_id)
00197 {
00198   return RTC::RTC_OK;
00199 }
00200 */
00201 
00202 /*
00203 RTC::ReturnCode_t ServoController::onError(RTC::UniqueId ec_id)
00204 {
00205   return RTC::RTC_OK;
00206 }
00207 */
00208 
00209 /*
00210 RTC::ReturnCode_t ServoController::onReset(RTC::UniqueId ec_id)
00211 {
00212   return RTC::RTC_OK;
00213 }
00214 */
00215 
00216 /*
00217 RTC::ReturnCode_t ServoController::onStateUpdate(RTC::UniqueId ec_id)
00218 {
00219   return RTC::RTC_OK;
00220 }
00221 */
00222 
00223 /*
00224 RTC::ReturnCode_t ServoController::onRateChanged(RTC::UniqueId ec_id)
00225 {
00226   return RTC::RTC_OK;
00227 }
00228 */
00229 
00230 bool ServoController::setJointAngle(short id, double angle, double tm)
00231 {
00232     if ( ! serial ) return true;
00233     double rad = angle * M_PI / 180;
00234     for(unsigned int i=0; i<servo_id.size(); i++){
00235       if(servo_id[i]==id) serial->setPosition(id,rad+servo_offset[i], tm);
00236     }
00237     return true;
00238 }
00239 
00240 bool ServoController::setJointAngles(const OpenHRP::ServoControllerService::dSequence angles, double tm)
00241 {
00242     if ( ! serial ) return true;
00243 
00244     int id[servo_id.size()];
00245     double tms[servo_id.size()];
00246     double rad[servo_id.size()];
00247     for( unsigned int i = 0; i < servo_id.size(); i++ ) {
00248         id[i] = servo_id[i];
00249         tms[i] = tm;
00250         rad[i] = (angles.get_buffer()[i]*servo_dir[i]+servo_offset[i]);
00251     }
00252     if ( angles.length() != servo_id.size() ) {
00253         std::cerr << "[ERROR] " <<  m_profile.instance_name << ": size of servo.id(" << angles.length() << ") is not correct, expected" << servo_id.size() << std::endl;
00254         return false;
00255     }
00256     serial->setPositions(servo_id.size(), id, rad, tms);
00257     return true;
00258 }
00259 
00260 bool ServoController::getJointAngle(short id, double &angle)
00261 {
00262     if ( ! serial ) return true;
00263 
00264     int ret = serial->getPosition(id, &angle);
00265     for(unsigned int i=0; i<servo_id.size(); i++){
00266       if(servo_id[i]==id){
00267         double servo_offset_angle = servo_offset[i] * 180 / M_PI;
00268         angle -= servo_offset_angle;
00269       }
00270     }
00271 
00272     if (ret < 0) return false;
00273     return true;
00274 }
00275 
00276 bool ServoController::getJointAngles(OpenHRP::ServoControllerService::dSequence_out &angles)
00277 {
00278     if ( ! serial ) return true;
00279 
00280     int ret;
00281 
00282     angles = new OpenHRP::ServoControllerService::dSequence();
00283     angles->length(servo_id.size());
00284     for(unsigned int i=0; i < servo_id.size(); i++){
00285         ret = serial->getPosition(servo_id[i], &(angles->get_buffer()[i]));
00286         if (ret < 0) return false;
00287     }
00288     return true;
00289 }
00290 
00291 bool ServoController::addJointGroup(const char *gname, const ::OpenHRP::ServoControllerService::iSequence ids)
00292 {
00293     if ( ! serial ) return true;
00294 
00295     std::vector<int> indices;
00296     for (size_t i=0; i<ids.length(); i++){
00297         indices.push_back(ids[i]);
00298     }
00299     joint_groups[gname] = indices;
00300 
00301     return true;
00302 }
00303 
00304 bool ServoController::removeJointGroup(const char *gname)
00305 {
00306     if ( ! serial ) return true;
00307 
00308     joint_groups.erase(gname);
00309 }
00310 
00311 bool ServoController::setJointAnglesOfGroup(const char *gname, const OpenHRP::ServoControllerService::dSequence angles, double tm)
00312 {
00313     if ( ! serial ) return true;
00314 
00315     if ( joint_groups.find(gname) != joint_groups.end()) {
00316         unsigned int len = joint_groups[gname].size();
00317         if ( angles.length() != len ) {
00318             std::cerr << "[ERROR] " <<  m_profile.instance_name << ": size of servo.id(" << angles.length() << ") is not correct, expected" << len << std::endl;
00319             return false;
00320         }
00321         int id[len];
00322         double tms[len];
00323         double rad[len];
00324         for( unsigned int i = 0; i < len; i++ ) {
00325             id[i] = joint_groups[gname][i];
00326             tms[i] = tm;
00327             double offset, dir;
00328             for( unsigned int j = 0; j < servo_id.size(); j++ ) {
00329                 if ( servo_id[j] == id[i]) {
00330                     offset = servo_offset[j];
00331                     dir = servo_dir[j];
00332                 }
00333             }
00334             rad[i] = (angles.get_buffer()[i])*dir+offset;
00335         }
00336         serial->setPositions(servo_id.size(), id, rad, tms);
00337     }
00338     return true;
00339 }
00340 
00341 bool ServoController::setMaxTorque(short id, short percentage)
00342 {
00343     if ( ! serial ) return true;
00344 
00345     int ret = serial->setMaxTorque(id, percentage);
00346 
00347     if (ret < 0) return false;
00348     return true;
00349 }
00350 
00351 bool ServoController::setReset(short id)
00352 {
00353     if ( ! serial ) return true;
00354 
00355     int ret = serial->setReset(id);
00356 
00357     if (ret < 0) return false;
00358     return true;
00359 }
00360 
00361 bool ServoController::getDuration(short id, double &duration)
00362 {
00363     if ( ! serial ) return true;
00364 
00365     int ret = serial->getDuration(id, &duration);
00366 
00367     if (ret < 0) return false;
00368     return true;
00369 }
00370 
00371 bool ServoController::getSpeed(short id, double &speed)
00372 {
00373     if ( ! serial ) return true;
00374 
00375     int ret = serial->getSpeed(id, &speed);
00376 
00377     if (ret < 0) return false;
00378     return true;
00379 }
00380 
00381 bool ServoController::getMaxTorque(short id, short &percentage)
00382 {
00383     if ( ! serial ) return true;
00384 
00385     int ret = serial->getMaxTorque(id, &percentage);
00386 
00387     if (ret < 0) return false;
00388     return true;
00389 }
00390 
00391 bool ServoController::getTorque(short id, double &torque)
00392 {
00393     if ( ! serial ) return true;
00394 
00395     int ret = serial->getTorque(id, &torque);
00396 
00397     if (ret < 0) return false;
00398     return true;
00399 }
00400 
00401 bool ServoController::getTemperature(short id, double &temperature)
00402 {
00403     if ( ! serial ) return true;
00404 
00405     int ret = serial->getTemperature(id, &temperature);
00406 
00407     if (ret < 0) return false;
00408     return true;
00409 }
00410 
00411 bool ServoController::getVoltage(short id, double &voltage)
00412 {
00413     if ( ! serial ) return true;
00414 
00415     int ret = serial->getVoltage(id, &voltage);
00416 
00417     if (ret < 0) return false;
00418     return true;
00419 }
00420 
00421 bool ServoController::servoOn()
00422 {
00423     if ( ! serial ) return true;
00424 
00425     int ret;
00426 
00427     for (vector<int>::iterator it = servo_id.begin(); it != servo_id.end(); it++ ){
00428         ret = serial->setTorqueOn(*it);
00429         if (ret < 0) return false;
00430     }
00431 
00432     return true;
00433 }
00434 
00435 bool ServoController::servoOff()
00436 {
00437     if ( ! serial ) return true;
00438 
00439     int ret;
00440 
00441     for (vector<int>::iterator it = servo_id.begin(); it != servo_id.end(); it++ ){
00442         ret = serial->setTorqueOff(*it);
00443         if (ret < 0) return false;
00444     }
00445     return true;
00446 }
00447 
00448 
00449 extern "C"
00450 {
00451 
00452   void ServoControllerInit(RTC::Manager* manager)
00453   {
00454     RTC::Properties profile(nullcomponent_spec);
00455     manager->registerFactory(profile,
00456                              RTC::Create<ServoController>,
00457                              RTC::Delete<ServoController>);
00458   }
00459 
00460 };
00461 
00462 


hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Wed Sep 6 2017 02:35:56