41 lines
3.8 KiB
Plaintext
41 lines
3.8 KiB
Plaintext
|
within AIDAModelica;
|
||
|
|
|
||
|
|
model ComputeMotorRate "[SF2.5] Compute thrust"
|
||
|
|
// CP: 65001
|
||
|
|
// SimulationX Version: 3.8.2.45319 x64
|
||
|
|
import Modelica.Math;
|
||
|
|
Modelica.Blocks.Interfaces.RealInput TotalThrust(quantity = "Mechanics.Translation.Force", displayUnit = "N") "Desired total thrust " annotation(
|
||
|
|
Placement(transformation(origin = {-120, 68}, extent = {{-20, -20}, {20, 20}}), iconTransformation(extent = {{-120, 55}, {-80, 95}})));
|
||
|
|
Modelica.Blocks.Interfaces.RealInput MomentumConsign[3](each quantity = "Mechanics.Rotation.Torque", each displayUnit = "Nm") "Desired momentums consign" annotation(
|
||
|
|
Placement(transformation(origin = {-120, -52}, 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(origin = {110, 84}, 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(origin = {110, 44}, 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(origin = {110, -36}, 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(origin = {110, -76}, 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](each quantity = "Mechanics.Rotation.RotVelocity", each 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;
|