Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- #region pre-script
- #if DEBUG
- using System;
- using System.Collections.Generic;
- using VRageMath;
- using VRage.Game;
- using VRage.Library;
- using System.Text;
- using Sandbox.ModAPI.Interfaces;
- using Sandbox.ModAPI.Ingame;
- using Sandbox.Common;
- using Sandbox.Game;
- using VRage.Collections;
- using VRage.Game.ModAPI.Ingame;
- using SpaceEngineers.Game.ModAPI.Ingame;
- namespace SE_Scripting
- {
- public class Program : MyGridProgram
- {
- #endif
- #endregion
- //=======================================================================================
- //////////////////////////START//////////////////////////////////////////
- //=======================================================================================
- string shipControllerNameTag = "Reference";
- double desiredSpeed = 5; // meters per second
- //PID Controller constants
- double proportionalGain = 5;
- double integralGain = 0;
- double derivativeGain = 2;
- PID frontBackPID;
- PID leftRightPID;
- PID upDownPID;
- Vector3D desiredDirection = new Vector3D(0, 0, 0);
- Program()
- {
- frontBackPID = new PID(proportionalGain, integralGain, derivativeGain, -10, 10, 1d/60d);
- leftRightPID = new PID(proportionalGain, integralGain, derivativeGain, -10, 10, 1d / 60d);
- upDownPID = new PID(proportionalGain, integralGain, derivativeGain, -10, 10, 1d / 60d);
- }
- void Main(string arg)
- {
- bool isSetup = GrabBlocks();
- if (!isSetup)
- return;
- Echo("It's workingggg!!!");
- switch (arg.ToLower())
- {
- case "front":
- desiredDirection = controller.WorldMatrix.Forward;
- break;
- case "back":
- desiredDirection = controller.WorldMatrix.Backward;
- break;
- case "left":
- desiredDirection = controller.WorldMatrix.Left;
- break;
- case "right":
- desiredDirection = controller.WorldMatrix.Right;
- break;
- case "up":
- desiredDirection = controller.WorldMatrix.Up;
- break;
- case "down":
- desiredDirection = controller.WorldMatrix.Down;
- break;
- case "stop":
- desiredDirection = Vector3D.Zero;
- break;
- default:
- ParseArgument(arg);
- break;
- }
- MoveInDirection(desiredDirection, desiredSpeed, controller);
- }
- void ParseArgument(string arg)
- {
- //do stuff
- }
- void MoveInDirection(Vector3D desiredDirection, double desiredSpeed, IMyShipController controller)
- {
- var currentVelocity = controller.GetShipVelocities().LinearVelocity;
- var desiredVelocity = desiredDirection * desiredSpeed;
- var relativeVelocity = desiredVelocity - currentVelocity; Echo($"RelVel: {relativeVelocity.Length()}");
- var forwardError = ScalarProjection(relativeVelocity, controller.WorldMatrix.Forward);
- var leftError = ScalarProjection(relativeVelocity, controller.WorldMatrix.Left);
- var upError = ScalarProjection(relativeVelocity, controller.WorldMatrix.Up);
- var forwardOutput = frontBackPID.Control(forwardError);
- var leftOutput = leftRightPID.Control(leftError);
- var upOutput = upDownPID.Control(upError);
- Echo($"Forward: {forwardOutput}\nLeft: {leftOutput}\nUp: {upOutput}");
- ThrustToTargetSpeed(forwardThrust, backwardThrust, forwardOutput);
- ThrustToTargetSpeed(leftThrust, rightThrust, leftOutput);
- ThrustToTargetSpeed(upThrust, downThrust, upOutput);
- }
- void ThrustToTargetSpeed(List<IMyThrust> positiveThrust, List<IMyThrust> negativeThrust, double desiredThrust)
- {
- if (desiredThrust < 0)
- ApplyThrust(negativeThrust, -desiredThrust);
- else
- ApplyThrust(positiveThrust, desiredThrust);
- }
- void ApplyThrust(List<IMyThrust> thrusters, double thrust)
- {
- foreach (var block in thrusters)
- {
- block.SetValue("Override", (float)thrust);
- Echo($"{block.CustomName}\nThrust: {thrust}");
- }
- }
- Vector3D VectorProjection(Vector3D a, Vector3D b)
- {
- if (Vector3D.IsZero(b))
- return Vector3D.Zero;
- return a.Dot(b) / b.LengthSquared() * b;
- }
- double ScalarProjection(Vector3D a, Vector3D b)
- {
- if (Vector3D.IsZero(b))
- return 0;
- return a.Dot(b) / b.Length() * Math.Sign(a.Dot(b));
- }
- List<IMyShipController> shipControllers = new List<IMyShipController>();
- List<IMyThrust> allThrust = new List<IMyThrust>();
- List<IMyThrust> upThrust = new List<IMyThrust>();
- List<IMyThrust> downThrust = new List<IMyThrust>();
- List<IMyThrust> leftThrust = new List<IMyThrust>();
- List<IMyThrust> rightThrust = new List<IMyThrust>();
- List<IMyThrust> forwardThrust = new List<IMyThrust>();
- List<IMyThrust> backwardThrust = new List<IMyThrust>();
- IMyShipController controller;
- bool GrabBlocks()
- {
- upThrust.Clear();
- downThrust.Clear();
- leftThrust.Clear();
- rightThrust.Clear();
- forwardThrust.Clear();
- backwardThrust.Clear();
- GridTerminalSystem.GetBlocksOfType(shipControllers, x => x.CustomName.Contains(shipControllerNameTag));
- GridTerminalSystem.GetBlocksOfType(allThrust);
- if (shipControllers.Count == 0)
- {
- Echo($"Error: No ship controllers named \"{shipControllerNameTag}\" found");
- return false;
- }
- controller = shipControllers[0];
- foreach (var block in allThrust)
- {
- if (controller.WorldMatrix.Forward == block.WorldMatrix.Backward)
- forwardThrust.Add(block);
- else if (controller.WorldMatrix.Backward == block.WorldMatrix.Backward)
- backwardThrust.Add(block);
- else if (controller.WorldMatrix.Left == block.WorldMatrix.Backward)
- leftThrust.Add(block);
- else if (controller.WorldMatrix.Right == block.WorldMatrix.Backward)
- rightThrust.Add(block);
- else if (controller.WorldMatrix.Up == block.WorldMatrix.Backward)
- upThrust.Add(block);
- else if (controller.WorldMatrix.Down == block.WorldMatrix.Backward)
- downThrust.Add(block);
- }
- return true;
- }
- //Whip's PID controller class v2 - 7/29/17
- public class PID
- {
- double _kP = 0;
- double _kI = 0;
- double _kD = 0;
- double _lowerBound = 0;
- double _upperBound = 0;
- double _timeStep = 0;
- double _errorSum = 0;
- double _lastError = 0;
- bool _firstRun = true;
- public PID(double kP, double kI, double kD, double lowerBound, double upperBound, double timeStep)
- {
- _kP = kP;
- _kI = kI;
- _kD = kD;
- _lowerBound = lowerBound;
- _upperBound = upperBound;
- _timeStep = timeStep;
- }
- public PID(float kP, float kI, float kD, float lowerBound, float upperBound, float timeStep)
- {
- _kP = kP;
- _kI = kI;
- _kD = kD;
- _lowerBound = lowerBound;
- _upperBound = upperBound;
- _timeStep = timeStep;
- }
- public double Control(double error)
- {
- //Compute derivative term
- var errorDerivative = (error - _lastError) / _timeStep;
- if (_firstRun)
- {
- errorDerivative = 0;
- _firstRun = false;
- }
- //Compute integral term
- _errorSum += error * _timeStep;
- //Clamp integral term
- if (_errorSum > _upperBound)
- _errorSum = _upperBound;
- else if (_errorSum < _lowerBound)
- _errorSum = _lowerBound;
- //Store this error as last error
- _lastError = error;
- //Construct output
- return _kP * error + _kI * _errorSum + _kD * errorDerivative;
- }
- public void Reset()
- {
- _errorSum = 0;
- _lastError = 0;
- _firstRun = true;
- }
- }
- //=======================================================================================
- //////////////////////////END//////////////////////////////////////////
- //=======================================================================================
- #region post-script
- #if DEBUG
- }
- }
- #endif
- #endregion
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement