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);
}