public static void GreenLine()
{
//CreateTargetedTrajectory(Starship.starship, Starship.InitPos);
Vector2 Ligne = new Vector2(1.0f, 0.0f);
/*Console.WriteLine("Angle 1 : " + Calculs.VectorAngleWithNegative(new Vector2(1.0f, 0.0f)));
* Console.WriteLine("Angle 2 : " + Calculs.VectorAngleWithNegative(new Vector2(1.0f, 0.0f)));*/
Vector2 DirGreenVector = Calculs.Rotate(Ligne, (float)(Math.PI / 2f));
Console.WriteLine("Dir Green Vector : " + DirGreenVector.ToString());
}
private void Tracking()
{
if (Cam != null)
{
double Head = 0;
double Pitch = 0;
double Altitude = 0;
Tuple <double, double> Position = new Tuple <double, double>(0, 0);
double TargetAltitude = 0;
Tuple <double, double> TargetPosition = new Tuple <double, double>(0, 0);
Part PitchHinge = Cam.Parts.WithTag("PitchServo")[0];
/*foreach (var item in PitchHinge.Modules)
* {
* Console.WriteLine(item.Name);
* }*/
Module PitchServo = PitchHinge.Modules.First(m => m.Name == "ModuleRoboticServoHinge");
PitchServo.SetFieldFloat("Target Angle", (float)-Pitch);
Module HeadServo = Cam.Parts.WithTag("HeadServo")[0].Modules.First(m => m.Name == "ModuleRoboticRotationServo");
HeadServo.SetFieldFloat("Target Angle", (float)-Head);
Console.WriteLine("Tracking Started");
while (Track == true)
{
Head = Cam.Flight(Cam.SurfaceReferenceFrame).Heading;
Pitch = Cam.Flight(Cam.SurfaceReferenceFrame).Pitch;
Head = Calculs.Mod((Head + 180), 360) - 180;
Altitude = Cam.Flight(Cam.SurfaceReferenceFrame).MeanAltitude;
Position = new Tuple <double, double>(Cam.Flight(Target.SurfaceReferenceFrame).Latitude, Cam.Flight(Target.SurfaceReferenceFrame).Longitude);
TargetAltitude = Target.Flight(Target.SurfaceReferenceFrame).MeanAltitude;
TargetPosition = new Tuple <double, double>(Target.Flight(Target.SurfaceReferenceFrame).Latitude, Target.Flight(Target.SurfaceReferenceFrame).Longitude);
double dAlt = (TargetAltitude - 30) - Altitude;
double Distance = Starship.Distance(Position.Item1, TargetPosition.Item1, Position.Item2, TargetPosition.Item2);
double PitchAngle = Starship.ToDegree(Math.Atan(dAlt / Distance)) - Pitch;
double HeadAngle = Starship.ToDegree(Calculs.VectorAngleWithNegative(new System.Numerics.Vector2((float)(TargetPosition.Item1 - Position.Item1), (float)(TargetPosition.Item2 - Position.Item2)))) - Head;
HeadAngle = Calculs.Mod((HeadAngle + 180), 360) - 180;
/*Console.WriteLine("Pitch Servo" + PitchAngle);
* Console.WriteLine("Head Servo" + HeadAngle);*/
PitchServo.SetFieldFloat("Target Angle", (float)PitchAngle);
HeadServo.SetFieldFloat("Target Angle", (float)HeadAngle);
}
}
}
public static void FoundIntersection(Vessel vessel, Tuple <double, double> LZ)
{
Vector2 ls = new Vector2((float)(vessel.Flight(vessel.SurfaceReferenceFrame).Latitude - LZ.Item1),
(float)(vessel.Flight(vessel.SurfaceReferenceFrame).Longitude - LZ.Item2));
double lp = Calculs.VectorScal(ls, targetedTrajectory) / targetedTrajectory.Length();
float alpha = (float)Calculs.VectorAngleWithNegative(targetedTrajectory);
Vector2 LP = new Vector2((float)(lp * Math.Cos(alpha)), (float)(lp * Math.Sin(alpha)));
intersection = new Vector2((float)LZ.Item1, (float)LZ.Item2) + LP;
float beta = (float)Calculs.VectorAngleWithNegative(ls);
teta = (float)((beta - alpha) % Math.PI);
}
public static void EngineGuidanceKOS(Vessel vessel, Tuple <double, double> LZ)
{
try
{
Console.WriteLine("Start of Engine Guidance");
//Console.WriteLine("Pitch PID Gains : " + vessel.AutoPilot.PitchPIDGains);
//Console.WriteLine("Yaw PID Gains : " + vessel.AutoPilot.YawPIDGains);
}
catch (Exception e)
{
Console.WriteLine(e);
}
Console.WriteLine("Start of Engine Guidance 2");
//vessel.AutoPilot.Engage();
//vessel.AutoPilot.TargetRoll = 180;
Console.WriteLine("Start of Engine Guidance 3");
//vessel.AutoPilot.AutoTune = false;
vessel.AutoPilot.AttenuationAngle = new Tuple <double, double, double>(0.01, 360, 45);
vessel.AutoPilot.StoppingTime = new Tuple <double, double, double>(3, 3, 6);
vessel.AutoPilot.DecelerationTime = new Tuple <double, double, double>(10, 10, 30);
vessel.AutoPilot.RollThreshold = 0.1;
vessel.AutoPilot.PitchPIDGains = new Tuple <double, double, double>(50, 50, 0);
vessel.AutoPilot.YawPIDGains = new Tuple <double, double, double>(50, 50, 0);
vessel.AutoPilot.RollPIDGains = new Tuple <double, double, double>(50, 50, 0);
Console.WriteLine("Start of Engine Guidance 4");
double ImpactLat = vessel.connection.Trajectories().ImpactPos().Item1;
double ImpactLon = vessel.connection.Trajectories().ImpactPos().Item2;
float Pitch = 90;
float Head = 0;
double prevDistance = Starship.Distance(LZ.Item1, ImpactLat, LZ.Item2, ImpactLon);
try
{
while (vessel.Flight(vessel.SurfaceReferenceFrame).SurfaceAltitude > 45)
{
while (vessel.Flight(vessel.SurfaceReferenceFrame).SurfaceAltitude > 80)
{
ImpactLat = vessel.connection.Trajectories().ImpactPos().Item1;
ImpactLon = vessel.connection.Trajectories().ImpactPos().Item2;
if (/*prevDistance + 2 > Starship.Distance(LZ.Item1, ImpactLat, LZ.Item2, ImpactLon) && */ vessel.Flight(vessel.SurfaceReferenceFrame).Speed < 40 && vessel.Flight(vessel.SurfaceReferenceFrame).HorizontalSpeed < 5)
{
/*if (Starship.Distance(LZ.Item1, vessel.Flight(vessel.SurfaceReferenceFrame).Latitude, LZ.Item2, vessel.Flight(vessel.SurfaceReferenceFrame).Latitude) - 10 > Starship.Distance(ImpactLat, vessel.Flight(vessel.SurfaceReferenceFrame).Latitude, ImpactLon, vessel.Flight(vessel.SurfaceReferenceFrame).Latitude))
* {*/
vessel.AutoPilot.AttenuationAngle = new Tuple <double, double, double>(0.01, 45, 15);
Console.WriteLine("1");
vessel.AutoPilot.TargetRoll = 180;
Pitch = Calculs.Clamp <float>(90 - ((float)Starship.Distance(ImpactLat, LZ.Item1, ImpactLon, LZ.Item2) / 10f), 70, 90); // / 15
Head = (float)ZoneHeading(vessel);
/*}
* else
* {
* Console.WriteLine("2");
* /*vessel.AutoPilot.TargetRoll = 0;
* Pitch = Calculs.Clamp<float>(90 + ((float)Starship.Distance(ImpactLat, LZ.Item1, ImpactLon, LZ.Item2) / 15), 90, 95);
* Head = (float)ZoneHeading(vessel) + 180;*/
/*vessel.AutoPilot.TargetRoll = 0;
* Pitch = 90 - (90 - (float)RetrogradePitch(vessel)) / 2;
* Head = (float)RetrogradeHeading(vessel);
* vessel.AutoPilot.TargetPitchAndHeading(Pitch, Head);
* }*/
}
else
{
vessel.AutoPilot.AttenuationAngle = new Tuple <double, double, double>(0.01, 180, 25);
vessel.AutoPilot.TargetRoll = 180;
Pitch = 90 - (Math.Abs(90 - (float)RetrogradePitch(vessel))) / 0.5f;
Head = (float)RetrogradeHeading(vessel);
vessel.AutoPilot.TargetPitchAndHeading(Calculs.Clamp(Pitch, 80, 110), Head);
while ((vessel.AutoPilot.PitchError > 10 && vessel.AutoPilot.HeadingError > 25) || (vessel.AutoPilot.PitchError < -10 && vessel.AutoPilot.HeadingError < -25))
{
Console.WriteLine("Retro");
vessel.AutoPilot.TargetRoll = 180;
Pitch = 90 - (Math.Abs(90 - (float)RetrogradePitch(vessel))) / 0.5f;
Head = (float)RetrogradeHeading(vessel);
vessel.AutoPilot.TargetPitchAndHeading(Calculs.Clamp(Pitch - 18, 75, 115), Head);
Console.WriteLine("Error : " + vessel.AutoPilot.Error);
Console.WriteLine("Pitch Target = " + vessel.AutoPilot.TargetPitch);
Console.WriteLine("Pitch Head = " + vessel.AutoPilot.TargetHeading);
Console.WriteLine("Pitch Roll = " + vessel.AutoPilot.TargetRoll);
vessel.Control.Pitch += 0.5f;
}
Console.WriteLine("Retro End");
}
vessel.AutoPilot.TargetPitchAndHeading(Calculs.Clamp(Pitch - 18, 80, 110), Head);
Console.WriteLine("Error : " + vessel.AutoPilot.Error);
Console.WriteLine("Pitch Target = " + vessel.AutoPilot.TargetPitch);
Console.WriteLine("Pitch Head = " + vessel.AutoPilot.TargetHeading);
Console.WriteLine("Pitch Roll = " + vessel.AutoPilot.TargetRoll);
prevDistance = Starship.Distance(LZ.Item1, ImpactLat, LZ.Item2, ImpactLon);
if (vessel.Flight(vessel.SurfaceReferenceFrame).TrueAirSpeed > 25)
{
Console.WriteLine("Wings full opened");
Wings.WingUpR[0].Orientation(0); //90
Wings.WingUpL[0].Orientation(0); //90
}
else
{
Console.WriteLine("Wings full closed");
Wings.WingUpR[0].Orientation(80); //90
Wings.WingUpL[0].Orientation(80); //90
}
}
Console.WriteLine("Retro 2");
vessel.AutoPilot.TargetRoll = 180;
Pitch = 90 - (Math.Abs(90 - (float)RetrogradePitch(vessel))) / 0.5f;
Head = (float)RetrogradeHeading(vessel);
vessel.AutoPilot.TargetPitchAndHeading(Calculs.Clamp(Pitch - 18, 65, 115), Head);
Console.WriteLine("Error : " + vessel.AutoPilot.Error);
Console.WriteLine("Pitch Target = " + vessel.AutoPilot.TargetPitch);
Console.WriteLine("Pitch Head = " + vessel.AutoPilot.TargetHeading);
Console.WriteLine("Pitch Roll = " + vessel.AutoPilot.TargetRoll);
}
}
catch { }
try
{
while (vessel.Flight(vessel.Orbit.Body.ReferenceFrame).VerticalSpeed < -0.1)
{
vessel.AutoPilot.AttenuationAngle = new Tuple <double, double, double>(0.01, 360, 25);
Console.WriteLine("Decel for TD");
vessel.AutoPilot.TargetRoll = 180;
Pitch = 90 - (90 - (float)RetrogradePitch(vessel)) / 2; //4
Head = (float)RetrogradeHeading(vessel); //90
vessel.AutoPilot.TargetPitchAndHeading(Calculs.Clamp(Pitch + 0, 85, 95), Head);
Console.WriteLine("Error : " + vessel.AutoPilot.Error);
Console.WriteLine("Pitch Target = " + vessel.AutoPilot.TargetPitch);
Console.WriteLine("Pitch Head = " + vessel.AutoPilot.TargetHeading);
Console.WriteLine("Pitch Roll = " + vessel.AutoPilot.TargetRoll);
}
}
catch
{
}
Console.WriteLine("Good Place to Vertical");
vessel.AutoPilot.TargetPitch = 90;
}
public void GimbalLimit(float value)
{
raptor.GimbalLimit = Calculs.Clamp(value, 0, 1);
Console.WriteLine("Set Raptor gimbal limit to " + raptor.GimbalLimit);
}