diff --git a/src/OncillaL1L2.cpp b/src/OncillaL1L2.cpp index a0be954..3d60044 100644 --- a/src/OncillaL1L2.cpp +++ b/src/OncillaL1L2.cpp @@ -1,108 +1,108 @@ #include "OncillaL1L2.h" #include using namespace std; namespace rci{ namespace oncilla{ OncillaL1L2::OncillaL1L2(OncillaSynchronizer &s, const std::string &name) : ResourceNode(name) , Controlled() , Sensing() , PositionControlled() - , PositionSensing() + , PositionSensing(2) , TorqueControlled() , ImpedanceControlled() , synchronizer(s) { /// \todo add default value as setting - this->dimension = 2; + this->_lastCommandedPosition = JointAngles::create(1,0.0); this->_latestJointPosition = JointAngles::create(2,0.0); } bool OncillaL1L2::isConverged() const { throw std::runtime_error("Not yet implemented."); return false; } bool OncillaL1L2::setJointPosition(JointAnglesPtr position) { // Successfull this->_lastCommandedPosition = position; return true; } JointAnglesPtr OncillaL1L2::getJointPosition() const { return this->_latestJointPosition; } bool OncillaL1L2::setJointVelocity(JointVelocitiesPtr ) { throw std::runtime_error("Not yet implemented."); } bool OncillaL1L2::setJointTorque(JointTorquesPtr ) { throw std::runtime_error("Not yet implemented."); } bool OncillaL1L2::setJointImpedance(JointImpedancePtr ) { if (this->synchronizer.tooBusy()) { throw std::runtime_error( "Synchronizer has to be switched off to change JointImpedance."); } else { throw std::runtime_error("Not yet implemented."); } } JointAnglesPtr OncillaL1L2::getLastPositionCommand() const { if (this->_lastCommandedPosition) { return this->_lastCommandedPosition; } // If we don`t have a command yet, we return the latest sensor value if (this->_latestJointPosition) { // TODO: Log return this->_latestJointPosition; } throw runtime_error("No position command received yet."); } OncillaL1L2::~OncillaL1L2() { } std::string OncillaL1L2::print() const { ostringstream outstream(ostringstream::out); outstream.precision(3); // Precision when printing double values outstream << "" << endl; return outstream.str(); } void OncillaL1L2::setRawLocalCommand(double value){ this->_lastCommandedPosition->setValue(0,value); } double OncillaL1L2::getRawLocalCommand() const{ return this->_lastCommandedPosition->asDouble(0); } double OncillaL1L2::getRawLocalMotorAxisPosition() const{ return this->_latestJointPosition->asDouble(MotorAxisIndex); } double OncillaL1L2::getRawLocalMagneticEncoderPosition() const{ return this->_latestJointPosition->asDouble(MagneticEncoderIndex); } void OncillaL1L2::updateRawLocalMotorAxisPosition(double value){ this->_latestJointPosition->setValue(MotorAxisIndex,value); } void OncillaL1L2::updateRawLocalMagneticEncoderPosition(double value){ this->_latestJointPosition->setValue(MagneticEncoderIndex,value); } } }