You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
151 lines
7.2 KiB
151 lines
7.2 KiB
// 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;
|
|
|