//
tr4.setGuardExpression("(outputV > 4.9) && (outputV < 5.1) "
+ "&& (inputPz > -10.05) && (inputPz < -9.95)");
TypedIORelation rSubPx = new TypedIORelation(sub, "rSubPx");
TypedIORelation rSubDPx = new TypedIORelation(sub, "rSubDPx");
TypedIORelation rSubDDPx = new TypedIORelation(sub, "rSubDDPx");
TypedIORelation rSubD3Px = new TypedIORelation(sub, "rSubD3Px");
TypedIORelation rSubD4Px = new TypedIORelation(sub, "rSubD4Px");
TypedIORelation rSubPz = new TypedIORelation(sub, "rSubPz");
TypedIORelation rSubDPz = new TypedIORelation(sub, "rSubDPz");
TypedIORelation rSubDDPz = new TypedIORelation(sub, "rSubDDPz");
TypedIORelation rSubD3Pz = new TypedIORelation(sub, "rSubD3Pz");
TypedIORelation rSubD4Pz = new TypedIORelation(sub, "rSubD4Pz");
TypedIORelation rSubOutVx = new TypedIORelation(sub, "rSubOutVx");
TypedIORelation rSubOutVz = new TypedIORelation(sub, "rSubOutVz");
TypedIORelation rSubOutV = new TypedIORelation(sub, "rSubOutV");
TypedIORelation rSubOutR = new TypedIORelation(sub, "rSubOutR");
subinPx.link(rSubPx);
subinDPx.link(rSubDPx);
subinDDPx.link(rSubDDPx);
subinD3Px.link(rSubD3Px);
subinD4Px.link(rSubD4Px);
subinPz.link(rSubPz);
subinDPz.link(rSubDPz);
subinDDPz.link(rSubDDPz);
subinD3Pz.link(rSubD3Pz);
subinD4Pz.link(rSubD4Pz);
suboutVx.link(rSubOutVx);
suboutVz.link(rSubOutVz);
sub.connect(subinAction, hscInAct);
//hscInPz.link(rSubPz);
//hscInV.link(rSubOutV);
//hscInR.link(rSubOutR);
Iterator entities = sub.entityList().iterator();
while (entities.hasNext()) {
Entity ent = (Entity) entities.next();
Port p = ent.getPort("inputPx");
if (p != null) {
p.link(rSubPx);
}
p = ent.getPort("inputDPx");
if (p != null) {
p.link(rSubDPx);
}
p = ent.getPort("inputDDPx");
if (p != null) {
p.link(rSubDDPx);
}
p = ent.getPort("inputD3Px");
if (p != null) {
p.link(rSubD3Px);
}
p = ent.getPort("inputD4Px");
if (p != null) {
p.link(rSubD4Px);
}
p = ent.getPort("inputPz");
if (p != null) {
p.link(rSubPz);
}
p = ent.getPort("inputDPz");
if (p != null) {
p.link(rSubDPz);
}
p = ent.getPort("inputDDPz");
if (p != null) {
p.link(rSubDDPz);
}
p = ent.getPort("inputD3Pz");
if (p != null) {
p.link(rSubD3Pz);
}
p = ent.getPort("inputD4Pz");
if (p != null) {
p.link(rSubD4Pz);
}
p = ent.getPort("outputVx");
if (p != null) {
p.link(rSubOutVx);
}
p = ent.getPort("outputVz");
if (p != null) {
p.link(rSubOutVz);
}
p = ent.getPort("outputV");
if (p != null) {
p.link(rSubOutV);
}
p = ent.getPort("outputR");
if (p != null) {
p.link(rSubOutR);
}
}
// CTActors
Clock clock = new Clock(this, "Clock");
clock.period.setToken(new DoubleToken(1e308));
clock.offsets.setExpression("{0.0, 20.0}");
clock.values.setExpression("{false, true}");
connect(clock.output, subinAction);
HelicopterActor heli = new HelicopterActor(this, "Helicopter");
ControllerActor ctrl = new ControllerActor(this, "Controller");
XZHigherDerivatives higher = new XZHigherDerivatives(this,
"XZHigherDerivatives");
Integrator Px = new Integrator(this, "IntegratorPx");
Integrator DPx = new Integrator(this, "IntegratorDPx");
Integrator Pz = new Integrator(this, "IntegratorPz");
Integrator DPz = new Integrator(this, "IntegratorDPz");
Integrator Th = new Integrator(this, "IntegratorTh");
Integrator DTh = new Integrator(this, "IntegratorDTh");
Integrator Tm = new Integrator(this, "IntegratorTm");
Integrator DTm = new Integrator(this, "IntegratorDTm");
Integrator DDTm = new Integrator(this, "IntegratorDDTm");
Integrator A = new Integrator(this, "IntegratorA");
Scale MINUS = new Scale(this, "MINUS");
//CTPlot ctPlot = new CTPlot(this, "CTPlot", ctPanel);
XYPlotter xzPlot = new XYPlotter(this, "Helicopter Position");
xzPlot.plot = new Plot();
xzPlot.plot.setTitle("Helicopter Position");
xzPlot.plot.setButtons(false);
xzPlot.plot.setGrid(true);
xzPlot.plot.setXRange(-1.0, 100.0);
xzPlot.plot.setYRange(1.0, 12.0);
xzPlot.plot.setSize(200, 200);
xzPlot.plot.addLegend(0, "x, z");
TimedPlotter vxPlot = new TimedPlotter(this, "Horizontal Speed");
vxPlot.plot = new Plot();
vxPlot.plot.setTitle("Horizontal Speed");
vxPlot.plot.setButtons(false);
vxPlot.plot.setGrid(true);
vxPlot.plot.setXRange(0.0, 70.0);
vxPlot.plot.setYRange(0.0, 6.0);
vxPlot.plot.setSize(200, 200);
vxPlot.plot.addLegend(0, "Vx");
TimedPlotter pzPlot = new TimedPlotter(this, "Vertical Position");
pzPlot.plot = new Plot();
pzPlot.plot.setTitle("Vertical Position");
pzPlot.plot.setButtons(false);
pzPlot.plot.setGrid(true);
pzPlot.plot.setXRange(0.0, 70.0);
pzPlot.plot.setYRange(0.0, 12.0);
pzPlot.plot.setSize(200, 200);
pzPlot.plot.addLegend(0, "Pz");
TimedPlotter thPlot = new TimedPlotter(this, "Pitch Angle");
thPlot.plot = new Plot();
thPlot.plot.setTitle("Pitch Angle");
thPlot.plot.setButtons(false);
thPlot.plot.setGrid(true);
thPlot.plot.setXRange(0.0, 70.0);
thPlot.plot.setYRange(-0.05, 0.05);
thPlot.plot.setSize(200, 200);
thPlot.plot.addLegend(0, "Th");
// CTConnections
TypedIORelation rPx = new TypedIORelation(this, "rPx");
TypedIORelation rDPx = new TypedIORelation(this, "rDPx");
TypedIORelation rDDPx = new TypedIORelation(this, "rDDPx");
TypedIORelation rD3Px = new TypedIORelation(this, "rD3Px");
TypedIORelation rD4Px = new TypedIORelation(this, "rD4Px");
TypedIORelation rPz = new TypedIORelation(this, "rPz");
TypedIORelation rDPz = new TypedIORelation(this, "rDPz");
TypedIORelation rDDPz = new TypedIORelation(this, "rDDPz");
TypedIORelation rD3Pz = new TypedIORelation(this, "rD3Pz");
TypedIORelation rD4Pz = new TypedIORelation(this, "rD4Pz");
TypedIORelation rTh = new TypedIORelation(this, "rTh");
TypedIORelation rDTh = new TypedIORelation(this, "rDTh");
TypedIORelation rDDTh = new TypedIORelation(this, "rDDTh");
TypedIORelation rTm = new TypedIORelation(this, "rTm");
TypedIORelation rDTm = new TypedIORelation(this, "rDTm");
TypedIORelation rDDTm = new TypedIORelation(this, "rDDTm");
TypedIORelation rA = new TypedIORelation(this, "rA");
TypedIORelation rVx = new TypedIORelation(this, "rVx");
TypedIORelation rVz = new TypedIORelation(this, "rVz");
sub.getPort("outputVx").link(rVx);
sub.getPort("outputVz").link(rVz);
sub.getPort("inputPx").link(rPx);
sub.getPort("inputDPx").link(rDPx);
sub.getPort("inputDDPx").link(rDDPx);
sub.getPort("inputD3Px").link(rD3Px);
sub.getPort("inputD4Px").link(rD4Px);
sub.getPort("inputPz").link(rPz);
sub.getPort("inputDPz").link(rDPz);
sub.getPort("inputDDPz").link(rDDPz);
sub.getPort("inputD3Pz").link(rD3Pz);
sub.getPort("inputD4Pz").link(rD4Pz);
Px.output.link(rPx);
DPx.output.link(rDPx);
heli.outputDDPx.link(rDDPx);
higher.outputD3Px.link(rD3Px);
higher.outputD4Px.link(rD4Px);
Pz.output.link(rPz);
DPz.output.link(rDPz);
heli.outputDDPz.link(rDDPz);
higher.outputD3Pz.link(rD3Pz);
higher.outputD4Pz.link(rD4Pz);
Th.output.link(rTh);
DTh.output.link(rDTh);
heli.outputDDTh.link(rDDTh);
Tm.output.link(rTm);
DTm.output.link(rDTm);
DDTm.output.link(rDDTm);
A.output.link(rA);
// Connect integrators
Px.input.link(rDPx);
DPx.input.link(rDDPx);
Pz.input.link(rDPz);
DPz.input.link(rDDPz);
Th.input.link(rDTh);
DTh.input.link(rDDTh);
connect(ctrl.outputDDDTm, DDTm.input);
connect(ctrl.outputDA, A.input);
DTm.input.link(rDDTm);
Tm.input.link(rDTm);
// Connect Helicopter
heli.inputTm.link(rTm);
heli.inputA.link(rA);
heli.inputTh.link(rTh);
// Connect Controller
ctrl.inputTm.link(rTm);
ctrl.inputDTm.link(rDTm);
ctrl.inputDDTm.link(rDDTm);
ctrl.inputA.link(rA);
ctrl.inputTh.link(rTh);
ctrl.inputDTh.link(rTh);
ctrl.inputVx.link(rVx);
ctrl.inputVz.link(rVz);
// Connect XZHigherDerivatives
higher.inputTm.link(rTm);
higher.inputDTm.link(rDTm);
higher.inputDDTm.link(rDDTm);
higher.inputTh.link(rTh);
higher.inputDTh.link(rDTh);
higher.inputA.link(rA);
// Connect HoverLinearizer
TypedIORelation rmPz = new TypedIORelation(this, "RMPz");
MINUS.input.link(rPz);
MINUS.output.link(rmPz);
xzPlot.inputX.link(rPx);
xzPlot.inputY.link(rmPz);