A TechnosoftIposBase implementation using the firmware-embedded PID.

#include <TechnosoftIposEmbedded.hpp>

Inheritance diagram for roboticslab::TechnosoftIposEmbedded:

Public Member Functions
bool	open (yarp::os::Searchable &config) override

bool	close () override

bool	synchronize (double timestamp) override
	Perform synchronized action on CAN master's request.

return_t	getControlModeRaw (int j, int *mode) override

return_t	setControlModeRaw (int j, int mode) override

return_t	positionMoveRaw (int j, double ref) override

return_t	relativeMoveRaw (int j, double delta) override

return_t	checkMotionDoneRaw (int j, bool *flag) override

return_t	setRefSpeedRaw (int j, double sp) override

return_t	setRefAccelerationRaw (int j, double acc) override

return_t	getRefSpeedRaw (int j, double *ref) override

return_t	getRefAccelerationRaw (int j, double *acc) override

return_t	stopRaw (int j) override

return_t	getTargetPositionRaw (int joint, double *ref) override

return_t	setPositionRaw (int j, double ref) override

return_t	getRefPositionRaw (int joint, double *ref) override

return_t	getRemoteVariableRaw (std::string key, yarp::os::Bottle &val) override

return_t	setRemoteVariableRaw (std::string key, const yarp::os::Bottle &val) override

return_t	getRemoteVariablesListRaw (yarp::os::Bottle *listOfKeys) override

return_t	velocityMoveRaw (int j, double sp) override

return_t	getRefVelocityRaw (int joint, double *vel) override

	TechnosoftIposBase (const TechnosoftIpos_ParamsParser &_params)

Public Member Functions inherited from roboticslab::TechnosoftIposBase
	TechnosoftIposBase (const TechnosoftIpos_ParamsParser &_params)

bool	open (yarp::os::Searchable &config) override

bool	close () override

unsigned int	getId () override
	Retrieve CAN node ID.

std::vector< unsigned int >	getAdditionalIds () override
	Retrieve more associated CAN node IDs, if any.

bool	notifyMessage (const can_message &message) override
	Notify observers that a new CAN message has arrived.

bool	initialize () override
	Perform CAN node initialization.

bool	finalize () override
	Finalize CAN node communications.

bool	registerSender (ICanSenderDelegate *sender) override
	Pass a handle to a CAN sender delegate instance.

return_t	getAxisNameRaw (int axis, std::string &name) override

return_t	getJointTypeRaw (int axis, yarp::dev::JointTypeEnum &type) override

return_t	setLimitsRaw (int axis, double min, double max) override

return_t	getLimitsRaw (int axis, double min, double max) override

return_t	setVelLimitsRaw (int axis, double min, double max) override

return_t	getVelLimitsRaw (int axis, double min, double max) override

return_t	getControlModesRaw (int *modes) override

return_t	getControlModesRaw (int n_joint, const int joints, int modes) override

return_t	setControlModesRaw (int *modes) override

return_t	setControlModesRaw (int n_joint, const int joints, int modes) override

return_t	getCurrentRaw (int m, double *curr) override

return_t	getCurrentsRaw (double *currs) override

return_t	getCurrentRangeRaw (int m, double min, double max) override

return_t	getCurrentRangesRaw (double min, double max) override

return_t	setRefCurrentRaw (int m, double curr) override

return_t	setRefCurrentsRaw (const double *currs) override

return_t	setRefCurrentsRaw (int n_motor, const int motors, const double currs) override

return_t	getRefCurrentRaw (int m, double *curr) override

return_t	getRefCurrentsRaw (double *currs) override

return_t	getAxes (int *ax) override

return_t	resetEncoderRaw (int j) override

return_t	resetEncodersRaw () override

return_t	setEncoderRaw (int j, double val) override

return_t	setEncodersRaw (const double *vals) override

return_t	getEncoderRaw (int j, double *v) override

return_t	getEncodersRaw (double *encs) override

return_t	getEncoderSpeedRaw (int j, double *sp) override

return_t	getEncoderSpeedsRaw (double *spds) override

return_t	getEncoderAccelerationRaw (int j, double *spds) override

return_t	getEncoderAccelerationsRaw (double *accs) override

return_t	getEncoderTimedRaw (int j, double encs, double time) override

return_t	getEncodersTimedRaw (double encs, double times) override

return_t	getImpedanceRaw (int j, double stiffness, double damping) override

return_t	setImpedanceRaw (int j, double stiffness, double damping) override

return_t	setImpedanceOffsetRaw (int j, double offset) override

return_t	getImpedanceOffsetRaw (int j, double *offset) override

return_t	getCurrentImpedanceLimitRaw (int j, double min_stiff, double max_stiff, double min_damp, double max_damp) override

