Skip to content

Commit

Permalink
small improvements in eobObjParser (wip)
Browse files Browse the repository at this point in the history
  • Loading branch information
@valegagge @MSECode
valegagge authored and MSECode committed Nov 21, 2023
1 parent 4119589 commit 27c02ef
Show file tree
Hide file tree
Showing 4 changed files with 55 additions and 60 deletions.
62 changes: 35 additions & 27 deletions src/libraries/icubmod/embObjMotionControl/embObjMotionControl.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -99,7 +99,6 @@ bool embObjMotionControl::alloc(int nj)
_gearbox_M2J = allocAndCheck<double>(nj);
_gearbox_E2J = allocAndCheck<double>(nj);
_deadzone = allocAndCheck<double>(nj);
_temperatureValues = allocAndCheck<double>(nj);
_foc_based_info= allocAndCheck<eomc::focBasedSpecificInfo_t>(nj);
_trj_pids= new eomc::PidInfo[nj];
//_dir_pids= new eomc::PidInfo[nj];
Expand Down Expand Up @@ -150,7 +149,6 @@ bool embObjMotionControl::dealloc()
checkAndDestroy(_gearbox_M2J);
checkAndDestroy(_gearbox_E2J);
checkAndDestroy(_deadzone);
checkAndDestroy(_temperatureValues);
checkAndDestroy(_impedance_limits);
checkAndDestroy(checking_motiondone);
checkAndDestroy(_ref_command_positions);
Expand Down Expand Up @@ -226,7 +224,6 @@ embObjMotionControl::embObjMotionControl() :
_gearbox_M2J = 0;
_gearbox_E2J = 0;
_deadzone = 0;
_temperatureValues = NULL;
opened = 0;
_trj_pids = NULL;
//_dir_pids = NULL;
Expand Down Expand Up @@ -988,15 +985,15 @@ bool embObjMotionControl::fromConfig_Step2(yarp::os::Searchable &config)

for (j = 0; j < _njoints; j++)
{
if (((_foc_based_info[j].temperatureSensorType != motor_temperature_sensor_none ) || (_foc_based_info[j].hasTempSensor != 0)) && ((_temperatureLimits[j].hardwareTemperatureLimit == 0) || (_temperatureLimits[j].warningTemperatureLimit == 0)))
if (((_temperatureSensorsVector.at(j)->getType() != motor_temperature_sensor_none )) && ((_temperatureLimits[j].hardwareTemperatureLimit == 0) || (_temperatureLimits[j].warningTemperatureLimit == 0)))
{
yError() << "In" << getBoardInfo() << "joint" << j << ": inconsistent configuration, please update it. If Temperature limits are not set then TemperatureSensorType must be NONE or not set and/or HasTempSensor must be zero. Aborting...";
return false;
}

if (_foc_based_info[j].temperatureSensorType == motor_temperature_sensor_none)
if (_temperatureSensorsVector.at(j)->getType() == motor_temperature_sensor_none)
{
yWarning() << "embObjMC " << getBoardInfo() << "joint " << j << " has motor not provided with any available type of temperature sensor. If needed update the configurations file accordingly";
yInfo() << "embObjMC " << getBoardInfo() << "joint " << j << " has motor not provided with any available type of temperature sensor. If needed update the configurations file accordingly";
}
}
}
Expand Down Expand Up @@ -1576,10 +1573,10 @@ bool embObjMotionControl::update(eOprotID32_t id32, double timestamp, void *rxda
if((eoprot_entity_mc_joint == ent) && (eoprot_tag_mc_joint_status_debug == tag) && (joint < mcdiagnostics.ports.size()))
{

eOprotID32_t id32 = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, joint, eoprot_tag_mc_joint_status_core);
eOprotID32_t id32sc = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, joint, eoprot_tag_mc_joint_status_core);
eOmc_joint_status_core_t jcore = {};

res->getLocalValue(id32, &jcore);
res->getLocalValue(id32sc, &jcore);

int32_t *debug32 = reinterpret_cast<int32_t*>(rxdata);
// write into relevant port
Expand All @@ -1596,7 +1593,7 @@ bool embObjMotionControl::update(eOprotID32_t id32, double timestamp, void *rxda
}
}

