Master thesis

class Program
{
	private Servo _servo1 = new Servo();

	static void Main(string[] args)
	{
		new Program();
	}

	public Program()
	{
		_servo1.open();
		while (true)
		{
			for (double angle = 0; angle < Math.PI * 2; angle+= Math.PI / 400)
			{
				PanTiltToServo(angle, Math.PI / 20);
				System.Threading.Thread.Sleep(10);
			}
		}
	}

	private void PanTiltToServo(double pan, double tilt)
	{
		int mirror_radius = 30; // cm
		SetServoPositionFromRopeLength(0, CalculateRopeLength(pan, tilt, mirror_radius, 0));
		SetServoPositionFromRopeLength(1, CalculateRopeLength(pan, tilt, mirror_radius, Math.PI * 2 / 3));
		SetServoPositionFromRopeLength(2, CalculateRopeLength(pan, tilt, mirror_radius, Math.PI * 4 / 3));
	}

	private double CalculateRopeLength(double pan, double tilt, double mirrorRadius, double ropeLocation)
	{
		return
			-(mirrorRadius * Math.Cos(pan) * Math.Sin(tilt) * Math.Cos(ropeLocation) +
			mirrorRadius * Math.Sin(pan) * Math.Sin(tilt) * Math.Sin(ropeLocation)) /
			Math.Cos(tilt);
	}

	private void SetServoPositionFromRopeLength(int servo, double rope_length)
	{
		if (_servo1.servos.Count > servo)
		{
			Console.WriteLine("Serve }} rope length: }}, servo, rope_length);
			int max_rope_length = 30; // -10 .. +10
			ServoServo s = _servo1.servos[servo];

			double proportion = (rope_length + max_rope_length) / (2 * max_rope_length);
			s.Position = s.PositionMin + (s.PositionMax - s.PositionMin) * proportion;
		}
	}}