return_t	getInteractionModeRaw (int axis, yarp::dev::InteractionModeEnum *mode) override

return_t	getInteractionModesRaw (int n_joints, int joints, yarp::dev::InteractionModeEnum modes) override

return_t	getInteractionModesRaw (yarp::dev::InteractionModeEnum *modes) override

return_t	setInteractionModeRaw (int axis, yarp::dev::InteractionModeEnum mode) override

return_t	setInteractionModesRaw (int n_joints, int joints, yarp::dev::InteractionModeEnum modes) override

return_t	setInteractionModesRaw (yarp::dev::InteractionModeEnum *modes) override

return_t	getLastJointFaultRaw (int j, int &fault, std::string &message) override

return_t	getNumberOfMotorsRaw (int *num) override

return_t	getTemperatureRaw (int m, double *val) override

return_t	getTemperaturesRaw (double *vals) override

return_t	getTemperatureLimitRaw (int m, double *temp) override

return_t	setTemperatureLimitRaw (int m, double temp) override

return_t	getGearboxRatioRaw (int m, double *val) override

return_t	setGearboxRatioRaw (int m, double val) override

return_t	getNumberOfMotorEncodersRaw (int *num) override

return_t	resetMotorEncoderRaw (int m) override

return_t	resetMotorEncodersRaw () override

return_t	setMotorEncoderCountsPerRevolutionRaw (int m, double cpr) override

return_t	getMotorEncoderCountsPerRevolutionRaw (int m, double *cpr) override

return_t	setMotorEncoderRaw (int m, double val) override

return_t	setMotorEncodersRaw (const double *vals) override

return_t	getMotorEncoderRaw (int m, double *v) override

return_t	getMotorEncodersRaw (double *encs) override

return_t	getMotorEncoderTimedRaw (int m, double encs, double stamp) override

return_t	getMotorEncodersTimedRaw (double encs, double stamps) override

return_t	getMotorEncoderSpeedRaw (int m, double *sp) override

return_t	getMotorEncoderSpeedsRaw (double *spds) override

return_t	getMotorEncoderAccelerationRaw (int m, double *spds) override

return_t	getMotorEncoderAccelerationsRaw (double *accs) override

return_t	setPidRaw (const yarp::dev::PidControlTypeEnum &pidtype, int j, const yarp::dev::Pid &pid) override

return_t	setPidsRaw (const yarp::dev::PidControlTypeEnum &pidtype, const yarp::dev::Pid *pids) override

return_t	setPidReferenceRaw (const yarp::dev::PidControlTypeEnum &pidtype, int j, double ref) override

return_t	setPidReferencesRaw (const yarp::dev::PidControlTypeEnum &pidtype, const double *refs) override

return_t	setPidErrorLimitRaw (const yarp::dev::PidControlTypeEnum &pidtype, int j, double limit) override

return_t	setPidErrorLimitsRaw (const yarp::dev::PidControlTypeEnum &pidtype, const double *limits) override

return_t	getPidErrorRaw (const yarp::dev::PidControlTypeEnum &pidtype, int j, double *err) override

return_t	getPidErrorsRaw (const yarp::dev::PidControlTypeEnum &pidtype, double *errs) override

return_t	getPidOutputRaw (const yarp::dev::PidControlTypeEnum &pidtype, int j, double *out) override

return_t	getPidOutputsRaw (const yarp::dev::PidControlTypeEnum &pidtype, double *outs) override

return_t	getPidRaw (const yarp::dev::PidControlTypeEnum &pidtype, int j, yarp::dev::Pid *pid) override

return_t	getPidsRaw (const yarp::dev::PidControlTypeEnum &pidtype, yarp::dev::Pid *pids) override

return_t	getPidReferenceRaw (const yarp::dev::PidControlTypeEnum &pidtype, int j, double *ref) override

return_t	getPidReferencesRaw (const yarp::dev::PidControlTypeEnum &pidtype, double *refs) override

return_t	getPidErrorLimitRaw (const yarp::dev::PidControlTypeEnum &pidtype, int j, double *limit) override

return_t	getPidErrorLimitsRaw (const yarp::dev::PidControlTypeEnum &pidtype, double *limits) override

return_t	resetPidRaw (const yarp::dev::PidControlTypeEnum &pidtype, int j) override

return_t	disablePidRaw (const yarp::dev::PidControlTypeEnum &pidtype, int j) override

return_t	enablePidRaw (const yarp::dev::PidControlTypeEnum &pidtype, int j) override

return_t	setPidOffsetRaw (const yarp::dev::PidControlTypeEnum &pidtype, int j, double v) override

