static int CalcWindowEnergy(BLOCK_SWITCHING_CONTROL *blockSwitchingControl,
float *timeSignal,
int chIncrement,
int windowLen)
{
int w, i;
float accuUE,accuFE;
float tempUnfiltered, tempFiltered;
/* pointers for blockSwitchingControl->windowNrg[1][w],
blockSwitchingControl->windowNrgF[1][w]
*/
for (w=0; w < BLOCK_SWITCH_WINDOWS; w++) {
accuUE = 0.0;
accuFE = 0.0;
/* pointer for timeSignal[] */
for(i=0;i<windowLen;i++) {
tempUnfiltered = timeSignal[(windowLen * w + i) * chIncrement] ;
tempFiltered = IIRFilter(tempUnfiltered,hiPassCoeff,blockSwitchingControl->iirStates);
accuUE += (tempUnfiltered * tempUnfiltered);
accuFE += (tempFiltered * tempFiltered);
}
blockSwitchingControl->windowNrg[1][w] = accuUE;
blockSwitchingControl->windowNrgF[1][w] =accuFE;
}
return(TRUE);
}
RTC::ReturnCode_t TorqueFilter::onInitialize()
{
std::cout << m_profile.instance_name << ": onInitialize()" << std::endl;
// <rtc-template block="bind_config">
// Bind variables and configuration variable
bindParameter("debugLevel", m_debugLevel, "0");
// </rtc-template>
// Registration: InPort/OutPort/Service
// <rtc-template block="registration">
// Set InPort buffers
addInPort("qCurrent", m_qCurrentIn);
addInPort("tauIn", m_tauInIn);
// Set OutPort buffer
addOutPort("tauOut", m_tauOutOut);
// Set service provider to Ports
// Set service consumers to Ports
// Set CORBA Service Ports
// </rtc-template>
RTC::Properties& prop = getProperties();
coil::stringTo(m_dt, prop["dt"].c_str());
m_robot = hrp::BodyPtr(new hrp::Body());
RTC::Manager& rtcManager = RTC::Manager::instance();
std::string nameServer = rtcManager.getConfig()["corba.nameservers"];
int comPos = nameServer.find(",");
if (comPos < 0){
comPos = nameServer.length();
}
nameServer = nameServer.substr(0, comPos);
RTC::CorbaNaming naming(rtcManager.getORB(), nameServer.c_str());
if (!loadBodyFromModelLoader(m_robot, prop["model"].c_str(),
CosNaming::NamingContext::_duplicate(naming.getRootContext())
)){
std::cerr << "failed to load model[" << prop["model"] << "] in "
<< m_profile.instance_name << std::endl;
return RTC::RTC_ERROR;
}
// init outport
m_tauOut.data.length(m_robot->numJoints());
// set gravity compensation flag
coil::stringTo(m_is_gravity_compensation, prop["gravity_compensation"].c_str());
if (m_debugLevel > 0) {
std::cerr << m_profile.instance_name << " : gravity compensation flag: " << m_is_gravity_compensation << std::endl;
}
// set torque offset
// offset = -(gear torque in neutral pose)
m_torque_offset.resize(m_robot->numJoints());
coil::vstring torque_offset = coil::split(prop["torque_offset"], ",");
if ( m_torque_offset.size() == torque_offset.size() && m_debugLevel > 0) {
for(int i = 0; i < m_robot->numJoints(); i++){
coil::stringTo(m_torque_offset[i], torque_offset[i].c_str());
std::cerr << "[" << m_profile.instance_name << "]" << "offset[" << m_robot->joint(i)->name << "]: " << m_torque_offset[i] << std::endl;
}
} else {
if (m_debugLevel > 0) {
std::cerr << "[" << m_profile.instance_name << "]" << "Size of torque_offset is not correct." << std::endl;
std::cerr << "[" << m_profile.instance_name << "]" << "joints: " << m_robot->numJoints() << std::endl;
std::cerr << "[" << m_profile.instance_name << "]" << "offsets: " << torque_offset.size() << std::endl;
}
}
// make filter
// filter_dim, fb_coeffs[0], ..., fb_coeffs[filter_dim], ff_coeffs[0], ..., ff_coeffs[filter_dim]
coil::vstring torque_filter_params = coil::split(prop["torque_filter_params"], ","); // filter values
int filter_dim = 0;
std::vector<double> fb_coeffs, ff_coeffs;
bool use_default_flag = false;
// check size of toruqe_filter_params
if ( torque_filter_params.size() > 0 ) {
coil::stringTo(filter_dim, torque_filter_params[0].c_str());
if (m_debugLevel > 0) {
std::cerr << "[" << m_profile.instance_name << "]" << "filter dim: " << filter_dim << std::endl;
std::cerr << "[" << m_profile.instance_name << "]" << "torque filter param size: " << torque_filter_params.size() << std::endl;
}
} else {
use_default_flag = true;
if (m_debugLevel > 0) {
std::cerr<< "[" << m_profile.instance_name << "]" << "There is no torque_filter_params. Use default values." << std::endl;
}
}
if (!use_default_flag && ((filter_dim + 1) * 2 + 1 != torque_filter_params.size()) ) {
if (m_debugLevel > 0) {
std::cerr<< "[" << m_profile.instance_name << "]" << "Size of torque_filter_params is not correct. Use default values." << std::endl;
}
use_default_flag = true;
}
// define parameters
if (use_default_flag) {
// ex) 2dim butterworth filter sampling = 200[hz] cutoff = 5[hz]
// octave$ [a, b] = butter(2, 5/200)
// fb_coeffs[0] = 1.00000; <- b0
// fb_coeffs[1] = 1.88903; <- -b1
// fb_coeffs[2] = -0.89487; <- -b2
// ff_coeffs[0] = 0.0014603; <- a0
// ff_coeffs[1] = 0.0029206; <- a1
// ff_coeffs[2] = 0.0014603; <- a2
filter_dim = 2;
fb_coeffs.resize(filter_dim+1);
fb_coeffs[0] = 1.00000;
fb_coeffs[1] = 1.88903;
fb_coeffs[2] =-0.89487;
ff_coeffs.resize(filter_dim+1);
ff_coeffs[0] = 0.0014603;
ff_coeffs[1] = 0.0029206;
ff_coeffs[2] = 0.0014603;
} else {
fb_coeffs.resize(filter_dim + 1);
ff_coeffs.resize(filter_dim + 1);
for (int i = 0; i < filter_dim + 1; i++) {
coil::stringTo(fb_coeffs[i], torque_filter_params[i + 1].c_str());
coil::stringTo(ff_coeffs[i], torque_filter_params[i + (filter_dim + 2)].c_str());
}
}
if (m_debugLevel > 0) {
for (int i = 0; i < filter_dim + 1; i++) {
std::cerr << "[" << m_profile.instance_name << "]" << "fb[" << i << "]: " << fb_coeffs[i] << std::endl;
std::cerr << "[" << m_profile.instance_name << "]" << "ff[" << i << "]: " << ff_coeffs[i] << std::endl;
}
}
// make filter instance
for(int i = 0; i < m_robot->numJoints(); i++){
m_filters.push_back(IIRFilter(filter_dim, fb_coeffs, ff_coeffs, std::string(m_profile.instance_name)));
}
return RTC::RTC_OK;
}
void ISR_AIC(void){
double output = IIRFilter(mono_read_16Bit());
mono_write_16Bit((Int16) output);
}