if((eoprot_entity_mc_motor == ent) && (eoprot_tag_mc_motor_status_basic == tag))
if((eoprot_entity_mc_joint == ent) && (eoprot_tag_mc_motor_status_basic == tag))
{
if(false == initialised())
return true;
Expand All @@ -1605,26 +1602,37 @@ bool embObjMotionControl::update(eOprotID32_t id32, double timestamp, void *rxda
if((_temperatureSensorsVector.at(motor)->getType() == motor_temperature_sensor_none))
return true;

eOmc_motor_status_basic_t *mc_motor_status_basic = reinterpret_cast<eOmc_motor_status_basic_t*>(rxdata);
if((double)mc_motor_status_basic->mot_temperature != temperatureErrorValue_s) //I get a valid value
eOmc_motor_status_basic_t mc_motor_status_basic = {};
id32 = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, motor, eoprot_tag_mc_motor_status_basic);

if (!res->getLocalValue(id32, &mc_motor_status_basic))
{
yError() << getBoardInfo() << "getLocalValue() cannot retrieve motor" << motor << "status basic";
return true; //do we need to actually return false here since is eth error
}

//eOmc_motor_status_basic_t *mc_motor_status_basic = reinterpret_cast<eOmc_motor_status_basic_t*>(rxdata);

if((double)mc_motor_status_basic.mot_temperature != temperatureErrorValue_s) //I get a valid value
{
double tmp = _temperatureSensorsVector.at(motor)->convertRawToTempCelsius((double)mc_motor_status_basic->mot_temperature);
double tmp = _temperatureSensorsVector.at(motor)->convertRawToTempCelsius((double)mc_motor_status_basic.mot_temperature);

if (tmp > _temperatureLimits[motor].warningTemperatureLimit)
{
if(! _temperatureExceededLimitWatchdog.at(motor).isStarted())
{
yWarning() << getBoardInfo() << "Motor" << motor << "The temperature (" << tmp << "[℃] ) exceeds the warning limit (" << _temperatureLimits[motor].warningTemperatureLimit << "[℃] ). Processes not stopped but it i strongly recommended decreasing motor usage or reducing currents and PWMs to not risk motor damaging";
_temperatureExceededLimitWatchdog.at(motor).start();
yWarning() << getBoardInfo() << "Motor" << motor << "The temperature (" << tmp << "[ ℃ ] ) exceeds the warning limit (" << _temperatureLimits[motor].warningTemperatureLimit << "[ ℃ ] ). Processes not stopped but it is strongly recommended decreasing motor usage or reducing currents and PWMs to not risk motor damaging";
_temperatureExceededLimitWatchdog.at(motor).start();
}
else
{
if(_temperatureExceededLimitWatchdog.at(motor).isExpired())
{
if(_temperatureExceededLimitWatchdog.at(motor).isExpired())
{
yWarning() << getBoardInfo() << "Motor" << motor << "The temperature (" << tmp << "[℃] ) exceeds the warning limit (" << _temperatureLimits[motor].warningTemperatureLimit << "[℃] ) again!. Processes not stopped but it i strongly recommended decreasing motor usage or reducing currents and PWMs to not risk motor damaging";
_temperatureExceededLimitWatchdog.at(motor).start();
}
_temperatureExceededLimitWatchdog.at(motor).decrement();
yWarning() << getBoardInfo() << "Motor" << motor << "The temperature (" << tmp << "[ ℃ ] ) exceeds the warning limit (" << _temperatureLimits[motor].warningTemperatureLimit << "[ ℃ ] ) again!. Processes not stopped but it is strongly recommended decreasing motor usage or reducing currents and PWMs to not risk motor damaging";
_temperatureExceededLimitWatchdog.at(motor).start();
}
_temperatureExceededLimitWatchdog.at(motor).increment();
}
}
else
{
Expand All @@ -1636,16 +1644,16 @@ bool embObjMotionControl::update(eOprotID32_t id32, double timestamp, void *rxda
if(! _temperatureSensorErrorWatchdog.at(motor).isStarted())
{
yError() << getBoardInfo() << "At timestamp" << yarp::os::Time::now() << "In motor" << motor << "cannot read Temperature from I2C. There might be cabling problems, TDB cable might be broken or sensor unreachable";
_temperatureSensorErrorWatchdog.at(motor).start();
_temperatureSensorErrorWatchdog.at(motor).start();
}
else
{
_temperatureSensorErrorWatchdog.at(motor).increment();
if( _temperatureSensorErrorWatchdog.at(motor).isExpired())
{
yError()<< getBoardInfo() << "Motor" << motor << "gets " << _temperatureSensorErrorWatchdog.at(motor).getCount() << "reading errors in " << yarp::os::Time::now() - _temperatureSensorErrorWatchdog.at(motor).getStartTime() << "seconds";
yError()<< getBoardInfo() << "Motor" << motor << "failed to read temperature for" << yarp::os::Time::now() - _temperatureSensorErrorWatchdog.at(motor).getStartTime() << "seconds";
_temperatureSensorErrorWatchdog.at(motor).start();
}
_temperatureSensorErrorWatchdog.at(motor).decrement();
}
}
}
Expand Down Expand Up @@ -3361,11 +3369,11 @@ bool embObjMotionControl::getTemperatureSensorTypeRaw(int j, std::string& ret)
{
// refresh cached value when reading data from the EMS
ret = "NONE";
if (_foc_based_info[j].temperatureSensorType = motor_temperature_sensor_pt100)
if (_temperatureSensorsVector.at(j)->getType() == motor_temperature_sensor_pt100)
{
ret = "PT100";
}
else if (_foc_based_info[j].temperatureSensorType = motor_temperature_sensor_pt1000)
else if (_temperatureSensorsVector.at(j)->getType() == motor_temperature_sensor_pt1000)
{
ret = "PT1000";
}
Expand Down Expand Up @@ -4801,13 +4809,13 @@ bool embObjMotionControl::getTemperatureRaw(int m, double* val)
bool ret = res->getLocalValue(protid, &status);
if(!ret)
{
yError() << "embObjMotionControl::getTemperatureRaw failed to complete getLocalValue() for " << getBoardInfo() << " Setting mot_temperature" << m << "to" << *val;
yError() << getBoardInfo() << "At timestamp" << yarp::os::Time::now() << "In motor" << m << "embObjMotionControl::getTemperatureRaw failed to complete getLocalValue()";
return ret;
}

if (((double)status.mot_temperature) == temperatureErrorValue_s) //using -5000 as the default value on 2FOC for initializing the temperature. If cannot read from I2C the value cannot be updated
{
yError() << getBoardInfo() << "At timestamp" << yarp::os::Time::now() << "In motor" << m << "cannot read Temperature from I2C. There might be cabling problems, TDB cable might be broken or sensor unreachable";
yError() << getBoardInfo() << "At timestamp" << yarp::os::Time::now() << "In motor" << m << "cannot read Temperature" << *val << "from I2C. There might be cabling problems, TDB cable might be broken or sensor unreachable";
return false;
}

Expand Down
9 changes: 4 additions & 5 deletions src/libraries/icubmod/embObjMotionControl/embObjMotionControl.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -119,8 +119,8 @@ double _time;

public:

Watchdog(): _count(1000), _isStarted(false), _threshold(1000), _time(0){;}
Watchdog(uint32_t threshold):_count(threshold), _isStarted(false),_threshold(threshold), _time(0){;}
Watchdog(): _count(0), _isStarted(false), _threshold(500), _time(0){;}
Watchdog(uint32_t threshold):_count(0), _isStarted(false),_threshold(threshold), _time(0){;}
~Watchdog() = default;
Watchdog(const Watchdog& other) = default;
Watchdog(Watchdog&& other) noexcept = default;
Expand All @@ -129,9 +129,9 @@ Watchdog& operator=(Watchdog&& other) noexcept = default;


bool isStarted(){return _isStarted;}
void start() {_count = _threshold; _time = yarp::os::Time::now();}
void start() {_count = 0; _time = yarp::os::Time::now(); _isStarted = true;}
bool isExpired() {return (_count > _threshold);}
void decrement() {_count--;}
void increment() {++_count;}
void clear(){_isStarted=false;}
double getStartTime() {return _time;}
uint32_t getCount() {return _count; }
Expand Down Expand Up @@ -236,7 +236,6 @@ class yarp::dev::embObjMotionControl: public DeviceDriver,
double * _gearbox_M2J; /** the gearbox ratio motor to joint */
double * _gearbox_E2J; /** the gearbox ratio encoder to joint */
double * _deadzone;
double * _temperatureValues;
std::vector<eomc::kalmanFilterParams_t> _kalman_params; /** Kalman filter parameters */

std::vector<std::unique_ptr<eomc::ITemperatureSensor>> _temperatureSensorsVector;
Expand Down
39 changes: 14 additions & 25 deletions src/libraries/icubmod/embObjMotionControl/eomcParser.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1376,37 +1376,28 @@ bool Parser::parseFocGroup(yarp::os::Searchable &config, eomc::focBasedSpecificI

if (!extractGroup(focGroup, xtmp, "TemperatureSensorType", "TemperatureSensorType PT100/PT1000/NONE ", _njoints, false))
{
yWarning("In board %s group TemperatureSensorType not filled. Setting it to NONE as default value. If needed update the configuration files accordingly", _boardname.c_str()) ;
for (i = 0; i < (unsigned)_njoints; i++)
{
foc_based_info[i].temperatureSensorType = motor_temperature_sensor_none;
foc_based_info[i].hasTempSensor = 0;
temperatureSensorsVector.at(i) = std::make_unique<eomc::TemperatureSensorNONE>();
}

if (!extractGroup(focGroup, xtmp, "HasTempSensor", "HasTempSensor 0/1 ", _njoints, false))
{
yWarning("In board %s group HasTempSensor not filled. Setting it to 0 as default value. If needed update the configuration files accordingly", _boardname.c_str());
for (i = 0; i < (unsigned)_njoints; i++)
foc_based_info[i].hasTempSensor = 0;
}
else
// 1. check if I have old config
if (extractGroup(focGroup, xtmp, "HasTempSensor", "HasTempSensor 0/1 ", _njoints, false))
{
yWarning() << "ATTENTION HasTempSensor will be soon DEPRECATED in favour of TemperatureSensorType. Currently kept for backward compatibility but update your configuration files if using a Temperature Sensor";
for (i = 1; i < xtmp.size(); i++)
{
if (xtmp.get(i).asInt32() != 0)
{
yError() << "In " << _boardname << "entry" << i << ": inconsistent configuration. HasTempSensor cannot be used alone. Will be soon deprecated. Use TemperatureSensorType in 2FOC group and set Temperature limits in LIMITS group." ;
return false;
}
else
{
foc_based_info[i - 1].hasTempSensor = xtmp.get(i).asInt32();
}
}
}
}

// if I'm here all joints have HasTempSensor =0
for (i = 0; i < _njoints; i++)
{
foc_based_info[i].hasTempSensor = 0;
temperatureSensorsVector.at(i) = std::make_unique<eomc::TemperatureSensorNONE>();
yWarning()<< _boardname << "ATTENTION HasTempSensor will be soon DEPRECATED in favour of TemperatureSensorType (PT100, PT1000, NONE(=default)). Currently kept for backward compatibility but update your configuration files if using a Temperature Sensor";
}

}
else
{
for (i = 1; i < xtmp.size(); i++)
Expand All @@ -1415,22 +1406,20 @@ bool Parser::parseFocGroup(yarp::os::Searchable &config, eomc::focBasedSpecificI
if(s == "PT100")
{
foc_based_info[i - 1].hasTempSensor = 1;
foc_based_info[i - 1].temperatureSensorType = motor_temperature_sensor_pt100;
temperatureSensorsVector.at(i-1) = std::make_unique<eomc::TemperatureSensorPT100>();

}
else if (s == "PT1000")
{

foc_based_info[i - 1].hasTempSensor = 1;
foc_based_info[i - 1].temperatureSensorType = motor_temperature_sensor_pt1000;
temperatureSensorsVector.at(i-1) = std::make_unique<eomc::TemperatureSensorPT1000>();
}
else
{
yWarning("Not available or Not supported TemperatureSensorType: %s. Setting NONE as default", s.c_str());
if(s != "NONE")//if sis == NONE the warning is not correct
yWarning("Not available or Not supported TemperatureSensorType: %s. Setting NONE as default", s.c_str());
foc_based_info[i - 1].hasTempSensor = 0;
foc_based_info[i - 1].temperatureSensorType = motor_temperature_sensor_none;
temperatureSensorsVector.at(i-1) = std::make_unique<eomc::TemperatureSensorNONE>();
}
}
Expand Down
5 changes: 2 additions & 3 deletions src/libraries/icubmod/embObjMotionControl/eomcParser.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -256,14 +256,14 @@ class TemperatureSensorNONE : public ITemperatureSensor

TemperatureSensorNONE()
{
yError("Private varibales NOT DEFINED for class TemperatureSensorNONE");
yInfo("Private varibales NOT DEFINED for class TemperatureSensorNONE");
}

~TemperatureSensorNONE() = default;

virtual double convertTempCelsiusToRaw(const double temperature) override
{
yError("convertTempCelsiusToRaw METHOD, NOT IMPLEMENTED for class TemperatureSensorNONE");
yInfo("convertTempCelsiusToRaw METHOD, NOT IMPLEMENTED for class TemperatureSensorNONE");
return 0;
}

Expand Down Expand Up @@ -425,7 +425,6 @@ typedef struct
{
bool hasHallSensor;
bool hasTempSensor;
motor_temperatureSensorTypeEnum_t temperatureSensorType; // uses a typedef enum
bool hasRotorEncoder;
bool hasRotorEncoderIndex;
int rotorIndexOffset;
Expand Down

0 comments on commit 27c02ef

Please sign in to comment.