Skip to content

Commit

Permalink
Update main. working on robots
Browse files Browse the repository at this point in the history
  • Loading branch information
@MSECode
ergocub authored and MSECode committed Nov 15, 2023
1 parent f1eb897 commit cd40f67
Show file tree
Hide file tree
Showing 4 changed files with 53 additions and 41 deletions.
77 changes: 44 additions & 33 deletions src/libraries/icubmod/embObjMotionControl/embObjMotionControl.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,6 +12,7 @@
#include <yarp/os/Time.h>
#include <string.h>
#include <iostream>
#include <cmath>

#include "embObjMotionControl.h"
#include <ethManager.h>
Expand Down Expand Up @@ -102,7 +103,7 @@ bool embObjMotionControl::alloc(int nj)
_gearbox_E2J = allocAndCheck<double>(nj);
_deadzone = allocAndCheck<double>(nj);
_temperatureValues = allocAndCheck<double>(nj);
_foc_based_info=allocAndCheck<eomc::focBasedSpecificInfo_t>(nj);
_foc_based_info= allocAndCheck<eomc::focBasedSpecificInfo_t>(nj);
_trj_pids= new eomc::PidInfo[nj];
//_dir_pids= new eomc::PidInfo[nj];
_trq_pids= new eomc::TrqPidInfo [nj];
Expand All @@ -124,7 +125,6 @@ bool embObjMotionControl::alloc(int nj)
_calibrated = allocAndCheck<bool>(nj);
_cacheImpedance = allocAndCheck<eOmc_impedance_t>(nj);


_rotorsLimits.resize(nj);
_jointsLimits.resize(nj);
_currentLimits.resize(nj);
Expand All @@ -137,8 +137,10 @@ bool embObjMotionControl::alloc(int nj)
_jointEncs.resize(nj);
_motorEncs.resize(nj);
_kalman_params.resize(nj);

//debug purpose
_temperatureSensorsVector.resize(nj);
_temperatureSensorErrorWatchdog.resize(nj, 1000); // use 1000 as limit on the watchdog for the error on the temperature sensor receiving of the values -
// since the ETH callback timing is 5ms by default so using 1000s we can set a checking threshould of 1 second
// in which we can allow the tdb to not respond. If cannot receive response over 1s we trigger the error

return true;
}
Expand All @@ -164,7 +166,6 @@ bool embObjMotionControl::dealloc()
checkAndDestroy(_calibrated);
checkAndDestroy(_foc_based_info);


if(_trj_pids)
delete [] _trj_pids;

Expand Down Expand Up @@ -219,12 +220,14 @@ embObjMotionControl::embObjMotionControl() :
_axesInfo(0),
_jointEncs(0),
_motorEncs(0),
_kalman_params(0)
_kalman_params(0),
_temperatureSensorsVector(0),
_temperatureSensorErrorWatchdog(0)
{
_gearbox_M2J = 0;
_gearbox_E2J = 0;
_deadzone = 0;
_temperatureValues = 0;
_temperatureValues = NULL;
opened = 0;
_trj_pids = NULL;
//_dir_pids = NULL;
Expand Down Expand Up @@ -1557,6 +1560,26 @@ bool embObjMotionControl::update(eOprotID32_t id32, double timestamp, void *rxda
{ // do it only if we already have opened the device
std::lock_guard<std::mutex> lck(_mutex);
_encodersStamp[joint] = timestamp;


{
if(!getTemperaturesRaw(_temperatureValues))
{
// yError() << "embObjMotionControl::getTemperaturesRaw failed for" << getBoardInfo();
return false;
}
else
{
for (uint8_t i = 0; i < _njoints; i++)
{
if (_temperatureValues[i] > _temperatureLimits[i].warningTemperatureLimit)
{
yWarning() << getBoardInfo() << "At joint" << joint << "temperature limit for motor" << i << " overcame! Processes not stopped but consider to decrese motor usage or reduce currents and PWMs to not risk motor damaging";
}
}
}
}

}


Expand Down Expand Up @@ -1591,25 +1614,7 @@ bool embObjMotionControl::update(eOprotID32_t id32, double timestamp, void *rxda
mcdiagnostics.ports[joint]->write();
}
}

if(!getTemperaturesRaw(_temperatureValues))
{
// yError() << "embObjMotionControl::getTemperaturesRaw failed for" << getBoardInfo();
return false;
}
else
{
for (uint8_t i = 0; i < _njoints; i++)
{
if (_temperatureValues[i] > _temperatureLimits[i].warningTemperatureLimit)
{
yWarning() << getBoardInfo() << " temperature limit for motor #" << i << " overcame! Processes not stopped but consider to decrese motor usage or reduce currents and PWMs to not risk motor damaging";
}
}
}



return true;
}

Expand Down Expand Up @@ -4751,26 +4756,32 @@ bool embObjMotionControl::getTemperatureRaw(int m, double* val)
eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, m, eoprot_tag_mc_motor_status_basic);