return_t	isPidEnabledRaw (const yarp::dev::PidControlTypeEnum &pidtype, int j, bool *enabled) override

return_t	positionMoveRaw (const double *refs) override

return_t	positionMoveRaw (int n_joint, const int joints, const double refs) override

return_t	relativeMoveRaw (const double *deltas) override

return_t	relativeMoveRaw (int n_joint, const int joints, const double deltas) override

return_t	checkMotionDoneRaw (bool *flag) override

return_t	checkMotionDoneRaw (int n_joint, const int joints, bool flag) override

return_t	setRefSpeedsRaw (const double *spds) override

return_t	setRefSpeedsRaw (int n_joint, const int joints, const double spds) override

return_t	setRefAccelerationsRaw (const double *accs) override

return_t	setRefAccelerationsRaw (int n_joint, const int joints, const double accs) override

return_t	getRefSpeedsRaw (double *spds) override

return_t	getRefSpeedsRaw (int n_joint, const int joints, double spds) override

return_t	getRefAccelerationsRaw (double *accs) override

return_t	getRefAccelerationsRaw (int n_joint, const int joints, double accs) override

return_t	stopRaw () override

return_t	stopRaw (int n_joint, const int *joints) override

return_t	getTargetPositionsRaw (double *refs) override

return_t	getTargetPositionsRaw (int n_joint, const int joints, double refs) override

return_t	setPositionsRaw (const double *refs) override

return_t	setPositionsRaw (int n_joint, const int joints, const double refs) override

return_t	getRefPositionsRaw (double *refs) override

return_t	getRefPositionsRaw (int n_joint, const int joints, double refs) override

return_t	getRefTorqueRaw (int j, double *t) override

return_t	getRefTorquesRaw (double *t) override

return_t	setRefTorqueRaw (int j, double t) override

return_t	setRefTorquesRaw (int n_joint, const int joints, const double t) override

return_t	setRefTorquesRaw (const double *t) override

return_t	getTorqueRaw (int j, double *t) override

return_t	getTorquesRaw (double *t) override

return_t	getTorqueRangeRaw (int j, double min, double max) override

return_t	getTorqueRangesRaw (double min, double max) override

return_t	getMotorTorqueParamsRaw (int j, yarp::dev::MotorTorqueParameters *params) override

return_t	setMotorTorqueParamsRaw (int j, const yarp::dev::MotorTorqueParameters params) override

return_t	velocityMoveRaw (const double *sp) override

return_t	velocityMoveRaw (int n_joint, const int joints, const double spds) override

return_t	getRefVelocitiesRaw (double *vels) override

return_t	getRefVelocitiesRaw (int n_joint, const int joints, double vels) override

Public Member Functions inherited from roboticslab::ICanBusSharer
virtual	~ICanBusSharer ()=default
	Destructor.

Public Member Functions inherited from roboticslab::ICanMessageNotifier
virtual	~ICanMessageNotifier ()=default
	Virtual destructor.

Private Member Functions
void	interpretModesOfOperation (std::int8_t modesOfOperation) override

void	interpretIpStatus (std::uint16_t ipStatus) override

void	onPositionLimitTriggered () override

void	reset () override

Private Attributes
InterpolatedPositionBuffer *	ipBuffer {nullptr}

std::string	ipMode

int	ipPeriodMs {0}

std::bitset< 16 >	ipStatus

std::atomic< bool >	ipMotionStarted {false}

std::atomic< bool >	ipBufferFilled {false}

std::atomic< bool >	ipBufferEnabled {false}

std::atomic< bool >	enableSync {false}

std::atomic< bool >	enableCsv {false}

std::atomic< double >	targetPosition {0.0}

std::atomic< double >	targetVelocity {0.0}

Additional Inherited Members
Public Types inherited from roboticslab::TechnosoftIposBase
using	return_t = bool

Static Public Attributes inherited from roboticslab::TechnosoftIposBase
static constexpr auto	ret_not_ok = false

Protected Types inherited from roboticslab::TechnosoftIposBase
enum	report_level { NONE , INFO , WARN , FAULT }

enum	limit_switch { POSITIVE , NEGATIVE , INACTIVE }

Protected Member Functions inherited from roboticslab::TechnosoftIposBase
bool	reportBitToggle (report_storage report, int level, std::size_t pos, const char msgSet, const char msgReset=nullptr)

bool	awaitControlMode (yarp::conf::vocab32_t mode)
	Wait with timeout for requested control mode change.

double	degreesToInternalUnits (double value, int derivativeOrder=0) const
	Convert position, speed or acceleration to internal units.

double	internalUnitsToDegrees (double value, int derivativeOrder=0) const
	Convert position, speed or acceleration to degrees.

