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.
79 lines
3.7 KiB
79 lines
3.7 KiB
within AIDAModelica; |
|
// CP: 65001 |
|
// SimulationX Version: 3.8.2.45319 x64 |
|
model ComputeMotorRate "[SF2.5] Compute thrust" |
|
import Modelica.Math; |
|
Modelica.Blocks.Interfaces.RealInput TotalThrust( |
|
quantity="Mechanics.Translation.Force", |
|
displayUnit="N") "Desired total thrust " annotation(Placement( |
|
transformation(extent={{-20,-20},{20,20}}), |
|
iconTransformation(extent={{-120,55},{-80,95}}))); |
|
Modelica.Blocks.Interfaces.RealInput MomentumConsign[3]( |
|
quantity="Mechanics.Rotation.Torque", |
|
displayUnit="Nm") "Desired momentums consign" annotation(Placement( |
|
transformation(extent={{-20,-20},{20,20}}), |
|
iconTransformation(extent={{-120,-95},{-80,-55}}))); |
|
Modelica.Blocks.Interfaces.RealOutput Motor1Rate( |
|
quantity="Mechanics.Rotation.RotVelocity", |
|
displayUnit="rpm") "Desired angular speed of 1 motor" annotation(Placement( |
|
transformation(extent={{-10,-10},{10,10}}), |
|
iconTransformation(extent={{90,65},{110,85}}))); |
|
Modelica.Blocks.Interfaces.RealOutput Motor2Rate( |
|
quantity="Mechanics.Rotation.RotVelocity", |
|
displayUnit="rpm") "Desired angular speed of 2 motor" annotation(Placement( |
|
transformation(extent={{-10,-10},{10,10}}), |
|
iconTransformation(extent={{90,15},{110,35}}))); |
|
Modelica.Blocks.Interfaces.RealOutput Motor3Rate( |
|
quantity="Mechanics.Rotation.RotVelocity", |
|
displayUnit="rpm") "Desired angular speed of 3 motor " annotation(Placement( |
|
transformation(extent={{-10,-10},{10,10}}), |
|
iconTransformation(extent={{90,-35},{110,-15}}))); |
|
Modelica.Blocks.Interfaces.RealOutput Motor4Rate( |
|
quantity="Mechanics.Rotation.RotVelocity", |
|
displayUnit="rpm") "Desired angular speed of 4 motor" annotation(Placement( |
|
transformation(extent={{-10,-10},{10,10}}), |
|
iconTransformation(extent={{90,-85},{110,-65}}))); |
|
Real M4[4,4] "Control effectiveness model matrix M"; |
|
parameter Real d( |
|
quantity="Basics.Length", |
|
displayUnit="m")=0.33 "Parameter d"; |
|
parameter Real cT( |
|
quantity="Mechanics.Translation.Force", |
|
displayUnit="N")=3.438e-7 "Parameter cT"; |
|
parameter Real coef_cT( |
|
quantity="Mechanics.Rotation.RotVelocity", |
|
displayUnit="rpm")=0.10471975511965977 "coefficient cT"; |
|
parameter Real cM( |
|
quantity="Mechanics.Rotation.Torque", |
|
displayUnit="Nm")=5.7e-9 "Parameter cM"; |
|
parameter Real coef_cM( |
|
quantity="Mechanics.Rotation.RotVelocity", |
|
displayUnit="rpm")=0.10471975511965977 "coefficient cM"; |
|
Real AngVelVector[4]( |
|
quantity="Mechanics.Rotation.RotVelocity", |
|
displayUnit="rpm") "Vector of desired angular velocities"; |
|
Real TMomVector[4] "Totat thust and moments vector"; |
|
Real ParamCosSin "Parameter Cos Sin 45"; |
|
equation |
|
// enter your equations here |
|
ParamCosSin = sqrt(2) / 2; |
|
TMomVector = {TotalThrust, MomentumConsign[1], MomentumConsign[2], MomentumConsign[3]}; |
|
M4 = {{cT / coef_cT ^ 2, cT / coef_cT ^ 2, cT / coef_cT ^ 2, cT / coef_cT ^ 2}, {d * (cT / coef_cT ^ 2) * ParamCosSin, -d * (cT / coef_cT ^ 2) * ParamCosSin, -d * (cT / coef_cT ^ 2) * ParamCosSin, d * (cT / coef_cT ^ 2) * ParamCosSin}, {d * (cT / coef_cT ^ 2) * ParamCosSin, d * (cT / coef_cT ^ 2) * ParamCosSin, -d * (cT / coef_cT ^ 2) * ParamCosSin, -d * (cT / coef_cT ^ 2) * ParamCosSin}, {cM / coef_cM ^ 2, -cM / coef_cM ^ 2, cM / coef_cM ^ 2, -cM / coef_cM ^ 2}}; |
|
M4*AngVelVector = TMomVector; |
|
Motor1Rate = sqrt(abs(AngVelVector[1])); |
|
Motor2Rate = sqrt(abs(AngVelVector[2])); |
|
Motor3Rate = sqrt(abs(AngVelVector[3])); |
|
Motor4Rate = sqrt(abs(AngVelVector[4])); |
|
annotation(Icon( |
|
coordinateSystem(extent={{-100,-125},{100,125}}), |
|
graphics={ |
|
Rectangle( |
|
fillColor={255,255,255}, |
|
fillPattern=FillPattern.Solid, |
|
extent={{-100,126.7},{103.3,-126.7}}), |
|
Text( |
|
textString="%name", |
|
fillPattern=FillPattern.None, |
|
extent={{-39,15},{39,-15}}, |
|
origin={-9,5})})); |
|
end ComputeMotorRate;
|
|
|