bool ret = res->getLocalValue(protid, &status);
*val = NAN;
if(ret)
{
*val = (double) status.mot_temperature ;
if (*val != -5000) //using -5000 as the default value on 2FOC for initializing the temperature. If cannot read from I2C the value cannot be updated
if (((double)status.mot_temperature) != -5000) //using -5000 as the default value on 2FOC for initializing the temperature. If cannot read from I2C the value cannot be updated
{
if (_foc_based_info[m].temperatureSensorType != motor_temperature_sensor_none)
if ((_foc_based_info[m].temperatureSensorType != motor_temperature_sensor_none) && (serviceConfig.ethservice.configuration.type == eomn_serv_MC_foc))
{
*val = _temperatureSensorsVector.at(m)->convertRawToTempCelsius(*val);
*val = _temperatureSensorsVector.at(m)->convertRawToTempCelsius((double)status.mot_temperature);
}

}
else
{
yError() << "Cannot read Temperature from I2C. There might be cabling problems, TDB cable might be broken or sensor unreachable";
return false;
--_temperatureSensorErrorWatchdog.at(m);
if(_temperatureSensorErrorWatchdog.at(m) < 0)
{
_temperatureSensorErrorWatchdog.at(m) = 1000;
yError() << getBoardInfo() << "At protid" << protid << "In motor" << m << "cannot read Temperature from I2C. There might be cabling problems, TDB cable might be broken or sensor unreachable";
return false;
}

}
}
else
{
*val = 0;
yError() << "embObjMotionControl::getTemperatureRaw failed to complete getLocalValue() for " << getBoardInfo() << " Setting mot_temperature #" << m << " to " << *val;
yError() << "embObjMotionControl::getTemperatureRaw failed to complete getLocalValue() for " << getBoardInfo() << " Setting mot_temperature" << m << "to" << *val;
}

return ret;
Expand Down
1 change: 1 addition & 0 deletions src/libraries/icubmod/embObjMotionControl/embObjMotionControl.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -269,6 +269,7 @@ class yarp::dev::embObjMotionControl: public DeviceDriver,
#define MAX_POSITION_MOVE_INTERVAL 0.080
double *_last_position_move_time; /** time stamp for last received position move command*/
eOmc_impedance_t *_cacheImpedance; /* cache impedance value to split up the 2 sets */
std::vector<int> _temperatureSensorErrorWatchdog; /* counter used to filter error coming from tdb reading fromm 2FOC board*/


#ifdef NETWORK_PERFORMANCE_BENCHMARK
Expand Down
12 changes: 6 additions & 6 deletions src/libraries/icubmod/embObjMotionControl/eomcParser.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1415,22 +1415,22 @@ bool Parser::parseFocGroup(yarp::os::Searchable &config, eomc::focBasedSpecificI
{
foc_based_info[i - 1].hasTempSensor = 1;
foc_based_info[i - 1].temperatureSensorType = motor_temperature_sensor_pt100;
temperatureSensorsVector.push_back(std::make_unique<eomc::TemperatureSensorPT100>());
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.push_back(std::make_unique<eomc::TemperatureSensorPT1000>());
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());
foc_based_info[i - 1].hasTempSensor = 0;
foc_based_info[i - 1].temperatureSensorType = motor_temperature_sensor_none;
temperatureSensorsVector.push_back(std::make_unique<eomc::TemperatureSensorNONE>());
temperatureSensorsVector.at(i-1) = std::make_unique<eomc::TemperatureSensorNONE>();
}
}
}
Expand Down Expand Up @@ -1747,8 +1747,8 @@ bool Parser::parseTemperatureLimits(yarp::os::Searchable &config, std::vector<te
yWarning("hardwareTemperatureLimits param not found in config file for board %s. Please update robot configuration files or contact https://github.com/robotology/icub-support if needed. Using default values.", _boardname.c_str());
for (i = 0; i < (unsigned)_njoints; i++)
{
temperatureLimits[i].hardwareTemperatureLimit = 0;
temperatureLimits[i].warningTemperatureLimit = 0;
//temperatureLimits[i].hardwareTemperatureLimit = 0;
//temperatureLimits[i].warningTemperatureLimit = 0;
}
}
else
Expand All @@ -1759,7 +1759,7 @@ bool Parser::parseTemperatureLimits(yarp::os::Searchable &config, std::vector<te
// warning limit - parsing it only if hardwareTemperatureLimit available
yWarning("warningTemperatureLimits param not found in config file for board %s. Please update robot configuration files or contact https://github.com/robotology/icub-support if needed. Using default values.", _boardname.c_str());

for (i = 0; i < (unsigned)_njoints; i++) temperatureLimits[i].warningTemperatureLimit = 0;
//for (i = 0; i < (unsigned)_njoints; i++) temperatureLimits[i].warningTemperatureLimit = 0;
}
else
{
Expand Down
4 changes: 2 additions & 2 deletions src/libraries/icubmod/embObjMotionControl/eomcParser.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -151,7 +151,7 @@ class TemperatureSensorPT100 : public ITemperatureSensor
res = (tmp * (_r_1*_r_2 + _r_1*_r_3 + _ptc_offset*_r_2 + _ptc_offset*_r_3) / den) + ((_r_3*_r_1 - _r_2*_ptc_offset) / den);


yDebug("Converted temperature to Celsius degree value:%f", res);
//yDebug("Converted temperature to Celsius degree value:%f", res);
return res;
}

Expand Down Expand Up @@ -235,7 +235,7 @@ class TemperatureSensorPT1000 : public ITemperatureSensor
double den = _ptc_gradient * (_r_2 - _r_2*tmp - _r_3*tmp);
res = (tmp * (_r_1*_r_2 + _r_1*_r_3 + _ptc_offset*_r_2 + _ptc_offset*_r_3) / den) + ((_r_3*_r_1 - _r_2*_ptc_offset) / den);

yDebug("Converted temperature to Celsius degree value:%f", res);
//yDebug("Converted temperature to Celsius degree value:%f", res);

return res;
}
Expand Down

0 comments on commit cd40f67

Please sign in to comment.