mirror of
http://172.16.200.102/AIDA/AIDASimulation.git
synced 2025-12-14 00:13:56 +01:00
152 lines
7.2 KiB
Plaintext
152 lines
7.2 KiB
Plaintext
// CP: 65001
|
|
// SimulationX Version: 3.8.2.45319 x64
|
|
within AIDAModelica;
|
|
model AttitudeControl_PID2 "Attitude Control Model_PID2"
|
|
parameter Real k=1;
|
|
parameter Real Ti=10000;
|
|
parameter Real Td=0.0001;
|
|
Modelica.Blocks.Interfaces.RealInput AttitudeConsignAP[2](
|
|
quantity="Mechanics.Rotation.Angle",
|
|
displayUnit="rad") "Desired phi and teta angles input" annotation(Placement(
|
|
transformation(extent={{-115,5},{-75,45}}),
|
|
iconTransformation(
|
|
origin={-50,100},
|
|
extent={{-20,-20},{20,20}},
|
|
rotation=270)));
|
|
Modelica.Blocks.Interfaces.RealInput AttitudeRCConsign[2](
|
|
quantity="Mechanics.Rotation.Angle",
|
|
displayUnit="rad") "Desired phi and teta angles input sac" annotation(Placement(
|
|
transformation(extent={{-115,-55},{-75,-15}}),
|
|
iconTransformation(extent={{-120,-20},{-80,20}})));
|
|
Modelica.Blocks.Interfaces.RealInput YawConsign(
|
|
quantity="Mechanics.Rotation.Angle",
|
|
displayUnit="rad") "Desired yaw angle input" annotation(Placement(
|
|
transformation(extent={{-80,-70},{-40,-30}}),
|
|
iconTransformation(extent={{-120,-70},{-80,-30}})));
|
|
Modelica.Blocks.Interfaces.RealInput AngularSpeed[3](
|
|
quantity="Mechanics.Rotation.RotVelocity",
|
|
displayUnit="rad/s") "Real angular velocities feedback input" annotation(Placement(
|
|
transformation(extent={{-50,15},{-10,55}}),
|
|
iconTransformation(
|
|
origin={-50,-100},
|
|
extent={{-20,-20},{20,20}},
|
|
rotation=90)));
|
|
Modelica.Blocks.Interfaces.RealInput Attitude[3](
|
|
quantity="Mechanics.Rotation.Angle",
|
|
displayUnit="rad") "Real attitude feedback input" annotation(Placement(
|
|
transformation(extent={{-25,15},{15,55}}),
|
|
iconTransformation(
|
|
origin={0,-100},
|
|
extent={{-20,-20},{20,20}},
|
|
rotation=90)));
|
|
Modelica.Blocks.Interfaces.RealOutput MomentumsConsign[3](
|
|
quantity="Mechanics.Rotation.Torque",
|
|
displayUnit="Nm") "Desired moments output" annotation(Placement(
|
|
transformation(extent={{90,-15},{110,5}}),
|
|
iconTransformation(extent={{90,-10},{110,10}})));
|
|
Modelica.Blocks.Interfaces.RealInput RCYawCommand(
|
|
quantity="Mechanics.Rotation.Angle",
|
|
displayUnit="rad") "RC yaw angle input" annotation(Placement(
|
|
transformation(extent={{-80,-100},{-40,-60}}),
|
|
iconTransformation(
|
|
origin={-100,50},
|
|
extent={{-20,-20},{20,20}})));
|
|
Modelica.Blocks.Interfaces.BooleanInput SelectedControlMode "'input Boolean' as connector" annotation(Placement(
|
|
transformation(extent={{-115,-25},{-75,15}}),
|
|
iconTransformation(
|
|
origin={50,-100},
|
|
extent={{-20,-20},{20,20}},
|
|
rotation=-270)));
|
|
PID_2 controlRollAngle1(
|
|
k=k,
|
|
Ti=Ti,
|
|
Td=Td) annotation(Placement(transformation(extent={{38,13},{58,33}})));
|
|
PID_2 controlPitchAngle1(
|
|
k=k,
|
|
Ti=Ti,
|
|
Td=Td) annotation(Placement(transformation(extent={{48,-15},{68,5}})));
|
|
PID_2 controlYawAngle1(
|
|
k=k,
|
|
Ti=Ti,
|
|
Td=Td) annotation(Placement(transformation(extent={{58,-39},{78,-19}})));
|
|
Modelica.Blocks.Logical.Switch SelectRollConsign annotation(Placement(transformation(extent={{-60,0},{-40,20}})));
|
|
Modelica.Blocks.Logical.Switch SelectPitchConsign annotation(Placement(transformation(extent={{-60,-30},{-40,-10}})));
|
|
Modelica.Blocks.Math.Add add1 annotation(Placement(transformation(extent={{-35,-75},{-15,-55}})));
|
|
equation
|
|
connect(add1.y, controlYawAngle1.Consign) annotation(
|
|
Line(points = {{-14, -65}, {55, -65}, {55, -27}, {59, -27}}, color = {0, 0, 127}, thickness = 0.0625));
|
|
connect(controlYawAngle1.y, MomentumsConsign[3]) annotation(
|
|
Line(points = {{79, -29}, {95, -29}, {95, -5}, {100, -5}}, color = {0, 0, 127}, thickness = 0.0625));
|
|
connect(controlYawAngle1.du, AngularSpeed[3]) annotation(
|
|
Line(points = {{59, -33}, {-25, -33}, {-25, 35}, {-30, 35}}, color = {0, 0, 127}, thickness = 0.0625));
|
|
connect(controlYawAngle1.u, Attitude[3]) annotation(
|
|
Line(points = {{58, -23}, {0, -23}, {0, 35}, {-5, 35}}, color = {0, 0, 127}, thickness = 0.0625));
|
|
connect(SelectRollConsign.y, controlRollAngle1.Consign) annotation(
|
|
Line(points = {{-39, 10}, {15, 10}, {15, 25}, {39, 25}}, color = {0, 0, 127}, thickness = 0.0625));
|
|
connect(controlRollAngle1.y, MomentumsConsign[1]) annotation(
|
|
Line(points = {{59, 23}, {95, 23}, {95, -5}, {100, -5}}, color = {0, 0, 127}, thickness = 0.0625));
|
|
connect(controlRollAngle1.du, AngularSpeed[1]) annotation(
|
|
Line(points = {{39, 19}, {-25, 19}, {-25, 35}, {-30, 35}}, color = {0, 0, 127}, thickness = 0.0625));
|
|
connect(controlRollAngle1.u, Attitude[1]) annotation(
|
|
Line(points = {{38, 29}, {0, 29}, {0, 35}, {-5, 35}}, color = {0, 0, 127}, thickness = 0.0625));
|
|
connect(SelectPitchConsign.y, controlPitchAngle1.Consign) annotation(
|
|
Line(points = {{-39, -20}, {35, -20}, {35, -3}, {49, -3}}, color = {0, 0, 127}, thickness = 0.0625));
|
|
connect(controlPitchAngle1.y, MomentumsConsign[2]) annotation(
|
|
Line(points = {{69, -5}, {100, -5}}, color = {0, 0, 127}, thickness = 0.0625));
|
|
connect(controlPitchAngle1.du, AngularSpeed[2]) annotation(
|
|
Line(points = {{49, -9}, {-25, -9}, {-25, 35}, {-30, 35}}, color = {0, 0, 127}, thickness = 0.0625));
|
|
//controlPitchAngle1.u=0; en boucle ouverte : Ki=100 s-1, tau=0.1s
|
|
connect(controlPitchAngle1.u, Attitude[2]) annotation(
|
|
|
|
Line(points = {{48, 1}, {0, 1}, {0, 35}, {-5, 35}}, color = {0, 0, 127}, thickness = 0.0625));
|
|
connect(SelectRollConsign.u3, AttitudeRCConsign[1]) annotation(
|
|
Line(points = {{-62, 2}, {-67, 2}, {-90, 2}, {-90, -35}, {-95, -35}}, color = {0, 0, 127}, thickness = 0.0625));
|
|
connect(SelectRollConsign.u2, SelectedControlMode) annotation(
|
|
Line(points = {{-62, 10}, {-67, 10}, {-90, 10}, {-90, -5}, {-95, -5}}, color = {255, 0, 255}, thickness = 0.0625));
|
|
connect(SelectRollConsign.u1, AttitudeConsignAP[1]) annotation(
|
|
Line(points = {{-62, 18}, {-67, 18}, {-90, 18}, {-90, 25}, {-95, 25}}, color = {0, 0, 127}, thickness = 0.0625));
|
|
connect(SelectPitchConsign.u3, AttitudeRCConsign[2]) annotation(
|
|
Line(points = {{-62, -28}, {-67, -28}, {-90, -28}, {-90, -35}, {-95, -35}}, color = {0, 0, 127}, thickness = 0.0625));
|
|
connect(SelectPitchConsign.u2, SelectedControlMode) annotation(
|
|
Line(points = {{-62, -20}, {-67, -20}, {-90, -20}, {-90, -5}, {-95, -5}}, color = {255, 0, 255}, thickness = 0.0625));
|
|
connect(SelectPitchConsign.u1, AttitudeConsignAP[2]) annotation(
|
|
Line(points = {{-62, -12}, {-67, -12}, {-90, -12}, {-90, 25}, {-95, 25}}, color = {0, 0, 127}, thickness = 0.0625));
|
|
connect(add1.u1, YawConsign) annotation(
|
|
Line(points = {{-37, -59}, {-42, -59}, {-55, -59}, {-55, -50}, {-60, -50}}, color = {0, 0, 127}, thickness = 0.0625));
|
|
connect(add1.u2, RCYawCommand) annotation(
|
|
Line(points = {{-37, -71}, {-42, -71}, {-55, -71}, {-55, -80}, {-60, -80}}, color = {0, 0, 127}, thickness = 0.0625));
|
|
annotation(
|
|
AttitudeConsignAP(flags=2),
|
|
AttitudeRCConsign(flags=2),
|
|
YawConsign(flags=2),
|
|
AngularSpeed(flags=2),
|
|
Attitude(flags=2),
|
|
MomentumsConsign(flags=2),
|
|
RCYawCommand(flags=2),
|
|
SelectedControlMode(flags=2),
|
|
SelectRollConsign(
|
|
u1(flags=2),
|
|
u2(flags=2),
|
|
u3(flags=2),
|
|
y(flags=2)),
|
|
SelectPitchConsign(
|
|
u1(flags=2),
|
|
u2(flags=2),
|
|
u3(flags=2),
|
|
y(flags=2)),
|
|
add1(
|
|
u1(flags=2),
|
|
u2(flags=2),
|
|
y(flags=2)),
|
|
Icon(graphics={
|
|
Rectangle(
|
|
fillColor={255,255,255},
|
|
fillPattern=FillPattern.Solid,
|
|
extent={{-100,100},{100,-100}})}),
|
|
experiment(
|
|
StopTime=1,
|
|
StartTime=0,
|
|
Interval=0.002,
|
|
MaxInterval="0.001"));
|
|
end AttitudeControl_PID2;
|