QVector<double> KolmogorovZurbenko::kza1d(int window, int iterations, int minimumWindowLength, double tolerance)
{
int n,q;
long qh, qt;
double m;
n = y.size();
QVector<double> *d = new QVector<double>(n);
QVector<double> *prime = new QVector<double>(n);
QVector<double> *ans = new QVector<double>(n);
QVector<double> *tmp = new QVector<double>(y);
q = window;
differenced(d, prime, q);
m = *std::max_element(std::begin(*d), std::end(*d));
for(int i = 0; i < iterations; i++){
#pragma omp parallel for
for(int t = 0; t < n; t++){
if(abs(prime->at(t)) < tolerance){
qh = (int) floor(q*adaptive(d->at(t), m));
qt = (int) floor(q*adaptive(d->at(t), m));
} else if (abs(prime->at(t)) < 0){
qh = q;
qt = (int) floor(q*adaptive(d->at(t), m));
} else {
qh = (int) floor(q*adaptive(d->at(t), m));
qt = q;
}
qt = (qt < minimumWindowLength) ? minimumWindowLength : qt;
qh = (qh < minimumWindowLength) ? minimumWindowLength : qh;
qh = (qh > n-t-1) ? n-t-1 : qh;
qt = (qt > t) ? t : qt;
ans->operator [](t) = mavga1d(*tmp, t-qt, t+qh+1);
}
ans->swap(*tmp);
}
return *ans;
}
void glob_opt()
{
parameter *paras;
cryStruct *oriCS;
int i;
paras = (parameter*)malloc(sizeof(parameter));
r_set(paras);
init_rand(paras);
oriCS = new_CS(paras);
init_oriCS(oriCS, paras);
if(paras->adaptive == 1)
adaptive(paras, oriCS);
else if((paras->algoGlob >= 1) && (paras->algoGlob <= 19))
{
if(paras->run == 1)
pso_opt_serial(paras, oriCS);
else if(paras->run == 11)
pso_opt(paras, oriCS);
}
del_CS(oriCS, paras);
for(i=0; i < paras->nType; i++)
free(paras->type[i]);
for(i=0; i < paras->nRmFile; i++)
free(paras->rmFile[i]);
free(paras->type);
free(paras->charge);
free(paras->rmFile);
free(paras);
return;
}
int main(void)
{
int temp = 0; // Read temperature
//int sp_temp_get;
float temp_real = 0; // Real format temperature
//int t_sample = 100; // Time period samples read ms
//int t_control = 1000; // Time period control system ms
uint16_t t_sample = 0; // Time period samples read ms
int t_control = 0; // Time period control system ms
//long millis_ant1; // Aux timer
//long millis_ant2; // Aux timer
uint16_t out_pwm; // Out PWM control
double y_temp[PmA+2] = {0}; // Array out incremental y(k), y(k-1), y(k-2), y(k-3)
double u_temp[PmB+3] = {0}; // Array process input incremental u(k), u(k-1), u(k-2), u(k-3), u(k-4)
double t_temp[5] = {1.0, -0.3, 0.1, 0.1, 0.1}; // Array parameters adaptive mechanism a1k, a2k, b1k, b2k, b3k
double sp_temp[2] = {0}; // Set point process sp(k)
double yp_temp[2] = {0}; // Array process out yp(k)
cli(); // Disable interrupts
conductor_block(); // Calculate parameters conductor block
usart_init(); // Initialize USART
UCSR0B |= (1 << RXCIE0); // Enable interrupt RX (If enabled do not use functions get_xx)
adc_Setup(); // Initialize ADC
timer1_init(); // Timer system without interrupts
//timer0_init(); // Initialize timer0 system ms
timer2_init(); // Initialize timer2 PWM
sei();
DDRD |= (1 << PIND3); // Out PWM OC2B PIND3 Digital PIN 3
temp = adc_read(0); // Initialize temperature readers
//millis_ant1 = millis; // Initialize period samples
//millis_ant2 = millis; // Initialize period control
//eeprom_update_float(&eeprom_float,f);
//float eeprom_float_read;
//eeprom_float_read = eeprom_read_float(&eeprom_float);
//put_float(eeprom_float_read);
//put_string("\n Input SP temperature: ");
//sp_temp_get = get_int();
while(1)
{
// Samples read
//if ((millis - millis_ant1) >= t_sample){
//millis_ant1 = millis;
t_sample = TCNT1; // Catch sample time for integer angle gyro
T_CNT = T_SAMPLE * 1000L/64; // Number count temp1. 1 Count Temp1 64us => T_sample = Value_CNT1 = ms/64us
if (t_sample >= T_CNT){ // Attitude calculates Read IMU (Accel and Gyro)
TCNT1 = 0; // Restart sample time
t_control++; // Increment time Period control
temp = lpf(adc_read(0), temp, 0.1);
}
//if ((millis - millis_ant2) >= t_control){
//millis_ant2 = millis;
if (t_control >= T_CONTROL){ //Control action balancer
t_control = 0;
temp_real = temp * 110.0 / 1023.0; // solo en el control
//Adaptive predictive balancer process
sp_temp[0] = sp_temp_get; // Set point
yp_temp[0] = temp_real; // Process out y(k).
adaptive(sp_temp, t_temp, y_temp, u_temp, yp_temp, UP_PWM); // Call adaptive function
out_pwm = u_temp[0]; // Out Controller adaptive
if (out_pwm > UP_PWM) out_pwm = UP_PWM; // Upper limit out
else if (out_pwm < 0) out_pwm = 0;
//out_pwm = 0; // Off control
OCR2B = 255 - out_pwm; // Out PWM
put_float(temp_real);
put_string(" ");
put_int(OCR2B);
put_string(" ");
put_int(sp_temp_get);
//put_string(" ");
//put_float(NL);
//put_string(" ");
//put_float(GainA);
//put_string(" ");
//put_float(GainB);
//put_string(" ");
//put_int(T_SAMPLE);
//put_string(" ");
//put_int(T_CONTROL);
put_string("\n");
}
//TODO:: Please write your application code
}
}
