1

git-svn-id: svn://db.shs.com.ru/libs@1 a8b55f48-bf90-11e4-a774-851b48703e85

mbricks/brick_link.cpp

@@ -0,0 +1,158 @@
+#include "brick_link.h"
+
+
+BrickLinkTransferFunction::BrickLinkTransferFunction(const PIString & num, const PIString & denom): BrickLinkBase(1, 1, 2) {
+	type = "TransferFunction";
+	setName(type);
+	paramNames[1] = "Numerator";
+	paramNames[2] = "Denominator";
+	parameters[1] = num;
+	parameters[2] = denom;
+	interactive = true;
+	parameterChanged(1);
+}
+
+
+void BrickLinkTransferFunction::parameterChanged(int index) {
+	if (index == 0) {
+		KF.setMethod((PIMathSolver::Method)parameters[0].toInt());
+		return;
+	}
+	PIString ts;
+	note_ = "Your function:\n\n";
+	PIStringList sl = parameters[1].toString().split(" ");
+	sl.removeStrings(""); sl.removeStrings("\t");
+	TF.vector_Bm.resize(sl.size());
+	for (int i = sl.size_s() - 1; i >= 0; --i) {
+		TF.vector_Bm[i] = sl[sl.size_s() - 1 - i].toDouble();
+		ts += PIString::fromNumber(TF.vector_Bm[i]);
+		if (i > 1) {
+			ts += "s^{" + PIString::fromNumber(i) + "} + ";
+			continue;
+		}
+		if (i > 0) ts += "s" + PIString(" + ");
+	}
+	if (ts.left(2) == "1s") ts.pop_front();
+	if (ts.left(3) == "-1s") ts.replace(0, 2, "-");
+	note_ += ts + "\n---\n";
+	sl = parameters[2].toString().split(" ");
+	sl.removeStrings(""); sl.removeStrings("\t");
+	TF.vector_An.resize(sl.size());
+	ts.clear();
+	for (int i = sl.size_s() - 1; i >= 0; --i) {
+		TF.vector_An[i] = sl[sl.size_s() - 1 - i].toDouble();
+		ts += PIString::fromNumber(TF.vector_An[i]);
+		if (i > 1) {
+			ts += "s^{" + PIString::fromNumber(i) + "} + ";
+			continue;
+		}
+		if (i > 0) ts += "s" + PIString(" + ");
+	}
+	if (ts.left(2) == "1s") ts.pop_front();
+	if (ts.left(3) == "-1s") ts.replace(0, 2, "-");
+	note_ += ts;
+	note_.replaceAll("+ -", "- ");
+	note_.replaceAll(" + 1s", " + s");
+	note_.replaceAll(" - 1s", " - s");
+}
+
+
+bool BrickLinkTransferFunction::tick_body(double time) {
+	KF.setTime(time);
+	KF.solve(inputs[0], dt);
+	outputs[0] = KF.X[0];
+	return true;
+}
+
+
+BrickLinkFilter1Degree::BrickLinkFilter1Degree(double t, double k): BrickLinkBase(3, 2) {
+	setParameters(t, k);
+	type = "Filter1Degree";
+	setName(type);
+	inNames[1] = "T";
+	inNames[2] = "K";
+	outNames[0] = "Low Pass";
+	outNames[1] = "High Pass";
+	s.setPreamp(1, -1.);
+	makeConnections();
+}
+
+
+bool BrickLinkFilter1Degree::tick_body(double time) {
+	if (inputs[T] == 0.) return false;
+	a.setGain(1. / inputs[T]);
+	s.setPreamp(0, inputs[K]);
+	s.setInputValue(0, inputs[Input]);
+	s.tick(time);
+	s.proceedConnections();
+	a.tick(time);
+	a.proceedConnections();
+	i.tick(time);
+	i.proceedConnections();
+	outputs[LowPass] = i.output();
+	outputs[HighPass] = s.output();
+	return true;
+}
+
+
+BrickLinkFilterBandpass::BrickLinkFilterBandpass(double lf, double hf): BrickLinkBase(3, 1) {
+	TF.vector_Bm.resize(2);
+	TF.vector_An.resize(3, 1.);
+	type = "FilterBandpass";
+	setName(type);
+	inNames[1] = "Low Freq";
+	inNames[2] = "High Freq";
+	setParameters(lf, hf);
+	pl = ph = 0.;
+}
+
+
+bool BrickLinkFilterBandpass::tick_body(double time) {
+	if (pl != inputs[LF] || ph != inputs[HF]) {
+		pl = inputs[LF];
+		ph = inputs[HF];
+		if (pl != 0.) t0 = 1. / pl;
+		else t0 = 0.;
+		if (ph != 0.) t1 = 1. / ph;
+		else t1 = 0.;
+		TF.vector_Bm[0] = 0.;
+		TF.vector_Bm[1] = t0;
+		TF.vector_An[0] = 1.;
+		TF.vector_An[1] = t0 + t1;
+		TF.vector_An[2] = t0 * t1;
+		KF.fromTF(TF);
+	}
+	KF.solve(inputs[0], dt);
+	outputs[0] = KF.X[0];
+	return true;
+}
+
+
+
+BrickLinkFilterMedian::BrickLinkFilterMedian(int count): BrickLinkBase(1, 1, 0) {
+	type = "FilterMedian";
+	setName(type);
+	paramNames[0] = "Count";
+	parameters[0].setValue(count);
+	note_ = "Size of window";
+	setParameters(count);
+}
+
+
+bool BrickLinkFilterMedian::tick_body(double time) {
+	for (int i = 0; i < hist_.size_s() - 1; ++i)
+		hist_[i] = hist_[i + 1];
+	hist_.back() = inputs[0];
+	hsort = hist_;
+	hsort.sort();
+	int size = hsort.size_s();
+	if (size == 0) {
+		outputs[0] = 0.;
+	} else {
+		if (size % 2 == 1)
+			outputs[0] = hsort[size / 2];
+		else
+			outputs[0] = (hsort[size / 2 - 1] + hsort[size / 2]) / 2.;
+	}
+	return true;
+}