Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Added emergency nortification for temperature and torque to ThermoLimiter #229

Merged
merged 15 commits into from
Oct 10, 2014
Merged

Added emergency nortification for temperature and torque to ThermoLimiter #229

Show file tree
Hide file tree
Changes from 1 commit
Commits
Show all changes
15 commits
Select commit Hold shift + click to select a range
a1f3b54
Forgotten commit, added init_beep to ThermoLimiter
orikuma May 20, 2014
5c6ac94
Separate isTempError check from limitTemperature and rename calcMaxTo…
orikuma May 20, 2014
eea07d4
Added current/max value to debug message
orikuma May 21, 2014
7223894
Added input buffer size check before calcEmergencyRate
orikuma May 21, 2014
29d2175
Added isDebug method which is alternative of DEBUGP
orikuma May 21, 2014
a309dd9
Debugged initialization problem in motorTemperatureLimit and Modified…
orikuma May 23, 2014
06d8afd
Removed tauIn and qCurrentIn in ThermoLimiter, which is not used for …
orikuma May 29, 2014
9f4c397
Replace DEBUGP to isDebug function in ThermoEstimator
orikuma May 29, 2014
aa5703b
Added emergency beep (notification of current load > max load) to The…
orikuma May 29, 2014
980e425
Added alarmRatio to config property, which defines current/max ratio …
orikuma May 29, 2014
7b6f7f1
Separated estimateJointTemperature process to member function
orikuma May 29, 2014
f6aa5dd
Added error2torque conversion to ThermoEstimator and make servoState …
orikuma May 29, 2014
03ff6f0
Debugged read port miss in inports and modified debug message position
orikuma May 29, 2014
9898518
Fixed default tauMax from model. climit -> climit*gearRatio*torqueConst
orikuma Jul 8, 2014
e4073a2
Connect q and qRef to ThermoEstimator to estimate joint torque from e…
orikuma Sep 12, 2014
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Prev Previous commit
Next Next commit
Added isDebug method which is alternative of DEBUGP
  • Loading branch information
@orikuma
orikuma committed May 21, 2014
commit 29d21757c6456890f9a2f37596ce9664dd5cf83c
25 changes: 14 additions & 11 deletions rtc/ThermoLimiter/ThermoLimiter.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -14,7 +14,6 @@
#include <hrpUtil/MatrixSolvers.h>
#include <cmath>

#define DEBUGP ((m_debugLevel==1 && loop%200==0) || m_debugLevel > 1 )
#define DQ_MAX 1.0

// Module specification
Expand Down Expand Up @@ -193,10 +192,9 @@ RTC::ReturnCode_t ThermoLimiter::onDeactivated(RTC::UniqueId ec_id)
RTC::ReturnCode_t ThermoLimiter::onExecute(RTC::UniqueId ec_id)
{
// std::cout << m_profile.instance_name<< ": onExecute(" << ec_id << "), data = " << m_data.data << std::endl;
static long long loop = 0;
loop ++;
m_loop++;

if (DEBUGP) {
if (isDebug()) {
std::cerr << "[" << m_profile.instance_name << "]" << std::endl;
}

Expand Down Expand Up @@ -224,7 +222,7 @@ RTC::ReturnCode_t ThermoLimiter::onExecute(RTC::UniqueId ec_id)
m_qCurrentInIn.read();
}

if (DEBUGP) {
if (isDebug()) {
std::cerr << "temperature: ";
for (int i = 0; i < m_tempIn.data.length(); i++) {
std::cerr << " " << m_tempIn.data[i];
Expand All @@ -247,7 +245,7 @@ RTC::ReturnCode_t ThermoLimiter::onExecute(RTC::UniqueId ec_id)
}
}

if (DEBUGP) {
if (isDebug()) {
std::cerr << "tauMax: ";
for (int i = 0; i < tauMax.size(); i++) {
std::cerr << tauMax[i] << " ";
Expand All @@ -263,8 +261,8 @@ RTC::ReturnCode_t ThermoLimiter::onExecute(RTC::UniqueId ec_id)
torqueLimitRatio = calcEmergencyRatio(m_tauIn, tauMax, alarmRatio, torqueLimitPrefix);
}

// call beep
callBeep(3136, std::max(thermoLimitRatio, torqueLimitRatio), alarmRatio);
// call beep (3136/0.8=3920)
callBeep(3920, std::max(thermoLimitRatio, torqueLimitRatio), alarmRatio);

// output restricted tauMax
for (int i = 0; i < m_robot->numJoints(); i++) {
Expand Down Expand Up @@ -316,8 +314,8 @@ void ThermoLimiter::calcMaxTorqueFromTemperature(hrp::dvector &tauMax)
double temp, tempLimit;
hrp::dvector squareTauMax(numJoints);

if ( m_tempIn.data.length() == m_robot->numJoints()
&& m_tauIn.data.length() == m_robot->numJoints() ) {
if (m_tempIn.data.length() == m_robot->numJoints()
&& m_tauIn.data.length() == m_robot->numJoints()) {

for (int i = 0; i < numJoints; i++) {
temp = m_tempIn.data[i];
Expand Down Expand Up @@ -346,7 +344,7 @@ double ThermoLimiter::calcEmergencyRatio(RTC::TimedDoubleSeq &current, hrp::dvec
for (int i = 0; i < current.data.length(); i++) {
double tmpEmergencyRatio = std::abs(current.data[i] / max[i]);
if (tmpEmergencyRatio > alarmRatio) {
std::cerr << prefix << "[" << i << "]" << " is over " << alarmRatio << "of the limit.";
std::cerr << prefix << "[" << m_robot->joint(i)->name << "]" << " is over " << alarmRatio << " of the limit.";
if (m_debugLevel > 0) {
std::cerr << ": " << current.data[i] << ">" << max[i];
}
Expand All @@ -371,6 +369,11 @@ void ThermoLimiter::callBeep(int maxFreq, double ratio, double alarmRatio)
return;
}

bool ThermoLimiter::isDebug(int cycle)
{
return ((m_debugLevel==1 && m_loop%cycle==0) || m_debugLevel > 1);
}

extern "C"
{

Expand Down
2 changes: 2 additions & 0 deletions rtc/ThermoLimiter/ThermoLimiter.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -145,6 +145,7 @@ class ThermoLimiter

private:
double m_dt;
long long m_loop;
unsigned int m_debugLevel;
hrp::dvector m_motorTemperatureLimit;
hrp::BodyPtr m_robot;
Expand All @@ -153,6 +154,7 @@ class ThermoLimiter
void calcMaxTorqueFromTemperature(hrp::dvector &tauMax);
double calcEmergencyRatio(RTC::TimedDoubleSeq &current, hrp::dvector &max, double alarmRatio, std::string &prefix);
void callBeep(int maxFreq, double ratio, double alarmRatio);
bool isDebug(int cycle = 200);
};


Expand Down