Advertisement
Whiplash141

Whip's Rotor Angle Controller Code

Dec 26th, 2017
321
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C# 7.50 KB | None | 0 0
  1.  
  2. /*
  3. // Whip's Rotor Controller v9 /// revision: 12/4/17
  4. ______________________________________________________________
  5. SETUP:
  6. 1) Place this code in a programmable block
  7. 2) That is all
  8.  
  9. ______________________________________________________________
  10. INSTRUCTIONS:
  11. * Run the code with the following argument syntax:
  12.  
  13. <rotor name>;<command>
  14.  
  15. Where:
  16.     <rotor name> is the name of the rotor. Names are case insensitive
  17.    
  18.     <command> is one of the commands listed below
  19.  
  20. ______________________________________________________________
  21. COMMANDS:
  22.  
  23. set <angle>
  24.     Sets the rotor <angle> to the specified angle
  25.  
  26. increment <angle>
  27.     increments current rotor angle by the specified <angle>
  28.  
  29. ______________________________________________________________
  30. Examples:
  31.  
  32. Rotor 1 ; set 45
  33.     Sets EVERY rotor with name "Rotor 1" to 45°
  34.    
  35. Potato ; increment 69
  36.     Increments EVERY rotor with "Potato" in its name by 69°
  37.    
  38. */
  39.  
  40. List<RotorController> activeRotorList = new List<RotorController>();
  41.  
  42. Program()
  43. {
  44.     Runtime.UpdateFrequency = UpdateFrequency.None;
  45.     Echo("Whip's Rotor Controller");
  46.     Echo(">>Inactive<<");
  47. }
  48.  
  49. void Main(string arg, UpdateType updateType)
  50. {
  51.     if (arg != "" && (updateType & (UpdateType.Trigger | UpdateType.Terminal | UpdateType.Script)) != 0) //checks if update source is from user
  52.     {
  53.         ParseArguments(arg);
  54.     }
  55.    
  56.     Echo("Whip's Rotor Controller");
  57.     if (activeRotorList.Count > 0)
  58.     {
  59.         Echo(">>Active<<");
  60.         Runtime.UpdateFrequency = UpdateFrequency.Update10;
  61.     }
  62.     else
  63.     {
  64.         Echo(">>Inactive<<");
  65.         Runtime.UpdateFrequency = UpdateFrequency.None;
  66.     }
  67.    
  68.     if ((updateType & UpdateType.Update10) == 0) //ignore the brunt of the logic if the
  69.         return;
  70.    
  71.     foreach (var rotor in activeRotorList)
  72.     {
  73.         rotor.MoveRotor();
  74.     }
  75.  
  76.     activeRotorList.RemoveAll(x => x.DoneMoving); //remove finished rotors
  77. }
  78.  
  79. void ParseArguments(string argument)
  80. {
  81.     string command = "";
  82.     bool setRotorAngle = false;
  83.  
  84.     var argumentSplit = argument.ToLower().Split(';');
  85.  
  86.     if (argumentSplit.Length != 2)
  87.     {
  88.         Echo($">> Error: '{argument}' not understood.\nArguments must be in the form of:\n<rotor name>;<command>");
  89.         return;
  90.     }
  91.  
  92.     var rotorName = argumentSplit[0].Trim();
  93.     var rotors = new List<IMyMotorStator>();
  94.     GridTerminalSystem.GetBlocksOfType(rotors, block => block.CustomName.ToLower().Contains(rotorName.ToLower()));
  95.    
  96.     if (rotors.Count == 0)
  97.     {
  98.         Echo($">> Error: No rotors named '{rotorName}' were found");
  99.         return;
  100.     }
  101.    
  102.     var command = argumentSplit[1].Trim().ToLower();
  103.  
  104.     if (command.Contains("set "))
  105.     {
  106.         command = argumentSplit[1].ToLower().Replace("set ", "");
  107.  
  108.         if (command.Contains("+"))
  109.             command = command.Replace("+", "");
  110.  
  111.         setRotorAngle = true;
  112.     }
  113.     else if (command.Contains("increment "))
  114.     {
  115.         command = argumentSplit[1].Replace("increment ", "");
  116.         if (command.Contains("+"))
  117.             command = command.Replace("+", "");
  118.     }
  119.     else
  120.     {
  121.         Echo($">> Error: '{argument}' not understood.\nArguments must be in the form of:\n<rotor name>;<command>");
  122.         return;
  123.     }
  124.  
  125.     float thisValue;
  126.     bool isNumeric = Single.TryParse(command, out thisValue);
  127.     if (isNumeric)
  128.     {
  129.         if (setRotorAngle) //set rotor angle
  130.         {
  131.             Echo("Argument parsed as: " + thisValue.ToString());
  132.             Echo("Setting rotor angle...");
  133.  
  134.             foreach (var block in rotors)
  135.             {
  136.                 var existingRotor = activeRotorList.Find(x => x.Rotor == block);
  137.                 if (existingRotor != default(RotorController))
  138.                 {
  139.                     existingRotor.SetAngle(thisValue);
  140.                 }
  141.                 else
  142.                 {
  143.                     var activeRotor = new RotorController(block, thisValue, false);
  144.                     activeRotorList.Add(activeRotor);
  145.                 }
  146.  
  147.             }
  148.         }
  149.         else //increment rotor angle
  150.         {
  151.             Echo("Argument parsed as: " + thisValue.ToString());
  152.             Echo("Incrementing rotors...");
  153.  
  154.             foreach (var block in rotors)
  155.             {
  156.                 var existingRotor = activeRotorList.Find(x => x.Rotor == block);
  157.                 if (existingRotor != default(RotorController))
  158.                 {
  159.                     existingRotor.IncrementAngle(thisValue);
  160.                 }
  161.                 else
  162.                 {
  163.                     var activeRotor = new RotorController(block, thisValue, true);
  164.                     activeRotorList.Add(activeRotor);
  165.                 }
  166.             }
  167.         }
  168.     }
  169.     else
  170.     {
  171.         Echo("Error: Argument '" + argument + "' could not be parsed");
  172.     }
  173. }
  174.  
  175. class RotorController
  176. {
  177.     public IMyMotorStator Rotor { get; private set; } = null;
  178.     float _initialAngle;
  179.     float _targetAngle;
  180.     float _epsilon;
  181.     float _rotationSpeedGain;
  182.     public bool DoneMoving { get; private set; } = false;
  183.  
  184.     public RotorController(IMyMotorStator rotor, float angle, bool increment = false, float rotationSpeedGain = 30f, float epsilonDeg = 0.1f)
  185.     {
  186.         Rotor = rotor;
  187.         _initialAngle = Rotor.Angle;
  188.         if (increment)
  189.             _targetAngle = _initialAngle + MathHelper.ToRadians(angle);
  190.         else
  191.             _targetAngle = MathHelper.ToRadians(angle);
  192.  
  193.         _epsilon = MathHelper.ToRadians(epsilonDeg);
  194.         _rotationSpeedGain = rotationSpeedGain;
  195.     }
  196.  
  197.     public void IncrementAngle(float angle)
  198.     {
  199.         _targetAngle += MathHelper.ToRadians(angle);
  200.     }
  201.  
  202.     public void SetAngle(float angle)
  203.     {
  204.         _targetAngle = MathHelper.ToRadians(angle);
  205.     }
  206.  
  207.     public void MoveRotor()
  208.     {
  209.         MathHelper.LimitRadians(ref _targetAngle);
  210.  
  211.         Rotor.SetValue("LowerLimit", -362f);
  212.         Rotor.SetValue("UpperLimit", 362f);
  213.         Rotor.SetValue("RotorLock", false);
  214.  
  215.         var rotorVector = Rotor.Top.WorldMatrix.Backward;
  216.         var rotorRightVector = Rotor.Top.WorldMatrix.Left; //bc keen...
  217.         var targetVector = GetVectorFromRotorAngle(_targetAngle, Rotor);
  218.  
  219.         var err = VectorAngleBetween(rotorVector, targetVector) * Math.Sign(targetVector.Dot(rotorRightVector));
  220.  
  221.         Rotor.TargetVelocityRPM = (float)err * _rotationSpeedGain;
  222.  
  223.         if (Math.Abs(err) < _epsilon)
  224.         {
  225.             Rotor.TargetVelocityRPM = 0;
  226.             Rotor.SetValue("LowerLimit", MathHelper.ToDegrees(_targetAngle));
  227.             Rotor.SetValue("UpperLimit", MathHelper.ToDegrees(_targetAngle));
  228.             Rotor.SetValue("RotorLock", true);
  229.             this.DoneMoving = true;
  230.         }
  231.     }
  232.  
  233.     Vector3D GetVectorFromRotorAngle(float angle, IMyMotorStator rotor)
  234.     {
  235.         double x = Math.Sin(angle);
  236.         double y = Math.Cos(angle);
  237.         var rotorMatrix = rotor.WorldMatrix;
  238.         return rotorMatrix.Backward * y + rotor.WorldMatrix.Left * x;
  239.     }
  240.  
  241.     private double VectorAngleBetween(Vector3D a, Vector3D b, bool useSmallestAngle = false) //returns radians
  242.     {
  243.         if (Vector3D.IsZero(a) || Vector3D.IsZero(b))
  244.             return 0;
  245.         else if (useSmallestAngle)
  246.             return Math.Acos(MathHelper.Clamp(Math.Abs(a.Dot(b)) / Math.Sqrt(a.LengthSquared() * b.LengthSquared()), -1, 1));
  247.         else
  248.             return Math.Acos(MathHelper.Clamp(a.Dot(b) / Math.Sqrt(a.LengthSquared() * b.LengthSquared()), -1, 1));
  249.     }
  250. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement