bool DoRotate(double rollAngle, string sPlane = "Roll", float maxYPR = -1)
{
// Echo("DR:angle=" + rollAngle.ToString("0.00"));
float targetRoll = 0;
IMyGyro gyro = gyros[0] as IMyGyro;
float maxRoll = gyro.GetMaximum <float>(sPlane);
if (maxYPR > 0)
{
maxRoll = maxYPR;
}
// float minRoll = gyro.GetMinimum<float>(sPlane);
if (Math.Abs(rollAngle) > 1.0)
{
// Echo("MAx gyro");
targetRoll = (float)maxRoll * (float)(rollAngle);
}
else if (Math.Abs(rollAngle) > .7)
{
// need to dampen
// Echo(".7 gyro");
targetRoll = (float)maxRoll * (float)(rollAngle) / 4;
}
else if (Math.Abs(rollAngle) > 0.5)
{
// Echo(".5 gyro");
targetRoll = 0.11f * Math.Sign(rollAngle);
}
else if (Math.Abs(rollAngle) > 0.1)
{
// Echo(".1 gyro");
targetRoll = 0.11f * Math.Sign(rollAngle);
}
else if (Math.Abs(rollAngle) > 0.01)
{
// Echo(".01 gyro");
targetRoll = 0.11f * Math.Sign(rollAngle);
}
else if (Math.Abs(rollAngle) > 0.001)
{
// Echo(".001 gyro");
targetRoll = 0.09f * Math.Sign(rollAngle);
}
else
{
targetRoll = 0;
}
GyroControl.SetYaw(targetRoll);
GyroControl.SetOverride(true);
GyroControl.RequestEnable(true);
return(true);
}
public Gyroscopes(IMyTerminalBlock referenceBlock, List <IMyTerminalBlock> blocks)
: base(referenceBlock)
{
UpdateGyroscopes(blocks);
IMyGyro gyro = gyroscopeBlocks[0];
Min = gyro.GetMininum <float>(GyroAction.Pitch.Name);
Max = gyro.GetMaximum <float>(GyroAction.Pitch.Name);
Default = gyro.GetDefaultValue <float>(GyroAction.Pitch.Name);
}
} //GetChangeInPose()
protected void TurnGyros(List <IMyTerminalBlock> gyroList, Vector3D axis, double angle, float coEff = 0.5f)
{
float ctrlVel = 0.1f; // control velocity
IMyGyro gyro = null; // for looping through gyros
int i = 0; // counter for looping
gyro = gyroList[0] as IMyGyro;
ctrlVel = gyro.GetMaximum <float>("Yaw") * (float)(angle / Math.PI) * coEff;
ctrlVel = Math.Min(gyro.GetMaximum <float>("Yaw"), ctrlVel);
ctrlVel = (float)Math.Max(0.005, ctrlVel);
axis.Normalize();
axis *= ctrlVel;
for (i = 0; i < gyroList.Count; i++)
{
gyro = gyroList[i] as IMyGyro;
gyro.SetValue("Pitch", Convert.ToSingle(axis.GetDim(0)));
gyro.SetValue("Yaw", -Convert.ToSingle(axis.GetDim(1)));
gyro.SetValue("Roll", -Convert.ToSingle(axis.GetDim(2)));
} // for i loop
} //TurnGyros method
bool DoRoll(double rollAngle, string sPlane = "Roll")
{
//Echo("rollAngle=" + Math.Round(rollAngle,5));
float targetRoll = 0;
IMyGyro gyro = gyros[0] as IMyGyro;
float maxRoll = gyro.GetMaximum <float>(sPlane);
float minRoll = gyro.GetMinimum <float>(sPlane);
if (Math.Abs(rollAngle) > 1.0)
{
targetRoll = (float)maxRoll * (float)(rollAngle);
}
else if (Math.Abs(rollAngle) > .7)
{
// need to dampen
targetRoll = (float)maxRoll * (float)(rollAngle) / 4;
}
else if (Math.Abs(rollAngle) > 0.5)
{
targetRoll = 0.11f * Math.Sign(rollAngle);
}
else if (Math.Abs(rollAngle) > 0.1)
{
targetRoll = 0.07f * Math.Sign(rollAngle);
}
else if (Math.Abs(rollAngle) > 0.01)
{
targetRoll = 0.05f * Math.Sign(rollAngle);
}
else if (Math.Abs(rollAngle) > 0.001)
{
targetRoll = 0.035f * Math.Sign(rollAngle);
}
else
{
targetRoll = 0;
}
// Echo("targetRoll=" + targetRoll);
// rollLevel = (int)(targetRoll * 1000);
for (int i = 0; i < gyros.Count; i++)
{
gyro = gyros[i] as IMyGyro;
gyro.SetValueFloat(sPlane, targetRoll);
gyro.SetValueBool("Override", true);
}
return(true);
}
public void UpdateBlocks(List <IMyTerminalBlock> list)
{
gyros = new List <IMyGyro>();
thrusters = new Dictionary <VRageMath.Vector3, List <IMyThrust> >();
VRageMath.Matrix tmp = new VRageMath.Matrix();
for (int i = 0; i < list.Count; ++i)
{
var block = list[i];
if (block is IMyGyro)
{
IMyGyro gyro = (IMyGyro)block;
if (gyros.Count == 0)
{
gyroRotateMin = gyro.GetMininum <float>(GyroAction.Pitch.GetName());
gyroRotateMax = gyro.GetMaximum <float>(GyroAction.Pitch.GetName());
}
gyros.Add(gyro);
}
else if (block is IMyThrust)
{
IMyThrust thruster = block as IMyThrust;
if (thrusters.Count == 0)
{
thrusterOverrideMin = thruster.GetMininum <float>("Override");
thrusterOverrideMax = thruster.GetMaximum <float>("Override");
}
block.Orientation.GetMatrix(out tmp);
// The exhaust is directed to the Forward vector of the thruster, so it accelerates to Backward.
VRageMath.Vector3 dir = tmp.Backward;
if (!thrusters.ContainsKey(dir))
{
thrusters[dir] = new List <IMyThrust>();
}
thrusters[dir].Add(thruster);
}
}
}