std::int16_t	currentToInternalUnits (double value) const
	Convert current to internal units.

double	internalUnitsToCurrent (std::int16_t value) const
	Convert current to amperes.

double	internalUnitsToPeakCurrent (std::int16_t value) const
	Apply internal iPOS conversion to express drive peak current in amperes.

double	currentToTorque (double current) const
	Convert current (amperes) to torque (Nm).

double	torqueToCurrent (double torque) const
	Convert torque (Nm) to current (amperes).

Protected Attributes inherited from roboticslab::TechnosoftIposBase
CanOpenNode *	can {nullptr}

CommandBuffer	commandBuffer

std::unique_ptr< StateObserver >	controlModeObserverPtr {new StateObserver(1.0)}

std::unique_ptr< EncoderRead >	lastEncoderRead {nullptr}

std::bitset< 16 >	msr

std::bitset< 16 >	mer

std::bitset< 16 >	der

std::bitset< 16 >	der2

std::bitset< 16 >	cer

std::int8_t	modesOfOperation {0}

DriveState	driveState {DriveState::NOT_READY_TO_SWITCH_ON}

bool	configuredOnce {false}

std::atomic< std::int16_t >	lastCurrentRead {0}

std::atomic< yarp::conf::vocab32_t >	actualControlMode {0}

std::atomic< yarp::conf::vocab32_t >	requestedcontrolMode {0}

std::atomic< double >	tr {0.0}

std::atomic< double >	k {0.0}

std::atomic< int >	encoderPulses {0}

std::atomic< double >	maxVel {0.0}

std::atomic< double >	min {0.0}

std::atomic< double >	max {0.0}

std::atomic< double >	refSpeed {0.0}

std::atomic< double >	refAcceleration {0.0}

std::atomic< double >	lastHeartbeat {0.0}

std::atomic< std::uint8_t >	lastNmtState {0}

std::atomic< std::uint16_t >	lastFaultCode {0}

std::atomic< const char * >	lastFaultMessage

std::atomic< limit_switch >	limitSwitchState {INACTIVE}

yarp::conf::vocab32_t	initialControlMode {0}

yarp::conf::vocab32_t	jointType {0}

PdoConfiguration	tpdo1Conf

PdoConfiguration	tpdo2Conf

PdoConfiguration	tpdo3Conf

const TechnosoftIpos_ParamsParser &	params

Member Function Documentation

◆ interpretIpStatus()

void TechnosoftIposEmbedded::interpretIpStatus ( std::uint16_t ipStatus )

overrideprivatevirtual

Reimplemented from roboticslab::TechnosoftIposBase.

◆ interpretModesOfOperation()

void TechnosoftIposEmbedded::interpretModesOfOperation ( std::int8_t modesOfOperation )

overrideprivatevirtual

Implements roboticslab::TechnosoftIposBase.

◆ onPositionLimitTriggered()

void TechnosoftIposEmbedded::onPositionLimitTriggered ( )

overrideprivatevirtual

Implements roboticslab::TechnosoftIposBase.

◆ reset()

void TechnosoftIposEmbedded::reset ( )

overrideprivatevirtual

Implements roboticslab::TechnosoftIposBase.

◆ synchronize()

bool TechnosoftIposEmbedded::synchronize ( double timestamp )

overridevirtual

Implements roboticslab::ICanBusSharer.

The documentation for this class was generated from the following files:

libraries/YarpPlugins/TechnosoftIpos/embedded-pid/TechnosoftIposEmbedded.hpp
libraries/YarpPlugins/TechnosoftIpos/embedded-pid/DeviceDriverImpl.cpp
libraries/YarpPlugins/TechnosoftIpos/embedded-pid/ICanBusSharerImpl.cpp
libraries/YarpPlugins/TechnosoftIpos/embedded-pid/IControlModeRawImpl.cpp
libraries/YarpPlugins/TechnosoftIpos/embedded-pid/IPositionControlRawImpl.cpp
libraries/YarpPlugins/TechnosoftIpos/embedded-pid/IPositionDirectRawImpl.cpp
libraries/YarpPlugins/TechnosoftIpos/embedded-pid/IRemoteVariablesRawImpl.cpp
libraries/YarpPlugins/TechnosoftIpos/embedded-pid/IVelocityControlRawImpl.cpp
libraries/YarpPlugins/TechnosoftIpos/embedded-pid/TechnosoftIposEmbedded.cpp

Public Member Functions

Private Member Functions

Private Attributes

Additional Inherited Members

Member Function Documentation

◆ interpretIpStatus()

◆ interpretModesOfOperation()

◆ onPositionLimitTriggered()

◆ reset()

◆ synchronize()