157 lines
3.9 KiB
C++
157 lines
3.9 KiB
C++
#include <iostream>
|
|
#include <Eigen/Dense>
|
|
#define M_PI 3.14159265358979323846 // pi
|
|
|
|
using namespace Eigen;
|
|
|
|
#define STATE_VARS 3*2 // Ńŕěč ďĺđĺěĺííűĺ č čő ďđîčâçîäíűĺ
|
|
#define PHI 5 // Ęîëč÷ĺńňâî óçëîâ â ńőĺěĺ
|
|
#define EDS 2 // Ęîëč÷ĺńňâî čńňî÷čęîâ íŕďđ˙ćĺíč˙
|
|
#define MATRIX_DIM STATE_VARS+PHI+EDS // Đŕçěĺđíîńňü ˙ęîáčŕíŕ
|
|
|
|
const double TK = 1e-3; // âđĺě˙ đŕń÷ĺňŕ
|
|
const double T_Start = 1e-10; // íŕ÷ŕëüíűé řŕă
|
|
const double SMN = 1e-15; // ěčíčěŕëüíűé řŕă
|
|
const double ACR = 1e-2; // ňî÷íîńňü
|
|
|
|
// Ďŕđŕěĺňđű ěîäĺëč
|
|
double TAU = 5e-9;
|
|
double MFt = 0.026;
|
|
|
|
double C1 = 0.1;
|
|
double Cb = 2e-12;
|
|
|
|
double L = 0.001;
|
|
|
|
double R1 = 1000;
|
|
double R2 = 10000;
|
|
double Ru = 1e+6;
|
|
double Rb = 20;
|
|
|
|
double It = 1e-12;
|
|
|
|
double IL_prev = 0;
|
|
double UC1_prev = 0;
|
|
double UCb_prev = 0;
|
|
|
|
double E2 = 5.;
|
|
// Ńčíóńîčäŕëüíűé čńňî÷íčę ňîęŕ
|
|
double E1(double t) {
|
|
const double amplitude = 10.0;
|
|
const double period = 1e-4;
|
|
const double frequency = 2 * M_PI / period;
|
|
const double phase = 0.0;
|
|
return amplitude * sin(frequency * t + phase);
|
|
}
|
|
|
|
// ßęîáčŕí
|
|
MatrixXd Y = MatrixXd::Zero(MATRIX_DIM, MATRIX_DIM);
|
|
// Âĺęňîđ íĺâ˙çîę
|
|
VectorXd N = VectorXd::Zero(MATRIX_DIM);
|
|
// Âĺęňîđ ďîďđŕâîę
|
|
VectorXd dX = VectorXd::Zero(MATRIX_DIM);
|
|
// Âĺęňîđ ôŕçîâűő ďĺđĺěĺííűő
|
|
VectorXd X = VectorXd::Zero(MATRIX_DIM);
|
|
// Ęîďč˙ âĺęňîđŕ ôŕçîâűő ďĺđĺěĺííűő (äë˙ îáđŕáîňęč ńëó÷ŕ˙ íĺóäŕ÷íîăî řŕăŕ)
|
|
VectorXd X_backup = VectorXd::Zero(MATRIX_DIM);
|
|
|
|
void Jacobian(double dt) {
|
|
Y(0, 0) = 1, Y(0, 3) = -1. / dt;
|
|
Y(1, 1) = 1, Y(1, 4) = -1. / dt;
|
|
Y(2, 2) = 1, Y(2, 5) = -1. / dt;
|
|
|
|
Y(3, 3) = 1., Y(3, 0) = -1.;
|
|
Y(4, 4) = 1., Y(4, 8) = -1., Y(4, 9) = 1.;
|
|
Y(5, 2) = L, Y(5, 7) = -1., Y(5, 10) = 1.;
|
|
|
|
Y(6, 6) = 1. / R1, Y(6, 7) - 1. / R1, Y(6, 11) = -1.;
|
|
Y(7, 5) = 1., Y(7, 6) = -1. / R1, Y(7, 7) = 1. / R1 + 1. / Rb, Y(7, 8) = -1. / Rb;
|
|
Y(8, 1) = Cb, Y(8, 7) = -1. / Rb, Y(8, 8) = 1. / Rb + 1. / Ru, Y(8, 9) = -1. / Ru;
|
|
Y(9, 1) = -Cb, Y(9, 8) = -1. / Ru, Y(9, 9) = 1. / Ru, Y(9, 11) = 1.;
|
|
Y(10, 0) = C1, Y(10, 5) = -1., Y(10, 10) = 1. / R2;
|
|
|
|
Y(11, 6) = -1.;
|
|
Y(12, 9) = -1.;
|
|
}
|
|
|
|
// 0 - 2 ďđîčçâîäíűĺ
|
|
// 3 - 5 ďĺđĺěĺííűĺ ńîńňî˙íč˙
|
|
// 6 - 10 óçëű
|
|
// 11 - 12 ňîęč íŕ ÝÄŃ
|
|
|
|
// Đŕń÷¸ň âĺęňîđŕ íĺâ˙çęč
|
|
void nevyzka(double dt, double cur_time) {
|
|
N(0) = X(0) - (X(3) - UC1_prev) / dt;
|
|
N(1) = X(1) - (X(4) - UCb_prev) / dt;
|
|
N(2) = X(2) - (X(5) - IL_prev) / dt;
|
|
|
|
N(3) = X(3) - X(10);
|
|
N(4) = X(4) - (X(8) - X(9));
|
|
N(5) = X(5) * L - (X(7) - X(10));
|
|
|
|
N(6) = -X(11) + (X(6) - X(7)) / R1;
|
|
N(7) = -(X(6) - X(7)) / R1 + X(5) + (X(7) - X(8)) / Rb;
|
|
N(8) = -(X(7) - X(8)) / Rb + It + (X(8) - X(9)) / Ru + Cb * X(1);
|
|
N(9) = -It - (X(8) - X(9)) / Ru - Cb * X(1) + X(11);
|
|
N(10) = -X(5) + C1 * X(0) + X(10) / R2;
|
|
|
|
N(11) = E1(cur_time) - X(6);
|
|
N(12) = E2 - X(9);
|
|
|
|
// Ďĺđĺä âĺęňîđîě íĺâ˙çîę ńňîčň ěčíóń
|
|
N = -1 * N;
|
|
}
|
|
|
|
int main() {
|
|
double dt = T_Start; // Řŕă ďî âđĺěĺíč
|
|
double dt_prev = dt; // Ďđĺäűäóůčé řŕă ďî âđĺěĺíč
|
|
double eps_max = ACR; // Ěŕęńčěŕëüíîĺ îňęëîíĺíčĺ
|
|
double eps_min = 1e-10; // Ěčíčěŕëüíîĺ îňęëîíĺíčĺ
|
|
int counter = 0; // Ń÷ĺň÷čę řŕăîâ ďî âđĺěĺíč
|
|
double cur_time = 0; // Ňĺęóůĺĺ âđĺě˙
|
|
double eps_cur;
|
|
double eps;
|
|
double step_c = 0;
|
|
|
|
while (dt <= TK) {
|
|
// Ń÷¸ň÷čę čňĺđŕöčé
|
|
int iteration = 0;
|
|
bool flag = true;
|
|
while (flag) {
|
|
Jacobian(dt);
|
|
nevyzka(dt, cur_time);
|
|
|
|
#ifdef _DEBUG
|
|
std::cout << Y << std::endl << N;
|
|
#endif
|
|
|
|
dX = Y.fullPivLu().solve(N);
|
|
#ifdef _DEBUG
|
|
std::cout << '\n' << dX;
|
|
#endif
|
|
|
|
X += dX;
|
|
iteration++;
|
|
double maximum = X.maxCoeff();
|
|
if (maximum <= ACR) {
|
|
flag = false;
|
|
}
|
|
else if (iteration == 7 && flag) {
|
|
iteration = 0;
|
|
dt /= 2.;
|
|
if (dt < SMN) {
|
|
std::cerr << "Sorry, but i can not solve this" << std::endl;
|
|
exit(-1);
|
|
}
|
|
X = X_backup;
|
|
}
|
|
}
|
|
// TODO: đĺŕëčçîâŕňü âű÷čńëĺíčĺ óňî÷í¸ííîĺ đĺřĺíč˙, ńđŕâíčňü ĺăî ń âű÷čńëĺííűě,
|
|
// âîçěîćíî čçěĺíčňü řŕă ěîäĺëčđîâŕíč˙
|
|
|
|
}
|
|
// Äë˙ ďđčěĺđŕ
|
|
/*Jacobian(0.1);
|
|
nevyzka(0.1, 0);
|
|
std::cout << Y << std::endl << N;*/
|
|
} |