int sc_main(int argc, char* argv[])
{
sc_signal <unsigned int> sinalA, sinalB, sinalC;
sc_clock clock(" Clock", 10, SC_NS,0.5, 1, SC_NS);
Estimulos est("Estimulos");
Maior m("Maior");
est.A(sinalA);
est.B(sinalB);
est.Clk(clock);
m.A(sinalA);
m.B(sinalB);
m.C(sinalC);
sc_trace_file* Tf;
Tf = sc_create_vcd_trace_file("traces");
sc_trace (Tf,sinalA,"A");
sc_trace (Tf,sinalB,"B");
sc_trace (Tf,sinalC,"C");
sc_trace (Tf,clock,"Clk");
sc_start();
sc_close_vcd_trace_file(Tf);
return 0;
}
// ---------------------------------------------------------------------------
// FundamentalBuilder::build
// ---------------------------------------------------------------------------
//
void FundamentalBuilder::build( const Peaks & peaks, double frameTime )
{
amplitudes.clear();
frequencies.clear();
for ( Peaks::const_iterator spkpos = peaks.begin(); spkpos != peaks.end(); ++spkpos )
{
if ( spkpos->amplitude() > mAmpThresh &&
spkpos->frequency() < mFreqThresh )
{
amplitudes.push_back( spkpos->amplitude() );
frequencies.push_back( spkpos->frequency() );
}
}
if ( ! amplitudes.empty() )
{
const double fmin = mFminEnv->valueAt( frameTime );
const double fmax = mFmaxEnv->valueAt( frameTime );
// estimate f0
F0Estimate est( amplitudes, frequencies, fmin, fmax, 0.1 );
if ( est.confidence() >= mMinConfidence &&
est.frequency() > fmin && est.frequency() < fmax )
{
// notifier << "f0 is " << est.frequency << endl;
// add breakpoint to fundamental envelope
mEnvelope.insert( frameTime, est.frequency() );
}
}
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "ardrone_test");
std::string filenameTime = "";
std::string filenameCoordinates = "";
for(int i = 1; i < argc; ++i)
{
if(std::string("--time") == std::string(argv[i]) && i+1 < argc)
{
++i;
filenameTime = argv[i];
}
else if(std::string("--coord") == std::string(argv[i]) && i+1 < argc)
{
++i;
filenameCoordinates = argv[i];
}
}
FlosdroneEstimator est(filenameTime, filenameCoordinates);
FlosdroneUI ui(&est);
FlosdroneController con;
ui.start();
ros::spin();
ui.stop();
return 0;
}
float TorsoStateFilter::getFulcrum(matrix<float> obs, FeetState feetState){
#define BACK_LIM -55
#define FRONT_LIM 90
#define COP_OFFSET 0
float fulcrum = 0.0;
if(frontal){ //frontal plane pendulum
//these are just estimates and empirically tuned
fulcrum = RAD2DEG(est(0, 0)) * 6.5;
if(est(0,0) < 0) fulcrum *= 1.6;
if(fulcrum > FRONT_LIM) fulcrum = FRONT_LIM;
if(fulcrum < BACK_LIM) fulcrum = BACK_LIM;
if(feetState.groundContact[0] && feetState.groundContact[1]){
fulcrum += (feetState.footPos[0][0] + feetState.footPos[1][0])/2.0;
} else {
fulcrum += (feetState.footPos[0][0]) * feetState.groundContact[0]
+ (feetState.footPos[1][0]) * feetState.groundContact[1];
}
} else {
fulcrum = -feetState.ZMP[1];
}
return fulcrum;
}
TorsoStateFilter::TorsoStateFilter(){
est = matrix<float>(STATE_DIM, 1);
estBar = matrix<float>(STATE_DIM, 1);
for(int i = 0; i < STATE_DIM; i++){
est(i, 0) = estBar(i, 0) = 0;
}
covEst = matrix<float>(STATE_DIM, STATE_DIM);
covEstBar = matrix<float>(STATE_DIM, STATE_DIM);
covR = matrix<float>(STATE_DIM, STATE_DIM);
covQ = matrix<float>(STATE_DIM, STATE_DIM);
for(int i = 0; i < STATE_DIM; i++){
for(int j = 0; j < STATE_DIM; j++){
covEst(i, j) = covEst(i, j) = covR(i, j) = covQ(i, j) = 0;
}
}
covR(0, 0) = PROCESS_ANGLE_SD * PROCESS_ANGLE_SD;
covR(1, 1) = PROCESS_VEL_SD * PROCESS_VEL_SD;
}
