OncillaL1.cpp
3cc9013c8013tuleu-dev

OncillaL1.cpp
View Options

			#include "OncillaL1.h"


			#include <stdexcept>

			using namespace std;

			namespace rci{
			namespace oncilla{
			OncillaL1::OncillaL1(OncillaSynchronizer &s, const std::string &name)
			: ResourceNode(name)
			, Controlled()
			, Sensing()
			, PositionControlled()
			, PositionSensing()
			, 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 OncillaL1::isConverged() const {
			throw std::runtime_error("Not yet implemented.");
			return false;
			}

			bool OncillaL1::setJointPosition(JointAnglesPtr position) {
			// Successfull
			this->_lastCommandedPosition = position;
			return true;
			}

			JointAnglesPtr OncillaL1::getJointPosition() const {
			return this->_latestJointPosition;
			}

			bool OncillaL1::setJointVelocity(JointVelocitiesPtr ) {
			throw std::runtime_error("Not yet implemented.");
			}

			bool OncillaL1::setJointTorque(JointTorquesPtr ) {
			throw std::runtime_error("Not yet implemented.");
			}

			bool OncillaL1::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 OncillaL1::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.");
			}

			OncillaL1::~OncillaL1() {
			}

			std::string OncillaL1::print() const {
			ostringstream outstream(ostringstream::out);
			outstream.precision(3); // Precision when printing double values
			outstream << "<Oncilla Hip>" << endl;
			return outstream.str();
			}


			void OncillaL1::setRawLocalCommand(double value){
			this->_lastCommandedPosition->setValue(0,value);
			}

			double OncillaL1::getRawLocalCommand() const{
			return this->_lastCommandedPosition->asDouble(0);
			}

			double OncillaL1::getRawLocalMotorAxisPosition() const{
			return this->_latestJointPosition->asDouble(MotorAxisIndex);
			}

			double OncillaL1::getRawLocalMagneticEncoderPosition() const{
			return this->_latestJointPosition->asDouble(MagneticEncoderIndex);
			}

			void OncillaL1::updateRawLocalMotorAxisPosition(double value){
			this->_latestJointPosition->setValue(MotorAxisIndex,value);
			}


			void OncillaL1::updateRawLocalMagneticEncoderPosition(double value){
			this->_latestJointPosition->setValue(MagneticEncoderIndex,value);
			}

			}
			}

liboncilla/src/OncillaL1.cpp3cc9013c8013tuleu-dev

OncillaL1.cppView Options

liboncilla/src/OncillaL1.cpp
3cc9013c8013tuleu-dev

OncillaL1.cpp
View Options