private bool fastMove(motorBase motor)
{
TimeSpan ts = befTime - DateTime.UtcNow;
double diffAngle = MathHelperD.ToDegrees(motor.targetRadians) - MathHelperD.ToDegrees(motor.motorRadian);
double rad = MathHelperD.ToRadians(diffAngle) / (-0.03);
if (targetRPM < Math.Abs(rad * 9.549))
{
MyMotor.TargetVelocityRPM = MathHelperD.ToDegrees(rad) < 0 ? targetRPM : -targetRPM;
}
else
{
motor.MyMotor.TargetVelocityRad = -(float)(rad / 2);
}
befTime = DateTime.UtcNow;
motor.befRadian = motor.motorRadian;
if (Math.Abs(MathHelperD.ToDegrees(motor.motorRadian) - MathHelperD.ToDegrees(motor.targetRadians)) < 0.5)
{
motor.MyMotor.TargetVelocityRad = 0;
return(true);
}
else
{
return(false);
}
}
private bool fastMove(motorBase motor)
{
TimeSpan ts = befTime - DateTime.UtcNow;
//double ang = (motor.Angle - motor.BefAngle);
double diffAngle = motor.TargetAngle - motor.Angle;
double rad = MathHelperD.ToRadians(diffAngle) / (-0.03);
// motor.MyMotor.TargetVelocityRad = -(float)(rad / 5);
if (TargetRPM < Math.Abs(rad * 9.549))
{
MyMotor.TargetVelocityRPM = MathHelperD.ToDegrees(rad) < 0 ? TargetRPM : -TargetRPM;
}
else
{
motor.MyMotor.TargetVelocityRad = -(float)(rad / 2);
}
befTime = DateTime.UtcNow;
motor.BefAngle = motor.Angle;
if (Math.Abs(motor.Angle - motor.TargetAngle) < 0.5)
{
motor.MyMotor.TargetVelocityRad = 0;
return(true);
}
else
{
return(false);
}
}
private bool justMove(motorBase motor)
{
TimeSpan ts = befTime - nowTime;
double ang = (MathHelperD.ToDegrees(motor.motorRadian) - MathHelperD.ToDegrees(motor.befRadian));
double diffAngle = MathHelperD.ToDegrees(motor.targetRadians) - MathHelperD.ToDegrees(motor.motorRadian);
double rad = MathHelperD.ToRadians(diffAngle) / ts.TotalSeconds;
if (diffAngle > rad)
{
motor.MyMotor.TargetVelocityRPM = motor.targetRPM;
}
else
{
motor.MyMotor.TargetVelocityRPM = (float)(diffAngle / (Math.PI * 2));
}
motor.befRadian = motor.motorRadian;
if ((MathHelperD.ToDegrees(motor.motorRadian) - MathHelperD.ToDegrees(motor.targetRadians)) < 0.5)
{
return(true);
}
else
{
return(false);
}
}
private bool fastMove()
{
TimeSpan ts = befTime - DateTime.UtcNow;
//double ang = (motor.Angle - motor.BefAngle);
double diffAngle = DataEntity.GetTargetAngle() - MathHelperD.ToDegrees(MyMotor.Angle);
double rad = MathHelperD.ToRadians(diffAngle) / (-0.03);
if (DataEntity.GetVelocity() < Math.Abs(rad * 9.549))
{
MyMotor.TargetVelocityRPM = MathHelperD.ToDegrees(rad) < 0 ? DataEntity.GetVelocity() : -DataEntity.GetVelocity();
}
else
{
MyMotor.TargetVelocityRad = -(float)(rad / 2);
}
befTime = DateTime.UtcNow;
BefAngleDegree = MathHelperD.ToDegrees(MyMotor.Angle);
if (Math.Abs(MathHelperD.ToDegrees(MyMotor.Angle) - DataEntity.GetTargetAngle()) < 1)
{
MyMotor.TargetVelocityRad = 0;
return(true);
}
else
{
return(false);
}
}
public void HingeKnee()
{
float velocity = 2f;
if (R1Flg == 0)
{
if (MathHelperD.ToDegrees(myMotorR1.Angle) > -20)
{
MorterMoveToAngle(myMotorR2, 80, velocity * 2);
}
else
{
MorterMoveToAngle(myMotorR2, 10, velocity * 2);
}
}
else
{
}
if (L1Flg == 0)
{
if (MathHelperD.ToDegrees(myMotorL1.Angle) < 20)
{
MorterMoveToAngle(myMotorL2, 80, velocity * 2);
}
else
{
MorterMoveToAngle(myMotorL2, 10, velocity * 2);
}
}
else
{
}
}
private bool justMove()
{
TimeSpan ts = befTime - nowTime;
double ang = (MathHelperD.ToDegrees(MyMotor.Angle) - BefAngleDegree);
double diffAngle = DataEntity.GetTargetAngle() - (MathHelperD.ToDegrees(MyMotor.Angle));
double rad = MathHelperD.ToRadians(diffAngle) / ts.TotalSeconds;
if (diffAngle > rad)
{
MyMotor.TargetVelocityRPM = DataEntity.GetVelocity();
}
else
{
MyMotor.TargetVelocityRPM = (float)(diffAngle / (Math.PI * 2));
}
BefAngleDegree = MathHelperD.ToDegrees(MyMotor.Angle);
if ((MathHelperD.ToDegrees(MyMotor.Angle) - DataEntity.GetTargetAngle()) < 0.5)
{
MyMotor.TargetVelocityRad = 0;
return(true);
}
else
{
return(false);
}
}
public void HingeControl()
{
str += "myMotorR1:" + MathHelperD.ToDegrees(myMotorR1.Angle).ToString() + " \r\n";
str += "myMotorR2:" + MathHelperD.ToDegrees(myMotorR2.Angle).ToString() + " \r\n";
str += "myMotorR3:" + MathHelperD.ToDegrees(myMotorR3.Angle).ToString() + " \r\n";
str += "myMotorL1:" + MathHelperD.ToDegrees(myMotorL1.Angle).ToString() + " \r\n";
str += "myMotorL2:" + MathHelperD.ToDegrees(myMotorL2.Angle).ToString() + " \r\n";
str += "myMotorL3:" + MathHelperD.ToDegrees(myMotorL3.Angle).ToString() + " \r\n";
}
public void Running()
{
float velocity = 2f;
if (R1Flg == 0)
{
MorterMoveToAngle(myMotorR1, -80, velocity);
}
else if (R1Flg == 1)
{
MorterMoveToAngle(myMotorR1, 20, velocity);
}
if (L1Flg == 0)
{
MorterMoveToAngle(myMotorL1, 80, velocity);
}
else if (L1Flg == 1)
{
MorterMoveToAngle(myMotorL1, -20, velocity);
}
if (R1Flg == 0)
{
if (MathHelperD.ToDegrees(myMotorR1.Angle) > -20)
{
MorterMoveToAngle(myMotorR2, 80, velocity * 2);
}
else
{
MorterMoveToAngle(myMotorR2, 10, velocity * 2);
}
}
if (L1Flg == 0)
{
if (MathHelperD.ToDegrees(myMotorL1.Angle) < 20)
{
MorterMoveToAngle(myMotorL2, 80, velocity * 2);
}
else
{
MorterMoveToAngle(myMotorL2, 10, velocity * 2);
}
}
}
public void AimTarget_sensor(IMySensorBlock sensor)
{
string[] vs = RArm.myMotor1.MyMotor.CustomData.Split(',');
var str = sensor.CustomData.Split(',');
var vector = new Vector3D(double.Parse(str[0]), double.Parse(str[1]), double.Parse(str[2]));
Vector3D worldDirection = vector - sensor.GetPosition();
Vector3D direction = Vector3D.TransformNormal(worldDirection, MatrixD.Transpose(sensor.WorldMatrix));
vs[1] = MathHelperD.ToDegrees(-Math.Atan2(direction.Z, direction.Y)).ToString();
RArm.myMotor1.MyMotor.CustomData = vs[0] + "," + vs[1] + ',' + vs[2];
vs = RArm.myMotor2.MyMotor.CustomData.Split(',');
vs[1] = "0"; //MathHelperD.ToDegrees(Math.Atan2(direction.X, direction.Y)).ToString();
RArm.myMotor2.MyMotor.CustomData = vs[0] + "," + vs[1] + ',' + vs[2];
}
public void MorterMoveToAngle(IMyMotorStator motorStator, double TargetAngle, float velocity)
{
var angle = MathHelperD.ToDegrees(motorStator.Angle) - TargetAngle;
if (Math.Abs(angle) < velocity)
{
motorStator.TargetVelocityRad = 0;
}
else if (MathHelperD.ToDegrees(motorStator.Angle) > TargetAngle)
{
motorStator.TargetVelocityRad = -velocity;
}
else if (MathHelperD.ToDegrees(motorStator.Angle) < TargetAngle)
{
motorStator.TargetVelocityRad = velocity;
}
}
private void Tick()
{
var status = new MissileStatus()
{
Position = this.Position,
State = state,
DistToTarget = (this.Target - this.Position).Length(),
};
status.ExtraData.AppendLine($"DeltaTime: {(DateTime.Now - launchTime).TotalSeconds:F2} seconds");
status.ExtraData.AppendLine($"Distance from launch position: {(this.Position - this.launchPos).Length():F2}");
if (state == LaunchState.Separation)
{
if ((this.Position - this.launchPos).LengthSquared() >= downSeparationRange * downSeparationRange)
{
LogLine($"Separation phase finished, transitioning to boost phase");
foreach (var thrust in downThrusters)
{
thrust.ThrustOverridePercentage = 0;
}
foreach (var thrust in fwdThrusters)
{
thrust.Enabled = true;
thrust.ThrustOverridePercentage = 1;
}
this.state = LaunchState.Boost;
}
}
else if (state == LaunchState.Boost)
{
if ((this.Position - this.launchPos).LengthSquared() >= boostManeuverRange * boostManeuverRange)
{
LogLine($"Boost phase finished, transitioning to parabolic flight mode");
this.state = LaunchState.Flight;
foreach (var gyro in gyros)
{
gyro.Enabled = true;
}
this.enginesActive = false;
Vector3D planet = Vector3D.Zero;
if (!remote.TryGetPlanetPosition(out planet))
{
LogLine("Couldn't find planet!");
}
LogLine($"Generating parabolic trajectory");
this.trajectory = GenerateTrajectory(this.Position, this.finalTarget.Coords, 10, planet);
this.trajectoryStage = 0;
this.state = LaunchState.Flight;
// temporarily stop forward thrust in order to allow missile to align to target
foreach (var thruster in fwdThrusters)
{
thruster.ThrustOverride = 0;
}
}
}
else if (state == LaunchState.Flight)
{
double pitch, yaw, roll;
var desiredFwdVec = Vector3D.Normalize(Target - Position);
MathStuff.GetRotationAngles(desiredFwdVec, Vector3D.Zero, remote.WorldMatrix, out pitch, out yaw, out roll);
status.DistToTarget = (Target - Position).Length();
status.Pitch = pitch;
status.Yaw = yaw;
status.Roll = roll;
if (!this.enginesActive && (Math.Abs(pitch) < MIN_ANGLE_FOR_ENGINE_RESTART ||
Math.Abs(yaw) < MIN_ANGLE_FOR_ENGINE_RESTART || Math.Abs(roll) < MIN_ANGLE_FOR_ENGINE_RESTART))
{
foreach (var thrust in fwdThrusters)
{
thrust.ThrustOverridePercentage = 1;
}
this.enginesActive = true;
}
var pitchCorrection = pitchPID.GetCorrection(pitch, PID_TIME_DELTA);
var yawCorrection = yawPID.GetCorrection(yaw, PID_TIME_DELTA);
var rollCorrection = rollPID.GetCorrection(roll, PID_TIME_DELTA);
status.ExtraData.Append($"PID:\nPitchCorrection: {MathHelperD.ToDegrees(pitchCorrection):F2}\n" +
$"YawCorrection: {MathHelperD.ToDegrees(yawCorrection):F2}\n" +
$"RollCorrection: {MathHelperD.ToDegrees(rollCorrection):F2}\n");
if (applyGyro)
{
MathStuff.ApplyGyroOverride(pitchCorrection, yawCorrection, rollCorrection, this.gyros, this.remote.WorldMatrix);
}
if ((this.Position - Target).LengthSquared() <= SWITCH_WAY_POINT_DISTANCE * SWITCH_WAY_POINT_DISTANCE)
{
this.trajectoryStage++;
}
if (this.trajectoryStage >= this.trajectory.Count ||
(this.Position - finalTarget.Coords).LengthSquared() <= this.armDistance * this.armDistance)
{
ArmMissile();
this.trajectoryStage++;
this.state = LaunchState.Terminal;
}
}
else if (state == LaunchState.Terminal)
{
if ((this.Position - finalTarget.Coords).LengthSquared() <= blowDistance * blowDistance)
{
LogLine("Reached detonation threshold. Detonating and disabling script.");
LogStatus("Detonated.");
Detonate();
}
}
status.State = this.state;
LogStatus(status.ToString());
}
public void GyroControl()
{
var vector = cockpit.GetNaturalGravity();
//var cockpitmatrix = Matrix.CreateFromDir(cockpit.WorldMatrix.Down);
var cockpitmatrix = MatrixD.CreateWorld(cockpit.GetPosition(), cockpit.WorldMatrix.Forward, cockpit.WorldMatrix.Down);
//cockpitmatrix.Forward = cockpitmatrix.Down;
Vector3D direction = Vector3D.TransformNormal(vector, Matrix.Transpose(cockpitmatrix));
var diffangle = MathHelperD.ToDegrees(Math.Atan2(direction.Z, direction.Y));
if (diffangle < Math.Abs(oldAngle - diffangle))
{
}
float pitch = 0f;
WriteText("Gyro diffangle: " + diffangle.ToString() + " -\n");
WriteText("Gyro pitch1: " + ((pitch1)).ToString() + " -\n");
WriteText("Gyro pitch60: " + ((pitch60)).ToString() + " -\n");
WriteText("Gyro pitch: " + ((float)(pitch1 * diffangle)).ToString() + " -\n");
if (diffangle > 5)
{
if (diffangle > 20)
{
pitch = (float)(0.8 * Math.PI);
}
else
{
pitch = (float)(pitch1 * 10);
}
foreach (var item in gyros)
{
var gyro = item as IMyGyro;
gyro.CustomData = gyro.Pitch.ToString() + " ::::" + ((float)2 * Math.PI).ToString() + " cheack " + gyro.Pitch.Equals((float)(2 * Math.PI));
if (!gyro.Pitch.Equals(pitch))
{
gyro.Pitch = pitch;
}
}
}
else if (diffangle < -5)
{
if (diffangle < -20)
{
pitch = (float)(-0.8 * Math.PI);
}
else
{
pitch = (float)(pitch1 * -10);
}
foreach (var item in gyros)
{
var gyro = item as IMyGyro;
if (!gyro.Pitch.Equals(pitch))
{
gyro.Pitch = pitch;
}
}
}
else
{
foreach (var item in gyros)
{
var gyro = item as IMyGyro;
gyro.Pitch = (float)(pitch1 * diffangle);
}
}
oldAngle = diffangle;
//diffangle = MathHelperD.ToDegrees(Math.Atan2(direction.X, direction.Y));
//float roll = 0f;
//if (diffangle > 5)
//{
// if (diffangle > 5)
// {
// roll = (float)(1 * Math.PI);
// }
// foreach (var item in gyrorolls)
// {
// var gyro = item as IMyGyro;
// gyro.CustomData = gyro.Roll.ToString() + " ::::" + ((float)2 * Math.PI).ToString() + " cheack " + gyro.Roll.Equals((float)(2 * Math.PI));
// if (!gyro.Roll.Equals(roll))
// {
// gyro.Roll = roll;
// }
// }
//}
//else if (diffangle < -5)
//{
// if (diffangle < -5)
// {
// roll = (float)(-1 * Math.PI);
// }
// foreach (var item in gyrorolls)
// {
// var gyro = item as IMyGyro;
// if (!gyro.Roll.Equals(roll))
// {
// gyro.Roll = roll;
// }
// }
//}
//else
//{
// foreach (var item in gyrorolls)
// {
// var gyro = item as IMyGyro;
// if (!gyro.Roll.Equals(0))
// {
// gyro.Roll = 0;
// }
// }
//}
}
public double GetTargetAngle()
{
return(MathHelperD.ToDegrees(targetAngleRadian));
}